<commit_before>/*
 *  Copyright (c) 2016, The OpenThread Authors.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions are met:
 *  1. Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *  2. Redistributions in binary form must reproduce the above copyright
 *     notice, this list of conditions and the following disclaimer in the
 *     documentation and/or other materials provided with the distribution.
 *  3. Neither the name of the copyright holder nor the
 *     names of its contributors may be used to endorse or promote products
 *     derived from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 *  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 *  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 *  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 *  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 *  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 *  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 *  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 *  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 *  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 */

/**
 * @file
 *   This file implements IPv6 addresses.
 */

#include "ip6_address.hpp"

#include <stdio.h>

#include "common/code_utils.hpp"
#include "common/encoding.hpp"
#include "common/instance.hpp"

using ot::Encoding::BigEndian::HostSwap16;
using ot::Encoding::BigEndian::HostSwap32;

namespace ot {
namespace Ip6 {

void Address::Clear(void)
{
    memset(mFields.m8, 0, sizeof(mFields));
}

bool Address::IsUnspecified(void) const
{
    return (mFields.m32[0] == 0 && mFields.m32[1] == 0 && mFields.m32[2] == 0 && mFields.m32[3] == 0);
}

bool Address::IsLoopback(void) const
{
    return (mFields.m32[0] == 0 && mFields.m32[1] == 0 && mFields.m32[2] == 0 && mFields.m32[3] == HostSwap32(1));
}

bool Address::IsLinkLocal(void) const
{
    return (mFields.m16[0] & HostSwap16(0xffc0)) == HostSwap16(0xfe80);
}

bool Address::IsMulticast(void) const
{
    return mFields.m8[0] == 0xff;
}

bool Address::IsLinkLocalMulticast(void) const
{
    return IsMulticast() && (GetScope() == kLinkLocalScope);
}

bool Address::IsLinkLocalAllNodesMulticast(void) const
{
    return (mFields.m32[0] == HostSwap32(0xff020000) && mFields.m32[1] == 0 && mFields.m32[2] == 0 &&
            mFields.m32[3] == HostSwap32(0x01));
}

bool Address::IsLinkLocalAllRoutersMulticast(void) const
{
    return (mFields.m32[0] == HostSwap32(0xff020000) && mFields.m32[1] == 0 && mFields.m32[2] == 0 &&
            mFields.m32[3] == HostSwap32(0x02));
}

bool Address::IsRealmLocalMulticast(void) const
{
    return IsMulticast() && (GetScope() == kRealmLocalScope);
}

bool Address::IsMulticastLargerThanRealmLocal(void) const
{
    return IsMulticast() && (GetScope() > kRealmLocalScope);
}

bool Address::IsRealmLocalAllNodesMulticast(void) const
{
    return (mFields.m32[0] == HostSwap32(0xff030000) && mFields.m32[1] == 0 && mFields.m32[2] == 0 &&
            mFields.m32[3] == HostSwap32(0x01));
}

bool Address::IsRealmLocalAllRoutersMulticast(void) const
{
    return (mFields.m32[0] == HostSwap32(0xff030000) && mFields.m32[1] == 0 && mFields.m32[2] == 0 &&
            mFields.m32[3] == HostSwap32(0x02));
}

bool Address::IsRealmLocalAllMplForwarders(void) const
{
    return (mFields.m32[0] == HostSwap32(0xff030000) && mFields.m32[1] == 0 && mFields.m32[2] == 0 &&
            mFields.m32[3] == HostSwap32(0xfc));
}

bool Address::IsRoutingLocator(void) const
{
    return (mFields.m32[2] == HostSwap32(0x000000ff) && mFields.m16[6] == HostSwap16(0xfe00) &&
            mFields.m8[14] < kAloc16Mask && (mFields.m8[14] & kRloc16ReservedBitMask) == 0);
}

bool Address::IsAnycastRoutingLocator(void) const
{
    return (mFields.m32[2] == HostSwap32(0x000000ff) && mFields.m16[6] == HostSwap16(0xfe00) &&
            mFields.m8[14] == kAloc16Mask);
}

bool Address::IsAnycastServiceLocator(void) const
{
    return IsAnycastRoutingLocator() && (mFields.m16[7] >= HostSwap16(Mle::kAloc16ServiceStart)) &&
           (mFields.m16[7] <= HostSwap16(Mle::kAloc16ServiceEnd));
}

bool Address::IsSubnetRouterAnycast(void) const
{
    return (mFields.m32[2] == 0 && mFields.m32[3] == 0);
}

bool Address::IsReservedSubnetAnycast(void) const
{
    return (mFields.m32[2] == HostSwap32(0xfdffffff) && mFields.m16[6] == 0xffff && mFields.m8[14] == 0xff &&
            mFields.m8[15] >= 0x80);
}

bool Address::IsIidReserved(void) const
{
    return IsSubnetRouterAnycast() || IsReservedSubnetAnycast() || IsAnycastRoutingLocator();
}

const uint8_t *Address::GetIid(void) const
{
    return mFields.m8 + kInterfaceIdentifierOffset;
}

uint8_t *Address::GetIid(void)
{
    return mFields.m8 + kInterfaceIdentifierOffset;
}

void Address::SetIid(const uint8_t *aIid)
{
    memcpy(mFields.m8 + kInterfaceIdentifierOffset, aIid, kInterfaceIdentifierSize);
}

void Address::SetIid(const Mac::ExtAddress &aExtAddress)
{
    Mac::ExtAddress addr;

    addr = aExtAddress;
    addr.ToggleLocal();
    addr.CopyTo(mFields.m8 + kInterfaceIdentifierOffset);
}

void Address::ToExtAddress(Mac::ExtAddress &aExtAddress) const
{
    aExtAddress.Set(mFields.m8 + kInterfaceIdentifierOffset);
    aExtAddress.ToggleLocal();
}

void Address::ToExtAddress(Mac::Address &aMacAddress) const
{
    aMacAddress.SetExtended(mFields.m8 + kInterfaceIdentifierOffset);
    aMacAddress.GetExtended().ToggleLocal();
}

uint8_t Address::GetScope(void) const
{
    uint8_t rval;

    if (IsMulticast())
    {
        rval = mFields.m8[1] & 0xf;
    }
    else if (IsLinkLocal())
    {
        rval = kLinkLocalScope;
    }
    else if (IsLoopback())
    {
        rval = kNodeLocalScope;
    }
    else
    {
        rval = kGlobalScope;
    }

    return rval;
}

uint8_t Address::PrefixMatch(const uint8_t *aPrefixA, const uint8_t *aPrefixB, uint8_t aMaxLength)
{
    uint8_t rval = 0;
    uint8_t diff;

    if (aMaxLength > sizeof(Address))
    {
        aMaxLength = sizeof(Address);
    }

    for (uint8_t i = 0; i < aMaxLength; i++)
    {
        diff = aPrefixA[i] ^ aPrefixB[i];

        if (diff == 0)
        {
            rval += 8;
        }
        else
        {
            while ((diff & 0x80) == 0)
            {
                rval++;
                diff <<= 1;
            }

            break;
        }
    }

    return rval;
}

uint8_t Address::PrefixMatch(const otIp6Address &aOther) const
{
    return PrefixMatch(mFields.m8, aOther.mFields.m8, sizeof(Address));
}

bool Address::operator==(const Address &aOther) const
{
    return memcmp(mFields.m8, aOther.mFields.m8, sizeof(mFields.m8)) == 0;
}

otError Address::FromString(const char *aBuf)
{
    otError     error  = OT_ERROR_NONE;
    uint8_t *   dst    = reinterpret_cast<uint8_t *>(mFields.m8);
    uint8_t *   endp   = reinterpret_cast<uint8_t *>(mFields.m8 + 15);
    uint8_t *   colonp = NULL;
    const char *colonc = NULL;
    uint16_t    val    = 0;
    uint8_t     count  = 0;
    bool        first  = true;
    bool        hasIp4 = false;
    char        ch;
    uint8_t     d;

    Clear();

    dst--;

    for (;;)
    {
        ch = *aBuf++;
        d  = ch & 0xf;

        if (('a' <= ch && ch <= 'f') || ('A' <= ch && ch <= 'F'))
        {
            d += 9;
        }
        else if (ch == ':' || ch == '\0' || ch == ' ')
        {
            if (count)
            {
                VerifyOrExit(dst + 2 <= endp, error = OT_ERROR_PARSE);
                *(dst + 1) = static_cast<uint8_t>(val >> 8);
                *(dst + 2) = static_cast<uint8_t>(val);
                dst += 2;
                count = 0;
                val   = 0;
            }
            else if (ch == ':')
            {
                VerifyOrExit(colonp == NULL || first, error = OT_ERROR_PARSE);
                colonp = dst;
            }

            if (ch == '\0' || ch == ' ')
            {
                break;
            }

            colonc = aBuf;

            continue;
        }
        else if (ch == '.')
        {
            hasIp4 = true;

            // Do not count bytes of the embedded IPv4 address.
            endp -= kIp4AddressSize;

            VerifyOrExit(dst <= endp, error = OT_ERROR_PARSE);

            break;
        }
        else
        {
            VerifyOrExit('0' <= ch && ch <= '9', error = OT_ERROR_PARSE);
        }

        first = false;
        val   = static_cast<uint16_t>((val << 4) | d);
        VerifyOrExit(++count <= 4, error = OT_ERROR_PARSE);
    }

    VerifyOrExit(colonp || dst == endp, error = OT_ERROR_PARSE);

    while (colonp && dst > colonp)
    {
        *endp-- = *dst--;
    }

    while (endp > dst)
    {
        *endp-- = 0;
    }

    if (hasIp4)
    {
        val = 0;

        // Reset the start and end pointers.
        dst  = reinterpret_cast<uint8_t *>(mFields.m8 + 12);
        endp = reinterpret_cast<uint8_t *>(mFields.m8 + 15);

        for (;;)
        {
            ch = *colonc++;

            if (ch == '.' || ch == '\0' || ch == ' ')
            {
                VerifyOrExit(dst <= endp, error = OT_ERROR_PARSE);

                *dst++ = static_cast<uint8_t>(val);
                val    = 0;

                if (ch == '\0' || ch == ' ')
                {
                    // Check if embedded IPv4 address had exactly four parts.
                    VerifyOrExit(dst == endp + 1, error = OT_ERROR_PARSE);
                    break;
                }
            }
            else
            {
                VerifyOrExit('0' <= ch && ch <= '9', error = OT_ERROR_PARSE);

                val = (10 * val) + (ch & 0xf);

                // Single part of IPv4 address has to fit in one byte.
                VerifyOrExit(val <= 0xff, error = OT_ERROR_PARSE);
            }
        }
    }

exit:
    return error;
}

Address::InfoString Address::ToString(void) const
{
    return InfoString("%x:%x:%x:%x:%x:%x:%x:%x", HostSwap16(mFields.m16[0]), HostSwap16(mFields.m16[1]),
                      HostSwap16(mFields.m16[2]), HostSwap16(mFields.m16[3]), HostSwap16(mFields.m16[4]),
                      HostSwap16(mFields.m16[5]), HostSwap16(mFields.m16[6]), HostSwap16(mFields.m16[7]));
}

} // namespace Ip6
} // namespace ot
<commit_msg>[ip6-address] simplify IsAnycastServiceLocator()  (#4519)<commit_after>/*
 *  Copyright (c) 2016, The OpenThread Authors.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions are met:
 *  1. Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *  2. Redistributions in binary form must reproduce the above copyright
 *     notice, this list of conditions and the following disclaimer in the
 *     documentation and/or other materials provided with the distribution.
 *  3. Neither the name of the copyright holder nor the
 *     names of its contributors may be used to endorse or promote products
 *     derived from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 *  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 *  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 *  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 *  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 *  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 *  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 *  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 *  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 *  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 */

/**
 * @file
 *   This file implements IPv6 addresses.
 */

#include "ip6_address.hpp"

#include <stdio.h>

#include "common/code_utils.hpp"
#include "common/encoding.hpp"
#include "common/instance.hpp"

using ot::Encoding::BigEndian::HostSwap16;
using ot::Encoding::BigEndian::HostSwap32;

namespace ot {
namespace Ip6 {

void Address::Clear(void)
{
    memset(mFields.m8, 0, sizeof(mFields));
}

bool Address::IsUnspecified(void) const
{
    return (mFields.m32[0] == 0 && mFields.m32[1] == 0 && mFields.m32[2] == 0 && mFields.m32[3] == 0);
}

bool Address::IsLoopback(void) const
{
    return (mFields.m32[0] == 0 && mFields.m32[1] == 0 && mFields.m32[2] == 0 && mFields.m32[3] == HostSwap32(1));
}

bool Address::IsLinkLocal(void) const
{
    return (mFields.m16[0] & HostSwap16(0xffc0)) == HostSwap16(0xfe80);
}

bool Address::IsMulticast(void) const
{
    return mFields.m8[0] == 0xff;
}

bool Address::IsLinkLocalMulticast(void) const
{
    return IsMulticast() && (GetScope() == kLinkLocalScope);
}

bool Address::IsLinkLocalAllNodesMulticast(void) const
{
    return (mFields.m32[0] == HostSwap32(0xff020000) && mFields.m32[1] == 0 && mFields.m32[2] == 0 &&
            mFields.m32[3] == HostSwap32(0x01));
}

bool Address::IsLinkLocalAllRoutersMulticast(void) const
{
    return (mFields.m32[0] == HostSwap32(0xff020000) && mFields.m32[1] == 0 && mFields.m32[2] == 0 &&
            mFields.m32[3] == HostSwap32(0x02));
}

bool Address::IsRealmLocalMulticast(void) const
{
    return IsMulticast() && (GetScope() == kRealmLocalScope);
}

bool Address::IsMulticastLargerThanRealmLocal(void) const
{
    return IsMulticast() && (GetScope() > kRealmLocalScope);
}

bool Address::IsRealmLocalAllNodesMulticast(void) const
{
    return (mFields.m32[0] == HostSwap32(0xff030000) && mFields.m32[1] == 0 && mFields.m32[2] == 0 &&
            mFields.m32[3] == HostSwap32(0x01));
}

bool Address::IsRealmLocalAllRoutersMulticast(void) const
{
    return (mFields.m32[0] == HostSwap32(0xff030000) && mFields.m32[1] == 0 && mFields.m32[2] == 0 &&
            mFields.m32[3] == HostSwap32(0x02));
}

bool Address::IsRealmLocalAllMplForwarders(void) const
{
    return (mFields.m32[0] == HostSwap32(0xff030000) && mFields.m32[1] == 0 && mFields.m32[2] == 0 &&
            mFields.m32[3] == HostSwap32(0xfc));
}

bool Address::IsRoutingLocator(void) const
{
    return (mFields.m32[2] == HostSwap32(0x000000ff) && mFields.m16[6] == HostSwap16(0xfe00) &&
            mFields.m8[14] < kAloc16Mask && (mFields.m8[14] & kRloc16ReservedBitMask) == 0);
}

bool Address::IsAnycastRoutingLocator(void) const
{
    return (mFields.m32[2] == HostSwap32(0x000000ff) && mFields.m16[6] == HostSwap16(0xfe00) &&
            mFields.m8[14] == kAloc16Mask);
}

bool Address::IsAnycastServiceLocator(void) const
{
    return IsAnycastRoutingLocator() && (mFields.m8[15] >= (Mle::kAloc16ServiceStart & 0xff)) &&
           (mFields.m8[15] <= (Mle::kAloc16ServiceEnd & 0xff));
}

bool Address::IsSubnetRouterAnycast(void) const
{
    return (mFields.m32[2] == 0 && mFields.m32[3] == 0);
}

bool Address::IsReservedSubnetAnycast(void) const
{
    return (mFields.m32[2] == HostSwap32(0xfdffffff) && mFields.m16[6] == 0xffff && mFields.m8[14] == 0xff &&
            mFields.m8[15] >= 0x80);
}

bool Address::IsIidReserved(void) const
{
    return IsSubnetRouterAnycast() || IsReservedSubnetAnycast() || IsAnycastRoutingLocator();
}

const uint8_t *Address::GetIid(void) const
{
    return mFields.m8 + kInterfaceIdentifierOffset;
}

uint8_t *Address::GetIid(void)
{
    return mFields.m8 + kInterfaceIdentifierOffset;
}

void Address::SetIid(const uint8_t *aIid)
{
    memcpy(mFields.m8 + kInterfaceIdentifierOffset, aIid, kInterfaceIdentifierSize);
}

void Address::SetIid(const Mac::ExtAddress &aExtAddress)
{
    Mac::ExtAddress addr;

    addr = aExtAddress;
    addr.ToggleLocal();
    addr.CopyTo(mFields.m8 + kInterfaceIdentifierOffset);
}

void Address::ToExtAddress(Mac::ExtAddress &aExtAddress) const
{
    aExtAddress.Set(mFields.m8 + kInterfaceIdentifierOffset);
    aExtAddress.ToggleLocal();
}

void Address::ToExtAddress(Mac::Address &aMacAddress) const
{
    aMacAddress.SetExtended(mFields.m8 + kInterfaceIdentifierOffset);
    aMacAddress.GetExtended().ToggleLocal();
}

uint8_t Address::GetScope(void) const
{
    uint8_t rval;

    if (IsMulticast())
    {
        rval = mFields.m8[1] & 0xf;
    }
    else if (IsLinkLocal())
    {
        rval = kLinkLocalScope;
    }
    else if (IsLoopback())
    {
        rval = kNodeLocalScope;
    }
    else
    {
        rval = kGlobalScope;
    }

    return rval;
}

uint8_t Address::PrefixMatch(const uint8_t *aPrefixA, const uint8_t *aPrefixB, uint8_t aMaxLength)
{
    uint8_t rval = 0;
    uint8_t diff;

    if (aMaxLength > sizeof(Address))
    {
        aMaxLength = sizeof(Address);
    }

    for (uint8_t i = 0; i < aMaxLength; i++)
    {
        diff = aPrefixA[i] ^ aPrefixB[i];

        if (diff == 0)
        {
            rval += 8;
        }
        else
        {
            while ((diff & 0x80) == 0)
            {
                rval++;
                diff <<= 1;
            }

            break;
        }
    }

    return rval;
}

uint8_t Address::PrefixMatch(const otIp6Address &aOther) const
{
    return PrefixMatch(mFields.m8, aOther.mFields.m8, sizeof(Address));
}

bool Address::operator==(const Address &aOther) const
{
    return memcmp(mFields.m8, aOther.mFields.m8, sizeof(mFields.m8)) == 0;
}

otError Address::FromString(const char *aBuf)
{
    otError     error  = OT_ERROR_NONE;
    uint8_t *   dst    = reinterpret_cast<uint8_t *>(mFields.m8);
    uint8_t *   endp   = reinterpret_cast<uint8_t *>(mFields.m8 + 15);
    uint8_t *   colonp = NULL;
    const char *colonc = NULL;
    uint16_t    val    = 0;
    uint8_t     count  = 0;
    bool        first  = true;
    bool        hasIp4 = false;
    char        ch;
    uint8_t     d;

    Clear();

    dst--;

    for (;;)
    {
        ch = *aBuf++;
        d  = ch & 0xf;

        if (('a' <= ch && ch <= 'f') || ('A' <= ch && ch <= 'F'))
        {
            d += 9;
        }
        else if (ch == ':' || ch == '\0' || ch == ' ')
        {
            if (count)
            {
                VerifyOrExit(dst + 2 <= endp, error = OT_ERROR_PARSE);
                *(dst + 1) = static_cast<uint8_t>(val >> 8);
                *(dst + 2) = static_cast<uint8_t>(val);
                dst += 2;
                count = 0;
                val   = 0;
            }
            else if (ch == ':')
            {
                VerifyOrExit(colonp == NULL || first, error = OT_ERROR_PARSE);
                colonp = dst;
            }

            if (ch == '\0' || ch == ' ')
            {
                break;
            }

            colonc = aBuf;

            continue;
        }
        else if (ch == '.')
        {
            hasIp4 = true;

            // Do not count bytes of the embedded IPv4 address.
            endp -= kIp4AddressSize;

            VerifyOrExit(dst <= endp, error = OT_ERROR_PARSE);

            break;
        }
        else
        {
            VerifyOrExit('0' <= ch && ch <= '9', error = OT_ERROR_PARSE);
        }

        first = false;
        val   = static_cast<uint16_t>((val << 4) | d);
        VerifyOrExit(++count <= 4, error = OT_ERROR_PARSE);
    }

    VerifyOrExit(colonp || dst == endp, error = OT_ERROR_PARSE);

    while (colonp && dst > colonp)
    {
        *endp-- = *dst--;
    }

    while (endp > dst)
    {
        *endp-- = 0;
    }

    if (hasIp4)
    {
        val = 0;

        // Reset the start and end pointers.
        dst  = reinterpret_cast<uint8_t *>(mFields.m8 + 12);
        endp = reinterpret_cast<uint8_t *>(mFields.m8 + 15);

        for (;;)
        {
            ch = *colonc++;

            if (ch == '.' || ch == '\0' || ch == ' ')
            {
                VerifyOrExit(dst <= endp, error = OT_ERROR_PARSE);

                *dst++ = static_cast<uint8_t>(val);
                val    = 0;

                if (ch == '\0' || ch == ' ')
                {
                    // Check if embedded IPv4 address had exactly four parts.
                    VerifyOrExit(dst == endp + 1, error = OT_ERROR_PARSE);
                    break;
                }
            }
            else
            {
                VerifyOrExit('0' <= ch && ch <= '9', error = OT_ERROR_PARSE);

                val = (10 * val) + (ch & 0xf);

                // Single part of IPv4 address has to fit in one byte.
                VerifyOrExit(val <= 0xff, error = OT_ERROR_PARSE);
            }
        }
    }

exit:
    return error;
}

Address::InfoString Address::ToString(void) const
{
    return InfoString("%x:%x:%x:%x:%x:%x:%x:%x", HostSwap16(mFields.m16[0]), HostSwap16(mFields.m16[1]),
                      HostSwap16(mFields.m16[2]), HostSwap16(mFields.m16[3]), HostSwap16(mFields.m16[4]),
                      HostSwap16(mFields.m16[5]), HostSwap16(mFields.m16[6]), HostSwap16(mFields.m16[7]));
}

} // namespace Ip6
} // namespace ot
<|endoftext|>


<commit_before>/*
 * Author: Brendan Le Foll <brendan.le.foll@intel.com>
 * Copyright (c) 2014 Intel Corporation.
 *
 * Permission is hereby granted, free of charge, to any person obtaining
 * a copy of this software and associated documentation files (the
 * "Software"), to deal in the Software without restriction, including
 * without limitation the rights to use, copy, modify, merge, publish,
 * distribute, sublicense, and/or sell copies of the Software, and to
 * permit persons to whom the Software is furnished to do so, subject to
 * the following conditions:
 *
 * The above copyright notice and this permission notice shall be
 * included in all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
 * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE
 * LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
 * OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
 * WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
 */

#pragma once

#include "spi.h"
#include <stdexcept>

namespace mraa
{

/**
 * MRAA SPI Modes
 */
typedef enum {
    SPI_MODE0 = 0, /**< CPOL = 0, CPHA = 0, Clock idle low, data is clocked in on rising edge,
                      output data (change) on falling edge */
    SPI_MODE1 = 1, /**< CPOL = 0, CPHA = 1, Clock idle low, data is clocked in on falling edge,
                      output data (change) on rising edge */
    SPI_MODE2 = 2, /**< CPOL = 1, CPHA = 0, Clock idle low, data is clocked in on falling edge,
                      output data (change) on rising edge */
    SPI_MODE3 = 3, /**< CPOL = 1, CPHA = 1, Clock idle low, data is clocked in on rising, edge
                      output data (change) on falling edge */
} Spi_Mode;

/**
* @brief API to Serial Peripheral Interface
*
* This file defines the SPI interface for libmraa
*
* @snippet Spi-pot.cpp Interesting
*/
class Spi
{
  public:
    /**
     * Initialise SPI object using the board mapping to set muxes
     *
     * @param bus to use, as listed in the platform definition, normally 0
     */
    Spi(int bus)
    {
        m_spi = mraa_spi_init(bus);

        if (m_spi == NULL) {
            throw std::invalid_argument("Error initialising SPI bus");
        }
    }

    /**
     * Closes spi bus
     */
    ~Spi()
    {
        mraa_spi_stop(m_spi);
    }

    /**
     * Set the SPI device mode. see spidev0-3
     *
     * @param mode the mode. See Linux spidev doc
     * @return Result of operation
     */
    mraa_result_t
    mode(Spi_Mode mode)
    {
        return mraa_spi_mode(m_spi, (mraa_spi_mode_t) mode);
    }

    /**
     * Set the SPI device operating clock frequency
     *
     * @param hz the frequency to set in hz
     * @return Result of operation
     */
    mraa_result_t
    frequency(int hz)
    {
        return mraa_spi_frequency(m_spi, hz);
    }

    /**
     * Write single byte to the SPI device
     *
     * @param data the byte to send
     * @return data received on the miso line or -1 in case of error
     */
    int
    writeByte(uint8_t data)
    {
        return mraa_spi_write(m_spi, (uint8_t) data);
    }

    /**
     * Write single byte to the SPI device
     *
     * @param data the byte to send
     * @return data received on the miso line
     */
    uint16_t
    write_word(uint16_t data)
    {
        return mraa_spi_write_word(m_spi, (uint16_t) data);
    }

    /**
     * Write buffer of bytes to SPI device The pointer return has to be
     * free'd by the caller. It will return a NULL pointer in cases of
     * error
     *
     * @param txBuf buffer to send
     * @param length size of buffer to send
     * @return uint8_t* data received on the miso line. Same length as passed in
     */
    uint8_t*
    write(uint8_t* txBuf, int length)
    {
        return mraa_spi_write_buf(m_spi, txBuf, length);
    }

#ifndef SWIG
    /**
     * Write buffer of bytes to SPI device The pointer return has to be
     * free'd by the caller. It will return a NULL pointer in cases of
     * error
     *
     * @param txBuf buffer to send
     * @param length size of buffer (in bytes) to send
     * @return uint8_t* data received on the miso line. Same length as passed in
     */
    uint16_t*
    write_word(uint16_t* txBuf, int length)
    {
        return mraa_spi_write_buf_word(m_spi, txBuf, length);
    }
#endif

#ifndef SWIG
    /**
     * Transfer data to and from SPI device Receive pointer may be null if
     * return data is not needed.
     *
     * @param data buffer to send
     * @param rxBuf buffer to optionally receive data from spi device
     * @param length size of buffer to send
     * @return Result of operation
     */
    mraa_result_t
    transfer(uint8_t* txBuf, uint8_t* rxBuf, int length)
    {
        return mraa_spi_transfer_buf(m_spi, txBuf, rxBuf, length);
    }

    /**
     * Transfer data to and from SPI device Receive pointer may be null if
     * return data is not needed.
     *
     * @param data buffer to send
     * @param rxBuf buffer to optionally receive data from spi device
     * @param length size of buffer to send
     * @return Result of operation
     */
    mraa_result_t
    transfer_word(uint16_t* txBuf, uint16_t* rxBuf, int length)
    {
        return mraa_spi_transfer_buf_word(m_spi, txBuf, rxBuf, length);
    }
#endif

    /**
     * Change the SPI lsb mode
     *
     * @param lsb Use least significant bit transmission - 0 for msbi
     * @return Result of operation
     */
    mraa_result_t
    lsbmode(bool lsb)
    {
        return mraa_spi_lsbmode(m_spi, (mraa_boolean_t) lsb);
    }

    /**
     * Set bits per mode on transaction, default is 8
     *
     * @param bits bits per word
     * @return Result of operation
     */
    mraa_result_t
    bitPerWord(unsigned int bits)
    {
        return mraa_spi_bit_per_word(m_spi, bits);
    }

  private:
    mraa_spi_context m_spi;
};
}
<commit_msg>spi.hpp: fix wrong docstrings data -> txBuf<commit_after>/*
 * Author: Brendan Le Foll <brendan.le.foll@intel.com>
 * Copyright (c) 2014 Intel Corporation.
 *
 * Permission is hereby granted, free of charge, to any person obtaining
 * a copy of this software and associated documentation files (the
 * "Software"), to deal in the Software without restriction, including
 * without limitation the rights to use, copy, modify, merge, publish,
 * distribute, sublicense, and/or sell copies of the Software, and to
 * permit persons to whom the Software is furnished to do so, subject to
 * the following conditions:
 *
 * The above copyright notice and this permission notice shall be
 * included in all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
 * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE
 * LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
 * OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
 * WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
 */

#pragma once

#include "spi.h"
#include <stdexcept>

namespace mraa
{

/**
 * MRAA SPI Modes
 */
typedef enum {
    SPI_MODE0 = 0, /**< CPOL = 0, CPHA = 0, Clock idle low, data is clocked in on rising edge,
                      output data (change) on falling edge */
    SPI_MODE1 = 1, /**< CPOL = 0, CPHA = 1, Clock idle low, data is clocked in on falling edge,
                      output data (change) on rising edge */
    SPI_MODE2 = 2, /**< CPOL = 1, CPHA = 0, Clock idle low, data is clocked in on falling edge,
                      output data (change) on rising edge */
    SPI_MODE3 = 3, /**< CPOL = 1, CPHA = 1, Clock idle low, data is clocked in on rising, edge
                      output data (change) on falling edge */
} Spi_Mode;

/**
* @brief API to Serial Peripheral Interface
*
* This file defines the SPI interface for libmraa
*
* @snippet Spi-pot.cpp Interesting
*/
class Spi
{
  public:
    /**
     * Initialise SPI object using the board mapping to set muxes
     *
     * @param bus to use, as listed in the platform definition, normally 0
     */
    Spi(int bus)
    {
        m_spi = mraa_spi_init(bus);

        if (m_spi == NULL) {
            throw std::invalid_argument("Error initialising SPI bus");
        }
    }

    /**
     * Closes spi bus
     */
    ~Spi()
    {
        mraa_spi_stop(m_spi);
    }

    /**
     * Set the SPI device mode. see spidev0-3
     *
     * @param mode the mode. See Linux spidev doc
     * @return Result of operation
     */
    mraa_result_t
    mode(Spi_Mode mode)
    {
        return mraa_spi_mode(m_spi, (mraa_spi_mode_t) mode);
    }

    /**
     * Set the SPI device operating clock frequency
     *
     * @param hz the frequency to set in hz
     * @return Result of operation
     */
    mraa_result_t
    frequency(int hz)
    {
        return mraa_spi_frequency(m_spi, hz);
    }

    /**
     * Write single byte to the SPI device
     *
     * @param data the byte to send
     * @return data received on the miso line or -1 in case of error
     */
    int
    writeByte(uint8_t data)
    {
        return mraa_spi_write(m_spi, (uint8_t) data);
    }

    /**
     * Write single byte to the SPI device
     *
     * @param data the byte to send
     * @return data received on the miso line
     */
    uint16_t
    write_word(uint16_t data)
    {
        return mraa_spi_write_word(m_spi, (uint16_t) data);
    }

    /**
     * Write buffer of bytes to SPI device The pointer return has to be
     * free'd by the caller. It will return a NULL pointer in cases of
     * error
     *
     * @param txBuf buffer to send
     * @param length size of buffer to send
     * @return uint8_t* data received on the miso line. Same length as passed in
     */
    uint8_t*
    write(uint8_t* txBuf, int length)
    {
        return mraa_spi_write_buf(m_spi, txBuf, length);
    }

#ifndef SWIG
    /**
     * Write buffer of bytes to SPI device The pointer return has to be
     * free'd by the caller. It will return a NULL pointer in cases of
     * error
     *
     * @param txBuf buffer to send
     * @param length size of buffer (in bytes) to send
     * @return uint8_t* data received on the miso line. Same length as passed in
     */
    uint16_t*
    write_word(uint16_t* txBuf, int length)
    {
        return mraa_spi_write_buf_word(m_spi, txBuf, length);
    }
#endif

#ifndef SWIG
    /**
     * Transfer data to and from SPI device Receive pointer may be null if
     * return data is not needed.
     *
     * @param txBuf buffer to send
     * @param rxBuf buffer to optionally receive data from spi device
     * @param length size of buffer to send
     * @return Result of operation
     */
    mraa_result_t
    transfer(uint8_t* txBuf, uint8_t* rxBuf, int length)
    {
        return mraa_spi_transfer_buf(m_spi, txBuf, rxBuf, length);
    }

    /**
     * Transfer data to and from SPI device Receive pointer may be null if
     * return data is not needed.
     *
     * @param txBuf buffer to send
     * @param rxBuf buffer to optionally receive data from spi device
     * @param length size of buffer to send
     * @return Result of operation
     */
    mraa_result_t
    transfer_word(uint16_t* txBuf, uint16_t* rxBuf, int length)
    {
        return mraa_spi_transfer_buf_word(m_spi, txBuf, rxBuf, length);
    }
#endif

    /**
     * Change the SPI lsb mode
     *
     * @param lsb Use least significant bit transmission - 0 for msbi
     * @return Result of operation
     */
    mraa_result_t
    lsbmode(bool lsb)
    {
        return mraa_spi_lsbmode(m_spi, (mraa_boolean_t) lsb);
    }

    /**
     * Set bits per mode on transaction, default is 8
     *
     * @param bits bits per word
     * @return Result of operation
     */
    mraa_result_t
    bitPerWord(unsigned int bits)
    {
        return mraa_spi_bit_per_word(m_spi, bits);
    }

  private:
    mraa_spi_context m_spi;
};
}
<|endoftext|>


<commit_before>#pragma once

#include <openMVG/features/descriptor.hpp>
#include <openMVG/features/image_describer.hpp>
#include <openMVG/features/regions_factory.hpp>

extern "C" {
#include "nonFree/sift/vl/sift.h"
}

#include <cereal/cereal.hpp>

#include <iostream>
#include <numeric>

namespace openMVG {
namespace features {

struct SiftParams
{
  SiftParams(
    int first_octave = 0,
    int num_octaves = 6,
    int num_scales = 3,
    float edge_threshold = 10.0f,
    float peak_threshold = 0.04f,
    //
    std::size_t gridSize = 4,
    std::size_t maxTotalKeypoints = 1000,
    //
    bool root_sift = true
  ):
    _first_octave(first_octave),
    _num_octaves(num_octaves),
    _num_scales(num_scales),
    _edge_threshold(edge_threshold),
    _peak_threshold(peak_threshold),
    //
    _gridSize(gridSize),
    _maxTotalKeypoints(maxTotalKeypoints),
    //
    _root_sift(root_sift) {}

  template<class Archive>
  void serialize( Archive & ar )
  {
    ar(
      cereal::make_nvp("first_octave", _first_octave),
      cereal::make_nvp("num_octaves",_num_octaves),
      cereal::make_nvp("num_scales",_num_scales),
      cereal::make_nvp("edge_threshold",_edge_threshold),
      cereal::make_nvp("peak_threshold",_peak_threshold),
      //
      cereal::make_nvp("grid_size", _gridSize),
      cereal::make_nvp("max_total_keypoints", _maxTotalKeypoints),
      //
      cereal::make_nvp("root_sift",_root_sift));
  }

  // Parameters
  int _first_octave;      // Use original image, or perform an upscale if == -1
  int _num_octaves;       // Max octaves count
  int _num_scales;        // Scales per octave
  float _edge_threshold;  // Max ratio of Hessian eigenvalues
  float _peak_threshold;  // Min contrast
  //
  std::size_t _gridSize;
  std::size_t _maxTotalKeypoints;
  //
  bool _root_sift;        // see [1]

  bool setPreset(EDESCRIBER_PRESET preset)
  {
    switch(preset)
    {
    case LOW_PRESET:
    {
      _maxTotalKeypoints = 1000;
      _peak_threshold = 0.04f;
      _first_octave = 2;
      break;
    }
    case MEDIUM_PRESET:
    {
      _maxTotalKeypoints = 5000;
      _peak_threshold = 0.04f;
      _first_octave = 1;
      break;
    }
    case NORMAL_PRESET:
    {
      _maxTotalKeypoints = 10000;
      _peak_threshold = 0.04f;
      break;
    }
    case HIGH_PRESET:
    {
      _maxTotalKeypoints = 20000;
      _peak_threshold = 0.01f;
      break;
    }
    case ULTRA_PRESET:
    {
      _maxTotalKeypoints = 40000;
      _peak_threshold = 0.01f;
      _first_octave = -1;
      break;
    }
    default:
      return false;
    }
    return true;
  }

};

//convertSIFT
//////////////////////////////
template < typename TOut >
inline void convertSIFT(
  const vl_sift_pix* descr,
  Descriptor<TOut,128> & descriptor,
  bool brootSift = false
  );

template <>
inline void convertSIFT<float>(
  const vl_sift_pix* descr,
  Descriptor<float,128> &descriptor,
  bool brootSift)
{
  if(brootSift)
  {
    const float sum = std::accumulate(descr, descr + 128, 0.0f);
    for(int k = 0; k < 128; ++k)
      descriptor[k] = std::floor(512.f*sqrt(descr[k] / sum));
  }
  else
  {
    for(int k = 0; k < 128; ++k)
      descriptor[k] = std::floor(512.f*descr[k]);
  }
}

template <>
inline void convertSIFT<unsigned char>(
  const vl_sift_pix* descr,
  Descriptor<unsigned char,128> & descriptor,
  bool brootSift)
{
  if (brootSift)
  {
    // rootsift = sqrt( sift / sum(sift) );
    const float sum = std::accumulate(descr, descr+128, 0.0f);
    for (int k=0;k<128;++k)
      descriptor[k] = static_cast<unsigned char>(512.f*sqrt(descr[k]/sum));
  }
  else
  {
    for (int k=0;k<128;++k)
      descriptor[k] = static_cast<unsigned char>(512.f*descr[k]);
  }
}

/**
 * @brief Extract SIFT regions (in float or unsigned char).
 *
 * @param image
 * @param regions
 * @param params
 * @param bOrientation
 * @param mask
 * @return
 */
template < typename T >
bool extractSIFT(const image::Image<unsigned char>& image,
    std::unique_ptr<Regions> &regions,
    const SiftParams& params,
    bool bOrientation,
    const image::Image<unsigned char> * mask)
{
  const int w = image.Width(), h = image.Height();
  //Convert to float
  const image::Image<float> imageFloat(image.GetMat().cast<float>());

  VlSiftFilt *filt = vl_sift_new(w, h, params._num_octaves, params._num_scales, params._first_octave);
  if (params._edge_threshold >= 0)
    vl_sift_set_edge_thresh(filt, params._edge_threshold);
  if (params._peak_threshold >= 0)
    vl_sift_set_peak_thresh(filt, 255.0 * params._peak_threshold/params._num_scales);

  Descriptor<vl_sift_pix, 128> vlFeatDescriptor;
  Descriptor<T, 128> descriptor;

  // Process SIFT computation
  vl_sift_process_first_octave(filt, imageFloat.data());

  typedef Scalar_Regions<SIOPointFeature,T,128> SIFT_Region_T;
  regions.reset( new SIFT_Region_T );

  // Build alias to cached data
  SIFT_Region_T * regionsCasted = dynamic_cast<SIFT_Region_T*>(regions.get());
  // reserve some memory for faster keypoint saving
  const std::size_t reserveSize = (params._gridSize && params._maxTotalKeypoints) ? params._maxTotalKeypoints : 2000;
  regionsCasted->Features().reserve(reserveSize);
  regionsCasted->Descriptors().reserve(reserveSize);

  while (true)
  {
    vl_sift_detect(filt);

    VlSiftKeypoint const *keys  = vl_sift_get_keypoints(filt);
    const int nkeys = vl_sift_get_nkeypoints(filt);

    // Update gradient before launching parallel extraction
    vl_sift_update_gradient(filt);

    std::cout << "TEST octave_width: " << filt->octave_width << std::endl;
    #ifdef OPENMVG_USE_OPENMP
    #pragma omp parallel for private(vlFeatDescriptor, descriptor)
    #endif
    for (int i = 0; i < nkeys; ++i)
    {

      // Feature masking
      if (mask)
      {
        const image::Image<unsigned char> & maskIma = *mask;
        if (maskIma(keys[i].y, keys[i].x) > 0)
          continue;
      }

      double angles [4] = {0.0, 0.0, 0.0, 0.0};
      int nangles = 1; // by default (1 upright feature)
      if (bOrientation)
      { // compute from 1 to 4 orientations
        nangles = vl_sift_calc_keypoint_orientations(filt, angles, keys+i);
      }

      for (int q=0 ; q < nangles ; ++q)
      {
        vl_sift_calc_keypoint_descriptor(filt, &vlFeatDescriptor[0], keys+i, angles[q]);
        const SIOPointFeature fp(keys[i].x, keys[i].y,
          keys[i].sigma, static_cast<float>(angles[q]));

        convertSIFT<T>(&vlFeatDescriptor[0], descriptor, params._root_sift);
        #ifdef OPENMVG_USE_OPENMP
        #pragma omp critical
        #endif
        {
          regionsCasted->Descriptors().push_back(descriptor);
          regionsCasted->Features().push_back(fp);
        }

      }
    }

    if (vl_sift_process_next_octave(filt))
      break; // Last octave
  }
  vl_sift_delete(filt);

  const auto& features = regionsCasted->Features();
  const auto& descriptors = regionsCasted->Descriptors();

  //Sorting the extracted features according to their scale
  {
    std::vector<std::size_t> indexSort(features.size());
    std::iota(indexSort.begin(), indexSort.end(), 0);
    std::sort(indexSort.begin(), indexSort.end(), [&](std::size_t a, std::size_t b){ return features[a].scale() > features[b].scale(); });

    std::vector<typename SIFT_Region_T::FeatureT> sortedFeatures(features.size());
    std::vector<typename SIFT_Region_T::DescriptorT> sortedDescriptors(features.size());
    for(std::size_t i: indexSort)
    {
      sortedFeatures[i] = features[indexSort[i]];
      sortedDescriptors[i] = descriptors[indexSort[i]];
    }
    regionsCasted->Features().swap(sortedFeatures);
    regionsCasted->Descriptors().swap(sortedDescriptors);
  }

  // Grid filtering of the keypoints to ensure a global repartition
  if(params._gridSize && params._maxTotalKeypoints)
  {
    // Only filter features if we have more features than the maxTotalKeypoints
    if(features.size() > params._maxTotalKeypoints)
    {
      std::vector<typename SIFT_Region_T::FeatureT> filtered_keypoints;
      std::vector<typename SIFT_Region_T::FeatureT> rejected_keypoints;
      filtered_keypoints.reserve(std::min(features.size(), params._maxTotalKeypoints));
      rejected_keypoints.reserve(features.size());

      const std::size_t sizeMat = params._gridSize * params._gridSize;
      std::vector<std::size_t> countFeatPerCell(sizeMat, 0);
      for (int Indice = 0; Indice < sizeMat; Indice++)
      {
    	  countFeatPerCell[Indice] = 0;
      }
      const std::size_t keypointsPerCell = params._maxTotalKeypoints / sizeMat;
      const double regionWidth = w / double(params._gridSize);
      const double regionHeight = h / double(params._gridSize);

      //std::cout << "Grid filtering -- keypointsPerCell: " << keypointsPerCell
      //          << ", regionWidth: " << regionWidth
      //          << ", regionHeight: " << regionHeight << std::endl;

      for(const auto& keypoint: features)
      {
        const std::size_t cellX = std::min(std::size_t(keypoint.x() / regionWidth), params._gridSize);
        const std::size_t cellY = std::min(std::size_t(keypoint.y() / regionHeight), params._gridSize);
        //std::cout << "- keypoint.pt.x: " << keypoint.pt.x << ", keypoint.pt.y: " << keypoint.pt.y << std::endl;
        //std::cout << "- cellX: " << cellX << ", cellY: " << cellY << std::endl;
        //std::cout << "- countFeatPerCell: " << countFeatPerCell << std::endl;
        //std::cout << "- gridSize: " << _params.gridSize << std::endl;

        std::size_t &count = countFeatPerCell[cellX*params._gridSize + cellY];
        ++count;

        if(count < keypointsPerCell)
          filtered_keypoints.push_back(keypoint);
        else
          rejected_keypoints.push_back(keypoint);
      }
      // If we don't have enough features (less than maxTotalKeypoints) after the grid filtering (empty regions in the grid for example).
      // We add the best other ones, without repartition constraint.
      if( filtered_keypoints.size() < params._maxTotalKeypoints )
      {
        const std::size_t remainingElements = std::min(rejected_keypoints.size(), params._maxTotalKeypoints - filtered_keypoints.size());
        std::cout << "Grid filtering -- Copy remaining points: " << remainingElements << std::endl;
        filtered_keypoints.insert(filtered_keypoints.end(), rejected_keypoints.begin(), rejected_keypoints.begin() + remainingElements);
      }

      regionsCasted->Features().swap(filtered_keypoints);
    }
  }

  return true;
}

} //namespace openMVG
} //namespace features

<commit_msg>[sift] remove cout<commit_after>#pragma once

#include <openMVG/features/descriptor.hpp>
#include <openMVG/features/image_describer.hpp>
#include <openMVG/features/regions_factory.hpp>

extern "C" {
#include "nonFree/sift/vl/sift.h"
}

#include <cereal/cereal.hpp>

#include <iostream>
#include <numeric>

namespace openMVG {
namespace features {

struct SiftParams
{
  SiftParams(
    int first_octave = 0,
    int num_octaves = 6,
    int num_scales = 3,
    float edge_threshold = 10.0f,
    float peak_threshold = 0.04f,
    //
    std::size_t gridSize = 4,
    std::size_t maxTotalKeypoints = 1000,
    //
    bool root_sift = true
  ):
    _first_octave(first_octave),
    _num_octaves(num_octaves),
    _num_scales(num_scales),
    _edge_threshold(edge_threshold),
    _peak_threshold(peak_threshold),
    //
    _gridSize(gridSize),
    _maxTotalKeypoints(maxTotalKeypoints),
    //
    _root_sift(root_sift) {}

  template<class Archive>
  void serialize( Archive & ar )
  {
    ar(
      cereal::make_nvp("first_octave", _first_octave),
      cereal::make_nvp("num_octaves",_num_octaves),
      cereal::make_nvp("num_scales",_num_scales),
      cereal::make_nvp("edge_threshold",_edge_threshold),
      cereal::make_nvp("peak_threshold",_peak_threshold),
      //
      cereal::make_nvp("grid_size", _gridSize),
      cereal::make_nvp("max_total_keypoints", _maxTotalKeypoints),
      //
      cereal::make_nvp("root_sift",_root_sift));
  }

  // Parameters
  int _first_octave;      // Use original image, or perform an upscale if == -1
  int _num_octaves;       // Max octaves count
  int _num_scales;        // Scales per octave
  float _edge_threshold;  // Max ratio of Hessian eigenvalues
  float _peak_threshold;  // Min contrast
  //
  std::size_t _gridSize;
  std::size_t _maxTotalKeypoints;
  //
  bool _root_sift;        // see [1]

  bool setPreset(EDESCRIBER_PRESET preset)
  {
    switch(preset)
    {
    case LOW_PRESET:
    {
      _maxTotalKeypoints = 1000;
      _peak_threshold = 0.04f;
      _first_octave = 2;
      break;
    }
    case MEDIUM_PRESET:
    {
      _maxTotalKeypoints = 5000;
      _peak_threshold = 0.04f;
      _first_octave = 1;
      break;
    }
    case NORMAL_PRESET:
    {
      _maxTotalKeypoints = 10000;
      _peak_threshold = 0.04f;
      break;
    }
    case HIGH_PRESET:
    {
      _maxTotalKeypoints = 20000;
      _peak_threshold = 0.01f;
      break;
    }
    case ULTRA_PRESET:
    {
      _maxTotalKeypoints = 40000;
      _peak_threshold = 0.01f;
      _first_octave = -1;
      break;
    }
    default:
      return false;
    }
    return true;
  }

};

//convertSIFT
//////////////////////////////
template < typename TOut >
inline void convertSIFT(
  const vl_sift_pix* descr,
  Descriptor<TOut,128> & descriptor,
  bool brootSift = false
  );

template <>
inline void convertSIFT<float>(
  const vl_sift_pix* descr,
  Descriptor<float,128> &descriptor,
  bool brootSift)
{
  if(brootSift)
  {
    const float sum = std::accumulate(descr, descr + 128, 0.0f);
    for(int k = 0; k < 128; ++k)
      descriptor[k] = std::floor(512.f*sqrt(descr[k] / sum));
  }
  else
  {
    for(int k = 0; k < 128; ++k)
      descriptor[k] = std::floor(512.f*descr[k]);
  }
}

template <>
inline void convertSIFT<unsigned char>(
  const vl_sift_pix* descr,
  Descriptor<unsigned char,128> & descriptor,
  bool brootSift)
{
  if (brootSift)
  {
    // rootsift = sqrt( sift / sum(sift) );
    const float sum = std::accumulate(descr, descr+128, 0.0f);
    for (int k=0;k<128;++k)
      descriptor[k] = static_cast<unsigned char>(512.f*sqrt(descr[k]/sum));
  }
  else
  {
    for (int k=0;k<128;++k)
      descriptor[k] = static_cast<unsigned char>(512.f*descr[k]);
  }
}

/**
 * @brief Extract SIFT regions (in float or unsigned char).
 *
 * @param image
 * @param regions
 * @param params
 * @param bOrientation
 * @param mask
 * @return
 */
template < typename T >
bool extractSIFT(const image::Image<unsigned char>& image,
    std::unique_ptr<Regions> &regions,
    const SiftParams& params,
    bool bOrientation,
    const image::Image<unsigned char> * mask)
{
  const int w = image.Width(), h = image.Height();
  //Convert to float
  const image::Image<float> imageFloat(image.GetMat().cast<float>());

  VlSiftFilt *filt = vl_sift_new(w, h, params._num_octaves, params._num_scales, params._first_octave);
  if (params._edge_threshold >= 0)
    vl_sift_set_edge_thresh(filt, params._edge_threshold);
  if (params._peak_threshold >= 0)
    vl_sift_set_peak_thresh(filt, 255.0 * params._peak_threshold/params._num_scales);

  Descriptor<vl_sift_pix, 128> vlFeatDescriptor;
  Descriptor<T, 128> descriptor;

  // Process SIFT computation
  vl_sift_process_first_octave(filt, imageFloat.data());

  typedef Scalar_Regions<SIOPointFeature,T,128> SIFT_Region_T;
  regions.reset( new SIFT_Region_T );

  // Build alias to cached data
  SIFT_Region_T * regionsCasted = dynamic_cast<SIFT_Region_T*>(regions.get());
  // reserve some memory for faster keypoint saving
  const std::size_t reserveSize = (params._gridSize && params._maxTotalKeypoints) ? params._maxTotalKeypoints : 2000;
  regionsCasted->Features().reserve(reserveSize);
  regionsCasted->Descriptors().reserve(reserveSize);

  while (true)
  {
    vl_sift_detect(filt);

    VlSiftKeypoint const *keys  = vl_sift_get_keypoints(filt);
    const int nkeys = vl_sift_get_nkeypoints(filt);

    // Update gradient before launching parallel extraction
    vl_sift_update_gradient(filt);

    #ifdef OPENMVG_USE_OPENMP
    #pragma omp parallel for private(vlFeatDescriptor, descriptor)
    #endif
    for (int i = 0; i < nkeys; ++i)
    {

      // Feature masking
      if (mask)
      {
        const image::Image<unsigned char> & maskIma = *mask;
        if (maskIma(keys[i].y, keys[i].x) > 0)
          continue;
      }

      double angles [4] = {0.0, 0.0, 0.0, 0.0};
      int nangles = 1; // by default (1 upright feature)
      if (bOrientation)
      { // compute from 1 to 4 orientations
        nangles = vl_sift_calc_keypoint_orientations(filt, angles, keys+i);
      }

      for (int q=0 ; q < nangles ; ++q)
      {
        vl_sift_calc_keypoint_descriptor(filt, &vlFeatDescriptor[0], keys+i, angles[q]);
        const SIOPointFeature fp(keys[i].x, keys[i].y,
          keys[i].sigma, static_cast<float>(angles[q]));

        convertSIFT<T>(&vlFeatDescriptor[0], descriptor, params._root_sift);
        #ifdef OPENMVG_USE_OPENMP
        #pragma omp critical
        #endif
        {
          regionsCasted->Descriptors().push_back(descriptor);
          regionsCasted->Features().push_back(fp);
        }

      }
    }

    if (vl_sift_process_next_octave(filt))
      break; // Last octave
  }
  vl_sift_delete(filt);

  const auto& features = regionsCasted->Features();
  const auto& descriptors = regionsCasted->Descriptors();

  //Sorting the extracted features according to their scale
  {
    std::vector<std::size_t> indexSort(features.size());
    std::iota(indexSort.begin(), indexSort.end(), 0);
    std::sort(indexSort.begin(), indexSort.end(), [&](std::size_t a, std::size_t b){ return features[a].scale() > features[b].scale(); });

    std::vector<typename SIFT_Region_T::FeatureT> sortedFeatures(features.size());
    std::vector<typename SIFT_Region_T::DescriptorT> sortedDescriptors(features.size());
    for(std::size_t i: indexSort)
    {
      sortedFeatures[i] = features[indexSort[i]];
      sortedDescriptors[i] = descriptors[indexSort[i]];
    }
    regionsCasted->Features().swap(sortedFeatures);
    regionsCasted->Descriptors().swap(sortedDescriptors);
  }

  // Grid filtering of the keypoints to ensure a global repartition
  if(params._gridSize && params._maxTotalKeypoints)
  {
    // Only filter features if we have more features than the maxTotalKeypoints
    if(features.size() > params._maxTotalKeypoints)
    {
      std::vector<typename SIFT_Region_T::FeatureT> filtered_keypoints;
      std::vector<typename SIFT_Region_T::FeatureT> rejected_keypoints;
      filtered_keypoints.reserve(std::min(features.size(), params._maxTotalKeypoints));
      rejected_keypoints.reserve(features.size());

      const std::size_t sizeMat = params._gridSize * params._gridSize;
      std::vector<std::size_t> countFeatPerCell(sizeMat, 0);
      for (int Indice = 0; Indice < sizeMat; Indice++)
      {
    	  countFeatPerCell[Indice] = 0;
      }
      const std::size_t keypointsPerCell = params._maxTotalKeypoints / sizeMat;
      const double regionWidth = w / double(params._gridSize);
      const double regionHeight = h / double(params._gridSize);

      //std::cout << "Grid filtering -- keypointsPerCell: " << keypointsPerCell
      //          << ", regionWidth: " << regionWidth
      //          << ", regionHeight: " << regionHeight << std::endl;

      for(const auto& keypoint: features)
      {
        const std::size_t cellX = std::min(std::size_t(keypoint.x() / regionWidth), params._gridSize);
        const std::size_t cellY = std::min(std::size_t(keypoint.y() / regionHeight), params._gridSize);
        //std::cout << "- keypoint.pt.x: " << keypoint.pt.x << ", keypoint.pt.y: " << keypoint.pt.y << std::endl;
        //std::cout << "- cellX: " << cellX << ", cellY: " << cellY << std::endl;
        //std::cout << "- countFeatPerCell: " << countFeatPerCell << std::endl;
        //std::cout << "- gridSize: " << _params.gridSize << std::endl;

        std::size_t &count = countFeatPerCell[cellX*params._gridSize + cellY];
        ++count;

        if(count < keypointsPerCell)
          filtered_keypoints.push_back(keypoint);
        else
          rejected_keypoints.push_back(keypoint);
      }
      // If we don't have enough features (less than maxTotalKeypoints) after the grid filtering (empty regions in the grid for example).
      // We add the best other ones, without repartition constraint.
      if( filtered_keypoints.size() < params._maxTotalKeypoints )
      {
        const std::size_t remainingElements = std::min(rejected_keypoints.size(), params._maxTotalKeypoints - filtered_keypoints.size());
        std::cout << "Grid filtering -- Copy remaining points: " << remainingElements << std::endl;
        filtered_keypoints.insert(filtered_keypoints.end(), rejected_keypoints.begin(), rejected_keypoints.begin() + remainingElements);
      }

      regionsCasted->Features().swap(filtered_keypoints);
    }
  }

  return true;
}

} //namespace openMVG
} //namespace features

<|endoftext|>


<commit_before>#ifndef CXX_OBJECT_HXX
# define CXX_OBJECT_HXX

# include <boost/bind.hpp>

# include <object/atom.hh>
# include <object/primitive-class.hh>
# include <object/string-class.hh>
# include <object/object-class.hh>
# include <object/primitives.hh>

namespace object
{
  template <typename T>
  bool CxxObject::add(const std::string& name, rObject& tgt)
  {
    std::cerr << "REGISTER: " << name << std::endl;
    initializers_get().push_back(new TypeInitializer<T>(name, tgt));
    return true;
  }

  template <typename T>
  CxxObject::TypeInitializer<T>::TypeInitializer(const std::string& name,
                                                 rObject& tgt)
    : Initializer(tgt), name_(name)
  {}

  namespace
  {
    template <typename T>
    rObject cxx_object_clone(runner::Runner&, objects_type args)
    {
      CHECK_ARG_COUNT(1);
      rObject tgt = args[0];
      rObject res;
      if (tgt->is_a<T>())
        res = new T(tgt->as<T>());
      else
        res = new T();
      return res;
    }

    template <typename T>
    rObject cxx_object_id(runner::Runner&, objects_type args)
    {
      CHECK_ARG_COUNT(1);
      return args[0];
    }
  }

  template <typename T>
  void
  CxxObject::TypeInitializer<T>::create()
  {
    res_ = new Object();
  }

  template <typename T>
  rObject
  CxxObject::TypeInitializer<T>::make_class()
  {
    std::cerr << "INIT: " << name_ << std::endl;
    res_->proto_add(object_class);
    res_->slot_set(SYMBOL(protoName), new String(name_));
    res_->slot_set(SYMBOL(clone),
                   rPrimitive(new Primitive(boost::bind(cxx_object_clone<T>,
                                                        _1, _2))));
    Binder<T> b(res_);
    T::initialize(b);

    libport::Symbol conversion =
      libport::Symbol(std::string("as") + name_.name_get());
    if (!res_->slot_locate(conversion, 0))
      res_->slot_set(conversion,
                     rPrimitive(new Primitive(boost::bind(cxx_object_id<T>,
                                                          _1, _2))));
    return res_;
  }

  template <typename T>
  libport::Symbol
  CxxObject::TypeInitializer<T>::name()
  {
    return name_;
  }

  // BINDER

  template <typename T>
  CxxObject::Binder<T>::Binder(rObject tgt)
    : tgt_(tgt)
  {}

  namespace
  {

# define PRINT_true(X) X
# define PRINT_false(X)
# define PRINT_truetrue(X) X
# define PRINT_falsetrue(X)
# define PRINT_truefalse(X)
# define PRINT_falsefalse(X)
# define NPRINT_true(X)
# define NPRINT_false(X) X

# define WHEN(Cond, X) PRINT_##Cond(X)
# define WHEN_NOT(Cond, X) NPRINT_##Cond(X)

# define IF(Cond, Then, Else) WHEN(Cond, Then) WHEN_NOT(Cond, Else)

# define COMMA_ ,
# define COMMA(Cond) PRINT_##Cond(COMMA_)

# define COMMA2(Cond1, Cond2) PRINT_##Cond1##Cond2(COMMA_)

# define MET(Met, Name, Ret, Run, Arg1, Arg2, Arg3)     \
    IF(Ret, libport::shared_ptr<R>, void)               \
    (WHEN(Met, T::)*Name)(                              \
      WHEN(Run, runner::Runner&) COMMA2(Run, Arg1)      \
      WHEN(Arg1, libport::shared_ptr<A1>)               \
      COMMA(Arg2) WHEN(Arg2, libport::shared_ptr<A2>)   \
      COMMA(Arg3) WHEN(Arg3, libport::shared_ptr<A3>)   \
      )

#define GET_ARG(Met, N)                                                 \
    type_check<A##N>(args[N WHEN_NOT(Met, - 1)], name.name_get());     \
    libport::shared_ptr<A##N> a##N =                                   \
      args[N WHEN_NOT(Met, - 1)].unsafe_cast<A##N>();                  \
    assert(a##N);

#define FETCH_TGT()                                                     \
    type_check<T>(args[0], name.name_get());                            \
    libport::shared_ptr<T> tgt = args[0].unsafe_cast<T>();

#define PRIMITIVE(Met, Ret, ArgsC, Run, Arg1, Arg2, Arg3)               \
    template <typename T                                                \
              COMMA(Ret) WHEN(Ret, typename R)                          \
      COMMA(Arg1) WHEN(Arg1, typename A1)                               \
      COMMA(Arg2) WHEN(Arg2, typename A2)                               \
      COMMA(Arg3) WHEN(Arg3, typename A3)                               \
      >                                                                 \
    struct primitive<T, MET(Met, , Ret, Run, Arg1, Arg2, Arg3)>         \
    {                                                                   \
        static rObject make(                                            \
          MET(Met, method, Ret, Run, Arg1, Arg2, Arg3),                 \
          const libport::Symbol& IF(Met, name, WHEN(Arg1, name)),       \
          runner::Runner& WHEN(Run, r),                                 \
          objects_type args)                                            \
        {                                                               \
	  (void)args;							\
          assert(args.size() ==                                         \
                 ArgsC IF(Met, + 1, WHEN_NOT(Arg1, + 1)));              \
          WHEN(Met, FETCH_TGT());                                       \
          WHEN(Arg1, GET_ARG(Met, 1))                                   \
            WHEN(Arg2, GET_ARG(Met, 2))                                 \
            WHEN(Arg3, GET_ARG(Met, 3))                                 \
            WHEN(Ret, return)                                           \
            (WHEN(Met, tgt.get()->*)method)(                            \
              WHEN(Run, r) COMMA2(Run, Arg1)                            \
              WHEN(Arg1, a1)                                            \
              COMMA(Arg2) WHEN(Arg2, a2)                                \
              COMMA(Arg3) WHEN(Arg3, a3)                                \
              );                                                        \
          return void_class;                                            \
        }                                                               \
      }

#define LIST_MET(Name, Met, Ret, Run)                                  \
    IF(Ret, R, void)                                                   \
    (WHEN(Met, T::)*Name)(WHEN(Run, runner::Runner&) COMMA(Run)        \
                          objects_type)                                \

#define LIST_PRIMITIVE(Met, Ret, Run)                                   \
    template <typename T                                                \
              COMMA(Ret) WHEN(Ret, typename R)                          \
      >                                                                 \
    struct primitive<T, LIST_MET(, Met, Ret, Run)>                      \
    {                                                                   \
        static rObject make(                                            \
          LIST_MET(method, Met, Ret, Run),                              \
          const libport::Symbol& name,                                  \
          runner::Runner& WHEN(Run, r),                                 \
          objects_type args)                                            \
        {                                                               \
          WHEN(Met, FETCH_TGT());                                       \
          WHEN(Met, args.pop_front());                                  \
          WHEN(Ret, return)                                             \
            (WHEN(Met, tgt.get()->*)method)(                            \
              WHEN(Run, r) COMMA(Run)                                   \
              args                                                      \
              );                                                        \
          return void_class;                                            \
        }                                                               \
      }

    template <typename T, typename M>
    struct primitive
    {
    };

    /* Python for these:
for met in ('true', 'false'):
    for ret in ('true', 'false'):
        for run in ('true', 'false'):
            print "    LIST_PRIMITIVE(%s, %s, %s);" % (met, ret, run)
     */
    LIST_PRIMITIVE(true, true, true);
    LIST_PRIMITIVE(true, true, false);
    LIST_PRIMITIVE(true, false, true);
    LIST_PRIMITIVE(true, false, false);
    LIST_PRIMITIVE(false, true, true);
    LIST_PRIMITIVE(false, true, false);
    LIST_PRIMITIVE(false, false, true);
    LIST_PRIMITIVE(false, false, false);

    /* Python for these:
max = 3
for ret in ('true', 'false'):
    for run in ('true', 'false'):
        for nargs in range(max + 1):
            for macro in ('PRIMITIVE ', 'PRIMITIVEF'):
                print "    %s(%s, %i, %s" % (macro, ret, nargs, run),
                for i in range(max):
                    print ", %s" % str(i < nargs).lower(),
                print ");"
    */
    PRIMITIVE(true , true, 0, true , false , false , false );
    PRIMITIVE(true , true, 1, true , true , false , false );
    PRIMITIVE(true , true, 2, true , true , true , false );
    PRIMITIVE(true , true, 3, true , true , true , true );
    PRIMITIVE(true , true, 0, false , false , false , false );
    PRIMITIVE(true , true, 1, false , true , false , false );
    PRIMITIVE(true , true, 2, false , true , true , false );
    PRIMITIVE(true , true, 3, false , true , true , true );
    PRIMITIVE(true , false, 0, true , false , false , false );
    PRIMITIVE(true , false, 1, true , true , false , false );
    PRIMITIVE(true , false, 2, true , true , true , false );
    PRIMITIVE(true , false, 3, true , true , true , true );
    PRIMITIVE(true , false, 0, false , false , false , false );
    PRIMITIVE(true , false, 1, false , true , false , false );
    PRIMITIVE(true , false, 2, false , true , true , false );
    PRIMITIVE(true , false, 3, false , true , true , true );
    PRIMITIVE(false, true, 0, true , false , false , false );
    PRIMITIVE(false, true, 1, true , true , false , false );
    PRIMITIVE(false, true, 2, true , true , true , false );
    PRIMITIVE(false, true, 3, true , true , true , true );
    PRIMITIVE(false, true, 0, false , false , false , false );
    PRIMITIVE(false, true, 1, false , true , false , false );
    PRIMITIVE(false, true, 2, false , true , true , false );
    PRIMITIVE(false, true, 3, false , true , true , true );
    PRIMITIVE(false, false, 0, true , false , false , false );
    PRIMITIVE(false, false, 1, true , true , false , false );
    PRIMITIVE(false, false, 2, true , true , true , false );
    PRIMITIVE(false, false, 3, true , true , true , true );
    PRIMITIVE(false, false, 0, false , false , false , false );
    PRIMITIVE(false, false, 1, false , true , false , false );
    PRIMITIVE(false, false, 2, false , true , true , false );
    PRIMITIVE(false, false, 3, false , true , true , true );
  }

  template <typename T>
  template <typename M>
  void CxxObject::Binder<T>::operator()(const libport::Symbol& name,
                                        M method)
  {
    // If make is unfound here, you passed an unsupported pointer type
    // to the binder.
    rObject p =
      new Primitive(boost::bind(primitive<T, M>::make,
                                method, name, _1, _2));
    tgt_->slot_set(name, p);
  }

  template <typename T>
  inline void
  type_check(rObject o, const std::string& fun)
  {
    libport::shared_ptr<CxxObject> co = o.unsafe_cast<CxxObject>();
     // FIXME: I can't fill all the source type for now since some old
     // atoms don't define type_name for now
    if (!co)
      throw object::WrongArgumentType(T::type_name, "Object", fun);
    else if (!o->is_a<T>())
      throw object::WrongArgumentType(T::type_name, co->type_name_get(), fun);
  }

  template <>
  inline void
  type_check<Object>(rObject, const std::string&)
  {}

  template <>
  inline void
  type_check<List>(rObject, const std::string&)
  {}

}

#endif
<commit_msg>Remove forgotten debug output.<commit_after>#ifndef CXX_OBJECT_HXX
# define CXX_OBJECT_HXX

# include <boost/bind.hpp>

# include <object/atom.hh>
# include <object/primitive-class.hh>
# include <object/string-class.hh>
# include <object/object-class.hh>
# include <object/primitives.hh>

namespace object
{
  template <typename T>
  bool CxxObject::add(const std::string& name, rObject& tgt)
  {
    initializers_get().push_back(new TypeInitializer<T>(name, tgt));
    return true;
  }

  template <typename T>
  CxxObject::TypeInitializer<T>::TypeInitializer(const std::string& name,
                                                 rObject& tgt)
    : Initializer(tgt), name_(name)
  {}

  namespace
  {
    template <typename T>
    rObject cxx_object_clone(runner::Runner&, objects_type args)
    {
      CHECK_ARG_COUNT(1);
      rObject tgt = args[0];
      rObject res;
      if (tgt->is_a<T>())
        res = new T(tgt->as<T>());
      else
        res = new T();
      return res;
    }

    template <typename T>
    rObject cxx_object_id(runner::Runner&, objects_type args)
    {
      CHECK_ARG_COUNT(1);
      return args[0];
    }
  }

  template <typename T>
  void
  CxxObject::TypeInitializer<T>::create()
  {
    res_ = new Object();
  }

  template <typename T>
  rObject
  CxxObject::TypeInitializer<T>::make_class()
  {
    res_->proto_add(object_class);
    res_->slot_set(SYMBOL(protoName), new String(name_));
    res_->slot_set(SYMBOL(clone),
                   rPrimitive(new Primitive(boost::bind(cxx_object_clone<T>,
                                                        _1, _2))));
    Binder<T> b(res_);
    T::initialize(b);

    libport::Symbol conversion =
      libport::Symbol(std::string("as") + name_.name_get());
    if (!res_->slot_locate(conversion, 0))
      res_->slot_set(conversion,
                     rPrimitive(new Primitive(boost::bind(cxx_object_id<T>,
                                                          _1, _2))));
    return res_;
  }

  template <typename T>
  libport::Symbol
  CxxObject::TypeInitializer<T>::name()
  {
    return name_;
  }

  // BINDER

  template <typename T>
  CxxObject::Binder<T>::Binder(rObject tgt)
    : tgt_(tgt)
  {}

  namespace
  {

# define PRINT_true(X) X
# define PRINT_false(X)
# define PRINT_truetrue(X) X
# define PRINT_falsetrue(X)
# define PRINT_truefalse(X)
# define PRINT_falsefalse(X)
# define NPRINT_true(X)
# define NPRINT_false(X) X

# define WHEN(Cond, X) PRINT_##Cond(X)
# define WHEN_NOT(Cond, X) NPRINT_##Cond(X)

# define IF(Cond, Then, Else) WHEN(Cond, Then) WHEN_NOT(Cond, Else)

# define COMMA_ ,
# define COMMA(Cond) PRINT_##Cond(COMMA_)

# define COMMA2(Cond1, Cond2) PRINT_##Cond1##Cond2(COMMA_)

# define MET(Met, Name, Ret, Run, Arg1, Arg2, Arg3)     \
    IF(Ret, libport::shared_ptr<R>, void)               \
    (WHEN(Met, T::)*Name)(                              \
      WHEN(Run, runner::Runner&) COMMA2(Run, Arg1)      \
      WHEN(Arg1, libport::shared_ptr<A1>)               \
      COMMA(Arg2) WHEN(Arg2, libport::shared_ptr<A2>)   \
      COMMA(Arg3) WHEN(Arg3, libport::shared_ptr<A3>)   \
      )

#define GET_ARG(Met, N)                                                 \
    type_check<A##N>(args[N WHEN_NOT(Met, - 1)], name.name_get());     \
    libport::shared_ptr<A##N> a##N =                                   \
      args[N WHEN_NOT(Met, - 1)].unsafe_cast<A##N>();                  \
    assert(a##N);

#define FETCH_TGT()                                                     \
    type_check<T>(args[0], name.name_get());                            \
    libport::shared_ptr<T> tgt = args[0].unsafe_cast<T>();

#define PRIMITIVE(Met, Ret, ArgsC, Run, Arg1, Arg2, Arg3)               \
    template <typename T                                                \
              COMMA(Ret) WHEN(Ret, typename R)                          \
      COMMA(Arg1) WHEN(Arg1, typename A1)                               \
      COMMA(Arg2) WHEN(Arg2, typename A2)                               \
      COMMA(Arg3) WHEN(Arg3, typename A3)                               \
      >                                                                 \
    struct primitive<T, MET(Met, , Ret, Run, Arg1, Arg2, Arg3)>         \
    {                                                                   \
        static rObject make(                                            \
          MET(Met, method, Ret, Run, Arg1, Arg2, Arg3),                 \
          const libport::Symbol& IF(Met, name, WHEN(Arg1, name)),       \
          runner::Runner& WHEN(Run, r),                                 \
          objects_type args)                                            \
        {                                                               \
	  (void)args;							\
          assert(args.size() ==                                         \
                 ArgsC IF(Met, + 1, WHEN_NOT(Arg1, + 1)));              \
          WHEN(Met, FETCH_TGT());                                       \
          WHEN(Arg1, GET_ARG(Met, 1))                                   \
            WHEN(Arg2, GET_ARG(Met, 2))                                 \
            WHEN(Arg3, GET_ARG(Met, 3))                                 \
            WHEN(Ret, return)                                           \
            (WHEN(Met, tgt.get()->*)method)(                            \
              WHEN(Run, r) COMMA2(Run, Arg1)                            \
              WHEN(Arg1, a1)                                            \
              COMMA(Arg2) WHEN(Arg2, a2)                                \
              COMMA(Arg3) WHEN(Arg3, a3)                                \
              );                                                        \
          return void_class;                                            \
        }                                                               \
      }

#define LIST_MET(Name, Met, Ret, Run)                                  \
    IF(Ret, R, void)                                                   \
    (WHEN(Met, T::)*Name)(WHEN(Run, runner::Runner&) COMMA(Run)        \
                          objects_type)                                \

#define LIST_PRIMITIVE(Met, Ret, Run)                                   \
    template <typename T                                                \
              COMMA(Ret) WHEN(Ret, typename R)                          \
      >                                                                 \
    struct primitive<T, LIST_MET(, Met, Ret, Run)>                      \
    {                                                                   \
        static rObject make(                                            \
          LIST_MET(method, Met, Ret, Run),                              \
          const libport::Symbol& name,                                  \
          runner::Runner& WHEN(Run, r),                                 \
          objects_type args)                                            \
        {                                                               \
          WHEN(Met, FETCH_TGT());                                       \
          WHEN(Met, args.pop_front());                                  \
          WHEN(Ret, return)                                             \
            (WHEN(Met, tgt.get()->*)method)(                            \
              WHEN(Run, r) COMMA(Run)                                   \
              args                                                      \
              );                                                        \
          return void_class;                                            \
        }                                                               \
      }

    template <typename T, typename M>
    struct primitive
    {
    };

    /* Python for these:
for met in ('true', 'false'):
    for ret in ('true', 'false'):
        for run in ('true', 'false'):
            print "    LIST_PRIMITIVE(%s, %s, %s);" % (met, ret, run)
     */
    LIST_PRIMITIVE(true, true, true);
    LIST_PRIMITIVE(true, true, false);
    LIST_PRIMITIVE(true, false, true);
    LIST_PRIMITIVE(true, false, false);
    LIST_PRIMITIVE(false, true, true);
    LIST_PRIMITIVE(false, true, false);
    LIST_PRIMITIVE(false, false, true);
    LIST_PRIMITIVE(false, false, false);

    /* Python for these:
max = 3
for ret in ('true', 'false'):
    for run in ('true', 'false'):
        for nargs in range(max + 1):
            for macro in ('PRIMITIVE ', 'PRIMITIVEF'):
                print "    %s(%s, %i, %s" % (macro, ret, nargs, run),
                for i in range(max):
                    print ", %s" % str(i < nargs).lower(),
                print ");"
    */
    PRIMITIVE(true , true, 0, true , false , false , false );
    PRIMITIVE(true , true, 1, true , true , false , false );
    PRIMITIVE(true , true, 2, true , true , true , false );
    PRIMITIVE(true , true, 3, true , true , true , true );
    PRIMITIVE(true , true, 0, false , false , false , false );
    PRIMITIVE(true , true, 1, false , true , false , false );
    PRIMITIVE(true , true, 2, false , true , true , false );
    PRIMITIVE(true , true, 3, false , true , true , true );
    PRIMITIVE(true , false, 0, true , false , false , false );
    PRIMITIVE(true , false, 1, true , true , false , false );
    PRIMITIVE(true , false, 2, true , true , true , false );
    PRIMITIVE(true , false, 3, true , true , true , true );
    PRIMITIVE(true , false, 0, false , false , false , false );
    PRIMITIVE(true , false, 1, false , true , false , false );
    PRIMITIVE(true , false, 2, false , true , true , false );
    PRIMITIVE(true , false, 3, false , true , true , true );
    PRIMITIVE(false, true, 0, true , false , false , false );
    PRIMITIVE(false, true, 1, true , true , false , false );
    PRIMITIVE(false, true, 2, true , true , true , false );
    PRIMITIVE(false, true, 3, true , true , true , true );
    PRIMITIVE(false, true, 0, false , false , false , false );
    PRIMITIVE(false, true, 1, false , true , false , false );
    PRIMITIVE(false, true, 2, false , true , true , false );
    PRIMITIVE(false, true, 3, false , true , true , true );
    PRIMITIVE(false, false, 0, true , false , false , false );
    PRIMITIVE(false, false, 1, true , true , false , false );
    PRIMITIVE(false, false, 2, true , true , true , false );
    PRIMITIVE(false, false, 3, true , true , true , true );
    PRIMITIVE(false, false, 0, false , false , false , false );
    PRIMITIVE(false, false, 1, false , true , false , false );
    PRIMITIVE(false, false, 2, false , true , true , false );
    PRIMITIVE(false, false, 3, false , true , true , true );
  }

  template <typename T>
  template <typename M>
  void CxxObject::Binder<T>::operator()(const libport::Symbol& name,
                                        M method)
  {
    // If make is unfound here, you passed an unsupported pointer type
    // to the binder.
    rObject p =
      new Primitive(boost::bind(primitive<T, M>::make,
                                method, name, _1, _2));
    tgt_->slot_set(name, p);
  }

  template <typename T>
  inline void
  type_check(rObject o, const std::string& fun)
  {
    libport::shared_ptr<CxxObject> co = o.unsafe_cast<CxxObject>();
     // FIXME: I can't fill all the source type for now since some old
     // atoms don't define type_name for now
    if (!co)
      throw object::WrongArgumentType(T::type_name, "Object", fun);
    else if (!o->is_a<T>())
      throw object::WrongArgumentType(T::type_name, co->type_name_get(), fun);
  }

  template <>
  inline void
  type_check<Object>(rObject, const std::string&)
  {}

  template <>
  inline void
  type_check<List>(rObject, const std::string&)
  {}

}

#endif
<|endoftext|>


<commit_before>// this source meant to test the behaviour of g++ compiler when there are (syntactical) errors in the source code

// test01 - commenting #include line to pretend it was left out...
// test02 - include but intentionally misspell std_lib_facilities.h
// test03 - "forget" to terminate string on line 9 with a "
// test04 - call main() function using "integer" instead on "int" (this has to do with the expected result of the function)
// test05 - use only one < for directing the "input" for cout - one might expect this to make cout want to "replace" the console? at least if
//		writing a file that might be the case (ie. < is replace where << would be append)

#include "std_lib_facilities.h"

int main() {
	cout < "Hello, World!\n";
	keep_window_open();
	return 0;
}
<commit_msg>setup for test06<commit_after>// this source meant to test the behaviour of g++ compiler when there are (syntactical) errors in the source code

// test01 - commenting #include line to pretend it was left out...
// test02 - include but intentionally misspell std_lib_facilities.h
// test03 - "forget" to terminate string on line 9 with a "
// test04 - call main() function using "integer" instead on "int" (this has to do with the expected result of the function)
// test05 - use only one < for directing the "input" for cout - one might expect this to make cout want to "replace" the console? at least if
//		writing a file that might be the case (ie. < is replace where << would be append)
// test06 - using a single quote ' rather than double " to define the string

#include "std_lib_facilities.h"

int main() {
	cout << 'Hello, World!\n';
	keep_window_open();
	return 0;
}
<|endoftext|>


<commit_before>/**************************************************************************
 * Copyright(c) 1998-2007, ALICE Experiment at CERN, All rights reserved. *
 *                                                                        *
 * Author: The ALICE Off-line Project.                                    *
 * Contributors are mentioned in the code where appropriate.              *
 *                                                                        *
 * Permission to use, copy, modify and distribute this software and its   *
 * documentation strictly for non-commercial purposes is hereby granted   *
 * without fee, provided that the above copyright notice appears in all   *
 * copies and that both the copyright notice and this permission notice   *
 * appear in the supporting documentation. The authors make no claims     *
 * about the suitability of this software for any purpose. It is          *
 * provided "as is" without express or implied warranty.                  *
 **************************************************************************/

/* $Id$ */

//-------------------------------------------------------------------------
//     Event handler for AOD input
//     Author: Andreas Morsch, CERN
//-------------------------------------------------------------------------

#include <TTree.h>
#include <TList.h>
#include <TNamed.h>

#include "AliAODInputHandler.h"
#include "AliAODEvent.h"

ClassImp(AliAODInputHandler)

static Option_t *gAODDataType = "AOD";

//______________________________________________________________________________
AliAODInputHandler::AliAODInputHandler() :
    AliInputEventHandler(),
    fEvent(0),
    fFriends(new TList())
{
  // Default constructor
}

//______________________________________________________________________________
AliAODInputHandler::AliAODInputHandler(const char* name, const char* title):
  AliInputEventHandler(name, title),
  fEvent(0),
  fFriends(new TList())
{
    // Constructor
}

//______________________________________________________________________________
AliAODInputHandler::~AliAODInputHandler()
{
// Destructor
    fFriends->Delete();
}

Bool_t AliAODInputHandler::Init(TTree* tree, Option_t* /*opt*/)
{
    // Initialisation necessary for each new tree
    fTree = tree;
    TIter next(fFriends);
    TNamed* obj;

    if (!fTree) return kFALSE;

    while((obj = (TNamed*)next())) {
	if (fTree->GetTree()) {
	    (fTree->GetTree())->AddFriend("aodTree", obj->GetName());
	} else {
	    fTree->AddFriend("aodTree", obj->GetName());
	}
    }

    SwitchOffBranches();
    SwitchOnBranches();

    // Get pointer to AOD event
    if (fEvent) {
      delete fEvent;
      fEvent = 0;
    }
    fEvent = new AliAODEvent();

    fEvent->ReadFromTree(fTree);
    return kTRUE;
}

Bool_t AliAODInputHandler::BeginEvent(Long64_t /*entry*/)
{
    //
    //if (fTree) fTree->BranchRef();
    return kTRUE;
}

void AliAODInputHandler::AddFriend(char* filename)
{
    // Add a friend tree
    TNamed* obj = new TNamed(filename, filename);
    fFriends->Add(obj);
}

Option_t *AliAODInputHandler::GetDataType() const
{
// Returns handled data type.
   return gAODDataType;
}
<commit_msg>Corrected handling of friens trees in Input (A. Dainese)<commit_after>/**************************************************************************
 * Copyright(c) 1998-2007, ALICE Experiment at CERN, All rights reserved. *
 *                                                                        *
 * Author: The ALICE Off-line Project.                                    *
 * Contributors are mentioned in the code where appropriate.              *
 *                                                                        *
 * Permission to use, copy, modify and distribute this software and its   *
 * documentation strictly for non-commercial purposes is hereby granted   *
 * without fee, provided that the above copyright notice appears in all   *
 * copies and that both the copyright notice and this permission notice   *
 * appear in the supporting documentation. The authors make no claims     *
 * about the suitability of this software for any purpose. It is          *
 * provided "as is" without express or implied warranty.                  *
 **************************************************************************/

/* $Id$ */

//-------------------------------------------------------------------------
//     Event handler for AOD input
//     Author: Andreas Morsch, CERN
//-------------------------------------------------------------------------

#include <TTree.h>
#include <TList.h>
#include <TNamed.h>
#include <TFile.h>

#include "AliAODInputHandler.h"
#include "AliAODEvent.h"

ClassImp(AliAODInputHandler)

static Option_t *gAODDataType = "AOD";

//______________________________________________________________________________
AliAODInputHandler::AliAODInputHandler() :
    AliInputEventHandler(),
    fEvent(0),
    fFriends(new TList())
{
  // Default constructor
}

//______________________________________________________________________________
AliAODInputHandler::AliAODInputHandler(const char* name, const char* title):
  AliInputEventHandler(name, title),
  fEvent(0),
  fFriends(new TList())
{
    // Constructor
}

//______________________________________________________________________________
AliAODInputHandler::~AliAODInputHandler()
{
// Destructor
    fFriends->Delete();
}

Bool_t AliAODInputHandler::Init(TTree* tree, Option_t* /*opt*/)
{
    // Initialisation necessary for each new tree
    fTree = tree;
    TIter next(fFriends);
    TNamed* obj;

    if (!fTree) return kFALSE;

    TString aodTreeFName,aodFriendTreeFName;

    while((obj = (TNamed*)next())) {
	if (fTree->GetTree()) {
	  aodTreeFName = (fTree->GetTree()->GetCurrentFile())->GetName();
	  aodFriendTreeFName = aodTreeFName;
	  aodFriendTreeFName.ReplaceAll("AliAOD.root",obj->GetName());
	  aodFriendTreeFName.ReplaceAll("AliAODs.root",obj->GetName());
	  (fTree->GetTree())->AddFriend("aodTree", aodFriendTreeFName.Data());
	} else {
	  aodTreeFName = (fTree->GetCurrentFile())->GetName();
	  aodFriendTreeFName = aodTreeFName;
	  aodFriendTreeFName.ReplaceAll("AliAOD.root",obj->GetName());
	  aodFriendTreeFName.ReplaceAll("AliAODs.root",obj->GetName());
	  fTree->AddFriend("aodTree", aodFriendTreeFName.Data());
	}
    }

    SwitchOffBranches();
    SwitchOnBranches();

    // Get pointer to AOD event
    if (fEvent) {
      delete fEvent;
      fEvent = 0;
    }
    fEvent = new AliAODEvent();

    fEvent->ReadFromTree(fTree);
    return kTRUE;
}

Bool_t AliAODInputHandler::BeginEvent(Long64_t /*entry*/)
{
    //
    //if (fTree) fTree->BranchRef();
    return kTRUE;
}

void AliAODInputHandler::AddFriend(char* filename)
{
    // Add a friend tree
    TNamed* obj = new TNamed(filename, filename);
    fFriends->Add(obj);
}

Option_t *AliAODInputHandler::GetDataType() const
{
// Returns handled data type.
   return gAODDataType;
}
<|endoftext|>


<commit_before>// =============================================================================
// PROJECT CHRONO - http://projectchrono.org
//
// Copyright (c) 2014 projectchrono.org
// All right reserved.
//
// Use of this source code is governed by a BSD-style license that can be found
// in the LICENSE file at the top level of the distribution and at
// http://projectchrono.org/license-chrono.txt.
//
// =============================================================================
// Authors: Alessandro Tasora, Radu Serban
// =============================================================================

#include "chrono/physics/ChGlobal.h"
#include "chrono/physics/ChLink.h"
#include "chrono/physics/ChSystem.h"

namespace chrono {

// Register into the object factory, to enable run-time dynamic creation and persistence
ChClassRegisterABSTRACT<ChLink> a_registration_ChLink;

ChLink::ChLink(const ChLink& other) : ChLinkBase(other) {
    Body1 = NULL;
    Body2 = NULL;

    react_force = other.react_force;
    react_torque = other.react_torque;
}

void ChLink::UpdateTime(double time) {
    ChTime = time;
}

void ChLink::Update(double time, bool update_assets) {
    // 1 -
    UpdateTime(time);
}

void ChLink::Update(bool update_assets) {
    Update(ChTime, update_assets);  // use the same time
}

void ChLink::ArchiveOUT(ChArchiveOut& marchive) {
    // version number
    marchive.VersionWrite(1);

    // serialize parent class
    ChLinkBase::ArchiveOUT(marchive);

    // serialize all member data:
}

void ChLink::ArchiveIN(ChArchiveIn& marchive) {
    // version number
    int version = marchive.VersionRead();

    // deserialize parent class
    ChLinkBase::ArchiveIN(marchive);

    // deserialize all member data:
}

}  // end namespace chrono
<commit_msg>Update assets inserted in ChLink objects too (just like for assets added to ChBody stuff etc.)<commit_after>// =============================================================================
// PROJECT CHRONO - http://projectchrono.org
//
// Copyright (c) 2014 projectchrono.org
// All right reserved.
//
// Use of this source code is governed by a BSD-style license that can be found
// in the LICENSE file at the top level of the distribution and at
// http://projectchrono.org/license-chrono.txt.
//
// =============================================================================
// Authors: Alessandro Tasora, Radu Serban
// =============================================================================

#include "chrono/physics/ChGlobal.h"
#include "chrono/physics/ChLink.h"
#include "chrono/physics/ChSystem.h"

namespace chrono {

// Register into the object factory, to enable run-time dynamic creation and persistence
ChClassRegisterABSTRACT<ChLink> a_registration_ChLink;

ChLink::ChLink(const ChLink& other) : ChLinkBase(other) {
    Body1 = NULL;
    Body2 = NULL;

    react_force = other.react_force;
    react_torque = other.react_torque;
}

void ChLink::UpdateTime(double time) {
    ChTime = time;
}

void ChLink::Update(double time, bool update_assets) {
    // 1 -
    UpdateTime(time);

    // This will update assets
    ChPhysicsItem::Update(ChTime, update_assets);
}

void ChLink::Update(bool update_assets) {
    Update(ChTime, update_assets);  // use the same time
}

void ChLink::ArchiveOUT(ChArchiveOut& marchive) {
    // version number
    marchive.VersionWrite(1);

    // serialize parent class
    ChLinkBase::ArchiveOUT(marchive);

    // serialize all member data:
}

void ChLink::ArchiveIN(ChArchiveIn& marchive) {
    // version number
    int version = marchive.VersionRead();

    // deserialize parent class
    ChLinkBase::ArchiveIN(marchive);

    // deserialize all member data:
}

}  // end namespace chrono
<|endoftext|>