<commit_before>/*
 *    Copyright (c) 2019, 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.
 */

#include "border_agent.hpp"

#include <chrono>
#include <thread>

#include <fmt/format.h>
#include <mdns/mdns.h>

#include "common/address.hpp"
#include "common/utils.hpp"

namespace ot {

namespace commissioner {

struct BorderAgentOrErrorMsg
{
    BorderAgent mBorderAgent;
    std::string mErrorMsg;
};

static int HandleRecord(const struct sockaddr *from,
                        mdns_entry_type_t      entry,
                        uint16_t               type,
                        uint16_t               rclass,
                        uint32_t               ttl,
                        const void *           data,
                        size_t                 size,
                        size_t                 offset,
                        size_t                 length,
                        void *                 user_data);

Error DiscoverBorderAgent(BorderAgentHandler aBorderAgentHandler, size_t aTimeout)
{
    static constexpr size_t             kDefaultBufferSize = 1024 * 16;
    static constexpr mdns_record_type_t kMdnsQueryType     = MDNS_RECORDTYPE_PTR;
    static const char *                 kServiceName       = "_meshcop._udp.local";

    Error   error = Error::kNone;
    uint8_t buf[kDefaultBufferSize];

    auto begin = std::chrono::system_clock::now();

    int socket = mdns_socket_open_ipv4();
    VerifyOrExit(socket >= 0, error = Error::kTransportFailed);

    if (mdns_query_send(socket, kMdnsQueryType, kServiceName, strlen(kServiceName), buf, sizeof(buf)) != 0)
    {
        ExitNow(error = Error::kTransportFailed);
    }

    while (begin + std::chrono::milliseconds(aTimeout) >= std::chrono::system_clock::now())
    {
        BorderAgentOrErrorMsg curBorderAgentOrErrorMsg;

        mdns_query_recv(socket, buf, sizeof(buf), HandleRecord, &curBorderAgentOrErrorMsg, 1);

        if (!curBorderAgentOrErrorMsg.mErrorMsg.empty())
        {
            aBorderAgentHandler(nullptr, &curBorderAgentOrErrorMsg.mErrorMsg);
        }
        else if (curBorderAgentOrErrorMsg.mBorderAgent.mPresentFlags != 0)
        {
            aBorderAgentHandler(&curBorderAgentOrErrorMsg.mBorderAgent, nullptr);
        }

        std::this_thread::sleep_for(std::chrono::milliseconds(100));
    }

exit:
    if (socket >= 0)
    {
        mdns_socket_close(socket);
    }

    return error;
}

static inline std::string ToString(const mdns_string_t &aString)
{
    return std::string(aString.str, aString.length);
}

/**
 * This is the callback per mDNS record.
 *
 * It may be called multiple times for a single mDNS response.
 *
 */
static int HandleRecord(const struct sockaddr *from,
                        mdns_entry_type_t      entry,
                        uint16_t               type,
                        uint16_t               rclass,
                        uint32_t               ttl,
                        const void *           data,
                        size_t                 size,
                        size_t                 offset,
                        size_t                 length,
                        void *                 border_agent)
{
    Error                   error = Error::kNone;
    struct sockaddr_storage fromAddrStorage;
    Address                 fromAddr;
    std::string             fromAddrStr;
    std::string             entryType;
    char                    nameBuffer[256];

    BorderAgentOrErrorMsg &borderAgentOrErrorMsg = *reinterpret_cast<BorderAgentOrErrorMsg *>(border_agent);
    BorderAgent &          borderAgent           = borderAgentOrErrorMsg.mBorderAgent;
    std::string &          errorMsg              = borderAgentOrErrorMsg.mErrorMsg;

    (void)rclass;
    (void)ttl;

    *reinterpret_cast<struct sockaddr *>(&fromAddrStorage) = *from;
    if (fromAddr.Set(fromAddrStorage) != Error::kNone || fromAddr.ToString(fromAddrStr) != Error::kNone)
    {
        ExitNow(errorMsg = "invalid source address of mDNS response");
    }

    entryType = (entry == MDNS_ENTRYTYPE_ANSWER) ? "answer"
                                                 : ((entry == MDNS_ENTRYTYPE_AUTHORITY) ? "authority" : "additional");
    if (type == MDNS_RECORDTYPE_PTR)
    {
        mdns_string_t nameStr = mdns_record_parse_ptr(data, size, offset, length, nameBuffer, sizeof(nameBuffer));
        (void)nameStr;
        // TODO(wgtdkp): add debug logging.
    }
    else if (type == MDNS_RECORDTYPE_SRV)
    {
        mdns_record_srv_t server = mdns_record_parse_srv(data, size, offset, length, nameBuffer, sizeof(nameBuffer));

        borderAgent.mPort = server.port;
        borderAgent.mPresentFlags |= BorderAgent::kPortBit;
    }
    else if (type == MDNS_RECORDTYPE_A)
    {
        struct sockaddr_storage addrStorage;
        struct sockaddr_in      addr;
        std::string             addrStr;

        mdns_record_parse_a(data, size, offset, length, &addr);

        *reinterpret_cast<struct sockaddr_in *>(&addrStorage) = addr;
        if (fromAddr.Set(addrStorage) != Error::kNone || fromAddr.ToString(addrStr) != Error::kNone)
        {
            ExitNow(errorMsg = "invalid IPv4 address in A record");
        }

        // We prefer AAAA (IPv6) address than A (IPv4) address.
        if (!(borderAgent.mPresentFlags & BorderAgent::kAddrBit))
        {
            borderAgent.mAddr = addrStr;
            borderAgent.mPresentFlags |= BorderAgent::kAddrBit;
        }
    }
    else if (type == MDNS_RECORDTYPE_AAAA)
    {
        struct sockaddr_storage addrStorage;
        struct sockaddr_in6     addr;
        std::string             addrStr;

        mdns_record_parse_aaaa(data, size, offset, length, &addr);

        *reinterpret_cast<struct sockaddr_in6 *>(&addrStorage) = addr;
        if (fromAddr.Set(addrStorage) != Error::kNone || fromAddr.ToString(addrStr) != Error::kNone)
        {
            ExitNow(errorMsg = "invalid IPv6 address in AAAA record");
        }

        borderAgent.mAddr = addrStr;
        borderAgent.mPresentFlags |= BorderAgent::kAddrBit;
    }
    else if (type == MDNS_RECORDTYPE_TXT)
    {
        mdns_record_txt_t txtBuffer[128];
        size_t            parsed;

        parsed =
            mdns_record_parse_txt(data, size, offset, length, txtBuffer, sizeof(txtBuffer) / sizeof(mdns_record_txt_t));

        for (size_t i = 0; i < parsed; ++i)
        {
            auto key   = ToString(txtBuffer[i].key);
            auto value = ToString(txtBuffer[i].value);

            if (key == "rv")
            {
                VerifyOrExit(value == "1", errorMsg = fmt::format("value of TXT Key 'rv' is {} but not 1", value));
            }
            else if (key == "tv")
            {
                borderAgent.mThreadVersion = value;
                borderAgent.mPresentFlags |= BorderAgent::kThreadVersionBit;
            }
            else if (key == "sb")
            {
                ByteArray bitmap;
                if (utils::Hex(bitmap, value) != Error::kNone || bitmap.size() != 4)
                {
                    ExitNow(errorMsg = fmt::format("value of TXT Key 'sb' is invalid: value={}", value));
                }

                borderAgent.mState.mConnectionMode = (bitmap[0] >> 5);
                borderAgent.mState.mThreadIfStatus = (bitmap[0] << 3) >> 6;
                borderAgent.mState.mAvailability   = (bitmap[0] << 5) >> 6;
                borderAgent.mState.mBbrIsActive    = (bitmap[0] << 7) >> 7;
                borderAgent.mState.mBbrIsPrimary   = (bitmap[1] >> 7);
                borderAgent.mPresentFlags |= BorderAgent::kStateBit;
            }
            else if (key == "nn")
            {
                borderAgent.mNetworkName = value;
                borderAgent.mPresentFlags |= BorderAgent::kNetworkNameBit;
            }
            else if (key == "xp")
            {
                ByteArray extendPanId;
                if (utils::Hex(extendPanId, value) != Error::kNone || extendPanId.size() != 8)
                {
                    ExitNow(errorMsg = fmt::format("value of TXT Key 'xp' is invalid: value={}", value));
                }
                else
                {
                    borderAgent.mExtendedPanId = utils::Decode<uint64_t>(extendPanId);
                    borderAgent.mPresentFlags |= BorderAgent::kExtendedPanIdBit;
                }
            }
            else if (key == "vn")
            {
                borderAgent.mVendorName = value;
                borderAgent.mPresentFlags |= BorderAgent::kVendorNameBit;
            }
            else if (key == "mn")
            {
                borderAgent.mModelName = value;
                borderAgent.mPresentFlags |= BorderAgent::kModelNameBit;
            }
            else if (key == "at")
            {
                ByteArray activeTimestamp;
                if (utils::Hex(activeTimestamp, value) != Error::kNone || activeTimestamp.size() != 8)
                {
                    ExitNow(errorMsg = fmt::format("value of TXT Key 'at' is invalid: value={}", value));
                }
                else
                {
                    borderAgent.mActiveTimestamp = Timestamp::Decode(utils::Decode<uint64_t>(activeTimestamp));
                    borderAgent.mPresentFlags |= BorderAgent::kActiveTimestampBit;
                }
            }
            else if (key == "pt")
            {
                ByteArray partitionId;
                if (utils::Hex(partitionId, value) != Error::kNone || partitionId.size() != 4)
                {
                    ExitNow(errorMsg = fmt::format("value of TXT Key 'pt' is invalid: value={}", value));
                }
                else
                {
                    borderAgent.mPartitionId = utils::Decode<uint32_t>(partitionId);
                    borderAgent.mPresentFlags |= BorderAgent::kPartitionIdBit;
                }
            }
            else if (key == "vd")
            {
                borderAgent.mVendorData = value;
                borderAgent.mPresentFlags |= BorderAgent::kVendorDataBit;
            }
            else if (key == "vo")
            {
                ByteArray oui;
                if (utils::Hex(oui, value) != Error::kNone || oui.size() != 3)
                {
                    ExitNow(errorMsg = fmt::format("value of TXT Key 'vo' is invalid: value={}", value));
                }
                else
                {
                    borderAgent.mVendorOui = oui;
                    borderAgent.mPresentFlags |= BorderAgent::kVendorOuiBit;
                }
            }
            else if (key == "dn")
            {
                borderAgent.mDomainName = value;
                borderAgent.mPresentFlags |= BorderAgent::kDomainNameBit;
            }
            else if (key == "sq")
            {
                ByteArray bbrSeqNum;
                if (utils::Hex(bbrSeqNum, value) != Error::kNone || bbrSeqNum.size() != 1)
                {
                    ExitNow(errorMsg = fmt::format("[mDNS] value of TXT Key 'sq' is invalid: {}", value));
                }
                else
                {
                    borderAgent.mBbrSeqNumber = utils::Decode<uint8_t>(bbrSeqNum);
                    borderAgent.mPresentFlags |= BorderAgent::kBbrSeqNumberBit;
                }
            }
            else if (key == "bb")
            {
                ByteArray bbrPort;
                if (utils::Hex(bbrPort, value) != Error::kNone || bbrPort.size() != 2)
                {
                    ExitNow(errorMsg = fmt::format("[mDNS] value of TXT Key 'bb' is invalid: {}", value));
                }
                else
                {
                    borderAgent.mBbrPort = utils::Decode<uint16_t>(bbrPort);
                    borderAgent.mPresentFlags |= BorderAgent::kBbrPortBit;
                }
            }
            else
            {
                // TODO(wgtdkp): add debug logging.
            }
        }
    }
    else
    {
        // TODO(wgtdkp): add debug logging.
    }

exit:
    return error == Error::kNone ? 0 : -1;
}

} // namespace commissioner

} // namespace ot
<commit_msg>[border-agent] fix format of TXT values in mDNS response (#93)<commit_after>/*
 *    Copyright (c) 2019, 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.
 */

#include "border_agent.hpp"

#include <chrono>
#include <thread>

#include <fmt/format.h>
#include <mdns/mdns.h>

#include "common/address.hpp"
#include "common/utils.hpp"

namespace ot {

namespace commissioner {

struct BorderAgentOrErrorMsg
{
    BorderAgent mBorderAgent;
    std::string mErrorMsg;
};

static int HandleRecord(const struct sockaddr *from,
                        mdns_entry_type_t      entry,
                        uint16_t               type,
                        uint16_t               rclass,
                        uint32_t               ttl,
                        const void *           data,
                        size_t                 size,
                        size_t                 offset,
                        size_t                 length,
                        void *                 user_data);

Error DiscoverBorderAgent(BorderAgentHandler aBorderAgentHandler, size_t aTimeout)
{
    static constexpr size_t             kDefaultBufferSize = 1024 * 16;
    static constexpr mdns_record_type_t kMdnsQueryType     = MDNS_RECORDTYPE_PTR;
    static const char *                 kServiceName       = "_meshcop._udp.local";

    Error   error = Error::kNone;
    uint8_t buf[kDefaultBufferSize];

    auto begin = std::chrono::system_clock::now();

    int socket = mdns_socket_open_ipv4();
    VerifyOrExit(socket >= 0, error = Error::kTransportFailed);

    if (mdns_query_send(socket, kMdnsQueryType, kServiceName, strlen(kServiceName), buf, sizeof(buf)) != 0)
    {
        ExitNow(error = Error::kTransportFailed);
    }

    while (begin + std::chrono::milliseconds(aTimeout) >= std::chrono::system_clock::now())
    {
        BorderAgentOrErrorMsg curBorderAgentOrErrorMsg;

        mdns_query_recv(socket, buf, sizeof(buf), HandleRecord, &curBorderAgentOrErrorMsg, 1);

        if (!curBorderAgentOrErrorMsg.mErrorMsg.empty())
        {
            aBorderAgentHandler(nullptr, &curBorderAgentOrErrorMsg.mErrorMsg);
        }
        else if (curBorderAgentOrErrorMsg.mBorderAgent.mPresentFlags != 0)
        {
            aBorderAgentHandler(&curBorderAgentOrErrorMsg.mBorderAgent, nullptr);
        }

        std::this_thread::sleep_for(std::chrono::milliseconds(100));
    }

exit:
    if (socket >= 0)
    {
        mdns_socket_close(socket);
    }

    return error;
}

static inline std::string ToString(const mdns_string_t &aString)
{
    return std::string(aString.str, aString.length);
}

static inline ByteArray ToByteArray(const mdns_string_t &aString)
{
    return ByteArray{aString.str, aString.str + aString.length};
}

/**
 * This is the callback per mDNS record.
 *
 * It may be called multiple times for a single mDNS response.
 *
 */
static int HandleRecord(const struct sockaddr *from,
                        mdns_entry_type_t      entry,
                        uint16_t               type,
                        uint16_t               rclass,
                        uint32_t               ttl,
                        const void *           data,
                        size_t                 size,
                        size_t                 offset,
                        size_t                 length,
                        void *                 border_agent)
{
    Error                   error = Error::kNone;
    struct sockaddr_storage fromAddrStorage;
    Address                 fromAddr;
    std::string             fromAddrStr;
    std::string             entryType;
    char                    nameBuffer[256];

    BorderAgentOrErrorMsg &borderAgentOrErrorMsg = *reinterpret_cast<BorderAgentOrErrorMsg *>(border_agent);
    BorderAgent &          borderAgent           = borderAgentOrErrorMsg.mBorderAgent;
    std::string &          errorMsg              = borderAgentOrErrorMsg.mErrorMsg;

    (void)rclass;
    (void)ttl;

    *reinterpret_cast<struct sockaddr *>(&fromAddrStorage) = *from;
    if (fromAddr.Set(fromAddrStorage) != Error::kNone || fromAddr.ToString(fromAddrStr) != Error::kNone)
    {
        ExitNow(errorMsg = "invalid source address of mDNS response");
    }

    entryType = (entry == MDNS_ENTRYTYPE_ANSWER) ? "answer"
                                                 : ((entry == MDNS_ENTRYTYPE_AUTHORITY) ? "authority" : "additional");
    if (type == MDNS_RECORDTYPE_PTR)
    {
        mdns_string_t nameStr = mdns_record_parse_ptr(data, size, offset, length, nameBuffer, sizeof(nameBuffer));
        (void)nameStr;
        // TODO(wgtdkp): add debug logging.
    }
    else if (type == MDNS_RECORDTYPE_SRV)
    {
        mdns_record_srv_t server = mdns_record_parse_srv(data, size, offset, length, nameBuffer, sizeof(nameBuffer));

        borderAgent.mPort = server.port;
        borderAgent.mPresentFlags |= BorderAgent::kPortBit;
    }
    else if (type == MDNS_RECORDTYPE_A)
    {
        struct sockaddr_storage addrStorage;
        struct sockaddr_in      addr;
        std::string             addrStr;

        mdns_record_parse_a(data, size, offset, length, &addr);

        *reinterpret_cast<struct sockaddr_in *>(&addrStorage) = addr;
        if (fromAddr.Set(addrStorage) != Error::kNone || fromAddr.ToString(addrStr) != Error::kNone)
        {
            ExitNow(errorMsg = "invalid IPv4 address in A record");
        }

        // We prefer AAAA (IPv6) address than A (IPv4) address.
        if (!(borderAgent.mPresentFlags & BorderAgent::kAddrBit))
        {
            borderAgent.mAddr = addrStr;
            borderAgent.mPresentFlags |= BorderAgent::kAddrBit;
        }
    }
    else if (type == MDNS_RECORDTYPE_AAAA)
    {
        struct sockaddr_storage addrStorage;
        struct sockaddr_in6     addr;
        std::string             addrStr;

        mdns_record_parse_aaaa(data, size, offset, length, &addr);

        *reinterpret_cast<struct sockaddr_in6 *>(&addrStorage) = addr;
        if (fromAddr.Set(addrStorage) != Error::kNone || fromAddr.ToString(addrStr) != Error::kNone)
        {
            ExitNow(errorMsg = "invalid IPv6 address in AAAA record");
        }

        borderAgent.mAddr = addrStr;
        borderAgent.mPresentFlags |= BorderAgent::kAddrBit;
    }
    else if (type == MDNS_RECORDTYPE_TXT)
    {
        mdns_record_txt_t txtBuffer[128];
        size_t            parsed;

        parsed =
            mdns_record_parse_txt(data, size, offset, length, txtBuffer, sizeof(txtBuffer) / sizeof(mdns_record_txt_t));

        for (size_t i = 0; i < parsed; ++i)
        {
            auto key         = ToString(txtBuffer[i].key);
            auto value       = ToString(txtBuffer[i].value);
            auto binaryValue = ToByteArray(txtBuffer[i].value);

            if (key == "rv")
            {
                VerifyOrExit(value == "1", errorMsg = fmt::format("value of TXT Key 'rv' is {} but not 1", value));
            }
            else if (key == "tv")
            {
                borderAgent.mThreadVersion = value;
                borderAgent.mPresentFlags |= BorderAgent::kThreadVersionBit;
            }
            else if (key == "sb")
            {
                auto &bitmap = binaryValue;
                if (bitmap.size() != 4)
                {
                    ExitNow(errorMsg = fmt::format("value of TXT Key 'sb' is invalid: value={}", utils::Hex(bitmap)));
                }

                borderAgent.mState.mConnectionMode = (bitmap[0] >> 5);
                borderAgent.mState.mThreadIfStatus = (bitmap[0] << 3) >> 6;
                borderAgent.mState.mAvailability   = (bitmap[0] << 5) >> 6;
                borderAgent.mState.mBbrIsActive    = (bitmap[0] << 7) >> 7;
                borderAgent.mState.mBbrIsPrimary   = (bitmap[1] >> 7);
                borderAgent.mPresentFlags |= BorderAgent::kStateBit;
            }
            else if (key == "nn")
            {
                borderAgent.mNetworkName = value;
                borderAgent.mPresentFlags |= BorderAgent::kNetworkNameBit;
            }
            else if (key == "xp")
            {
                auto &extendPanId = binaryValue;
                if (extendPanId.size() != 8)
                {
                    ExitNow(errorMsg =
                                fmt::format("value of TXT Key 'xp' is invalid: value={}", utils::Hex(extendPanId)));
                }
                else
                {
                    borderAgent.mExtendedPanId = utils::Decode<uint64_t>(extendPanId);
                    borderAgent.mPresentFlags |= BorderAgent::kExtendedPanIdBit;
                }
            }
            else if (key == "vn")
            {
                borderAgent.mVendorName = value;
                borderAgent.mPresentFlags |= BorderAgent::kVendorNameBit;
            }
            else if (key == "mn")
            {
                borderAgent.mModelName = value;
                borderAgent.mPresentFlags |= BorderAgent::kModelNameBit;
            }
            else if (key == "at")
            {
                auto &activeTimestamp = binaryValue;
                if (activeTimestamp.size() != 8)
                {
                    ExitNow(errorMsg =
                                fmt::format("value of TXT Key 'at' is invalid: value={}", utils::Hex(activeTimestamp)));
                }
                else
                {
                    borderAgent.mActiveTimestamp = Timestamp::Decode(utils::Decode<uint64_t>(activeTimestamp));
                    borderAgent.mPresentFlags |= BorderAgent::kActiveTimestampBit;
                }
            }
            else if (key == "pt")
            {
                auto &partitionId = binaryValue;
                if (partitionId.size() != 4)
                {
                    ExitNow(errorMsg =
                                fmt::format("value of TXT Key 'pt' is invalid: value={}", utils::Hex(partitionId)));
                }
                else
                {
                    borderAgent.mPartitionId = utils::Decode<uint32_t>(partitionId);
                    borderAgent.mPresentFlags |= BorderAgent::kPartitionIdBit;
                }
            }
            else if (key == "vd")
            {
                borderAgent.mVendorData = value;
                borderAgent.mPresentFlags |= BorderAgent::kVendorDataBit;
            }
            else if (key == "vo")
            {
                auto &oui = binaryValue;
                if (oui.size() != 3)
                {
                    ExitNow(errorMsg = fmt::format("value of TXT Key 'vo' is invalid: value={}", utils::Hex(oui)));
                }
                else
                {
                    borderAgent.mVendorOui = oui;
                    borderAgent.mPresentFlags |= BorderAgent::kVendorOuiBit;
                }
            }
            else if (key == "dn")
            {
                borderAgent.mDomainName = value;
                borderAgent.mPresentFlags |= BorderAgent::kDomainNameBit;
            }
            else if (key == "sq")
            {
                auto &bbrSeqNum = binaryValue;
                if (bbrSeqNum.size() != 1)
                {
                    ExitNow(errorMsg =
                                fmt::format("[mDNS] value of TXT Key 'sq' is invalid: {}", utils::Hex(bbrSeqNum)));
                }
                else
                {
                    borderAgent.mBbrSeqNumber = utils::Decode<uint8_t>(bbrSeqNum);
                    borderAgent.mPresentFlags |= BorderAgent::kBbrSeqNumberBit;
                }
            }
            else if (key == "bb")
            {
                auto &bbrPort = binaryValue;
                if (bbrPort.size() != 2)
                {
                    ExitNow(errorMsg = fmt::format("[mDNS] value of TXT Key 'bb' is invalid: {}", utils::Hex(bbrPort)));
                }
                else
                {
                    borderAgent.mBbrPort = utils::Decode<uint16_t>(bbrPort);
                    borderAgent.mPresentFlags |= BorderAgent::kBbrPortBit;
                }
            }
            else
            {
                // TODO(wgtdkp): add debug logging.
            }
        }
    }
    else
    {
        // TODO(wgtdkp): add debug logging.
    }

exit:
    return error == Error::kNone ? 0 : -1;
}

} // namespace commissioner

} // namespace ot
<|endoftext|>


<commit_before>/*********************************************************************
* Software License Agreement (BSD License)
*
*  Copyright (c) 2011, Rice University
*  All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
*
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * 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.
*   * Neither the name of the Rice University 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 OWNER 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.
*********************************************************************/

/* Author: Jonathan Sobieski, Mark Moll */

#include "ompl/tools/config/SelfConfig.h"
#include "ompl/control/planners/pdst/PDST.h"

ompl::control::PDST::PDST(const SpaceInformationPtr &si)
    : base::Planner(si, "PDST"), siC_(si.get()), maxDistance_(0.0), bsp_(NULL),
    goalBias_(0.05), goalSampler_(NULL), iter_(1), lastGoalMotion_(NULL)
{
    Planner::declareParam<double>("range", this, &PDST::setRange, &PDST::getRange, "0.:1.:10000.");
    Planner::declareParam<double>("goal_bias", this, &PDST::setGoalBias, &PDST::getGoalBias, "0.:.05:1.");
}

ompl::control::PDST::~PDST(void)
{
    freeMemory();
}

ompl::base::PlannerStatus ompl::control::PDST::solve(const base::PlannerTerminationCondition &ptc)
{
    // Make sure the planner is configured correctly.
    // ompl::base::Planner::checkValidity checks that there is at least one
    // start state and an ompl::base::Goal object specified and throws an
    // exception if this is not the case.
    checkValidity();

    // depending on how the planning problem is set up, this may be necessary
    bsp_->bounds_ = projectionEvaluator_->getBounds();

    goalSampler_ = dynamic_cast<ompl::base::GoalSampleableRegion*>(pdef_->getGoal().get());

    // Initialize to correct values depending on whether or not previous calls to solve
    // generated an approximate or exact solution. If solve is being called for the first
    // time then initializes hasSolution to false and isApproximate to true.
    double distanceToGoal, closestDistanceToGoal = std::numeric_limits<double>::infinity();
    bool hasSolution = (lastGoalMotion_ != NULL);
    bool isApproximate = hasSolution ?
        goalSampler_->isSatisfied(lastGoalMotion_->state_, &closestDistanceToGoal) : true;
    unsigned ndim = projectionEvaluator_->getDimension();

    // If an exact solution has already been found, do not run for another iteration.
    if (hasSolution && !isApproximate)
        return ompl::base::PlannerStatus::EXACT_SOLUTION;

    // Store the start states in startMotions.
    while (base::State *st = const_cast<base::State*>(pis_.nextStart()))
    {
        Motion *startMotion = new Motion(si_->cloneState(st), projectionEvaluator_);
        bsp_->addMotion(startMotion);
        startMotion->heapElement_ = priorityQueue_.insert(startMotion);
        startMotions_.push_back(startMotion);
    }

    // Ensure that we have a state sampler AND a control sampler
    if (!sampler_)
        sampler_ = si_->allocValidStateSampler();
    if (!controlSampler_)
        controlSampler_ = siC_->allocDirectedControlSampler();

    ompl::base::State *scratch = si_->allocState(), *scratch2 = si_->allocState();
    while (!ptc)
    {
        // Get the top priority path.
        Motion *motionSelected = priorityQueue_.top()->data;
        motionSelected->updatePriority();
        priorityQueue_.update(motionSelected->heapElement_);

        Motion *newMotion = propagateFrom(motionSelected, hasSolution, distanceToGoal, scratch);
        if (newMotion == NULL)
            continue;

        // Check if the newMotion reached the goal.
        if (hasSolution)
        {
            closestDistanceToGoal = distanceToGoal;
            lastGoalMotion_ = newMotion;
            isApproximate = false;
            break;
        }
        else if (distanceToGoal < closestDistanceToGoal)
        {
            closestDistanceToGoal = distanceToGoal;
            lastGoalMotion_ = newMotion;
        }

        // subdivide cell that contained selected motion, put motions of that
        // cell in subcells and split motions so that they contained within
        // one subcell
        Cell *cellSelected = motionSelected->cell_;
        std::vector<Motion*> motions;
        motions.swap(cellSelected->motions_);
        cellSelected->subdivide(ndim);
        for (std::vector<Motion*>::iterator m = motions.begin(); m!=motions.end(); m++)
            insertSampleIntoBsp(*m, cellSelected, scratch, scratch2);
        iter_++;
    }
    si_->freeState(scratch);
    si_->freeState(scratch2);

    if (lastGoalMotion_ != NULL)
        hasSolution = true;

    // If a solution path has been computed, save it in the problem definition object.
    if (hasSolution)
    {
        double dt = siC_->getPropagationStepSize();
        PathControl *path = new PathControl(si_);

        // Compute the path by going up the tree of motions.
        std::vector<Motion*> mpath;
        Motion *solution = lastGoalMotion_;
        while (solution->parent_ != NULL)
        {
            mpath.push_back(solution);
            solution = solution->parent_;
        }

        // Add the solution path in order from the start state to the goal.
        for (std::vector<Motion*>::reverse_iterator rIt = mpath.rbegin();
            rIt != mpath.rend(); ++rIt)
        {
            if ((*rIt)->parent_ != NULL)
                path->append((*rIt)->state_, (*rIt)->control_, (*rIt)->controlDuration_ * dt);
            else
                path->append((*rIt)->state_);
        }
        pdef_->addSolutionPath(ompl::base::PathPtr(path), isApproximate, closestDistanceToGoal);
    }

    return ompl::base::PlannerStatus(hasSolution, isApproximate);
}

ompl::control::PDST::Motion* ompl::control::PDST::propagateFrom(
    Motion* motion, bool& goalReached, double& distanceToGoal, base::State* scratch)
{
    base::Goal *goal = pdef_->getGoal().get();

    if (motion->controlDuration_ > 1)
    {
        // sample a point along the trajectory given by motion
        unsigned int duration = rng_.uniformInt(1, motion->controlDuration_);
        if (duration != motion->controlDuration_)
        {
            // split motion into two motions
            motion = motion->split(siC_, duration);
            motion->heapElement_ = priorityQueue_.insert(motion);
        }
    }

    // generate a random nearby state
    if (goalSampler_ && rng_.uniform01() < goalBias_ && goalSampler_->canSample())
        goalSampler_->sampleGoal(scratch);
    else
    {
        if (!sampler_->sampleNear(scratch, motion->state_, maxDistance_))
        {
            goalReached = false;
            distanceToGoal = std::numeric_limits<double>::infinity();
            return NULL;
        }
    }

    // generate a random control
    Control *randomControl = siC_->allocControl();
    // ignore returned duration. It's only the direction we care about; we
    // always propagate for (at most) siC_->getMaxControlDuration() steps
    controlSampler_->sampleTo(randomControl, motion->control_,
        motion->state_, scratch);

    // propagate using the random control and split when crossing cell bounds
    Motion *m = new Motion(si_->allocState(), motion, randomControl, 0, motion->projection_, iter_, NULL),
        *prevm;
    base::State* prevState = si_->cloneState(motion->state_);
    Cell *prevCell = motion->cell_;
    std::vector<Motion*> newMotions;
    unsigned i=1, totalDuration=0;

    while (true)
    {
        siC_->propagate(prevState, randomControl, 1, m->state_);
        if (!si_->isValid(m->state_))
        {
            std::swap(prevState, m->state_);
            goalReached = goal->isSatisfied(m->state_, &distanceToGoal);
            break;
        }
        projectionEvaluator_->project(m->state_, m->projection_);
        m->cell_ = bsp_->stab(m->projection_);
        if (m->cell_ != prevCell)
        {
            if (m->controlDuration_ > 0) // the first propagate call is allowed to cross a cell boundary
            {
                // start a new motion, save previous one
                prevm = m;
                m = new Motion(prevm->state_, prevm, siC_->cloneControl(randomControl),
                    1, motion->projection_, iter_, m->cell_);
                prevm->cell_= prevCell;
                prevm->state_ = si_->cloneState(prevState);
                totalDuration += prevm->controlDuration_;
                newMotions.push_back(prevm);
            }
            prevCell = m->cell_;
        }
        else
            ++(m->controlDuration_);
        if ((goalReached = goal->isSatisfied(m->state_, &distanceToGoal))
            || i == siC_->getMaxControlDuration())
            break;
        std::swap(prevState, m->state_);
        ++i;
    } // end while

    si_->freeState(prevState);
    // add last motion if it has non-zero duration
    if (m->controlDuration_ > 0)
    {
        totalDuration += m->controlDuration_;
        newMotions.push_back(m);
    }
    else
        freeMotion(m);

    // check if total motion duration is long enough
    if (totalDuration >= siC_->getMinControlDuration())
    {
        // if so, add motin segments to the priority queue and bsp.
        for (i=0; i<newMotions.size(); ++i)
        {
            newMotions[i]->heapElement_ = priorityQueue_.insert(newMotions[i]);
            newMotions[i]->cell_->motions_.push_back(newMotions[i]);
        }
        return newMotions.back();
    }
    else
    {
        // otherwise, clean up
        for (i=0; i<newMotions.size(); ++i)
            freeMotion(newMotions[i]);
        goalReached = false;
        distanceToGoal = std::numeric_limits<double>::infinity();
        return NULL;
    }
}

void ompl::control::PDST::insertSampleIntoBsp(Motion *motion, Cell *bsp, base::State* prevState, base::State* state)
{
    bsp->stab(motion->projection_)->addMotion(motion);
    priorityQueue_.update(motion->heapElement_);

    // If the motion is at most 1 step, then it cannot be split across cell
    // bounds. So we're done.
    if (motion->controlDuration_ <= 1)
        return;

    // motion can cross at most one cell boundary, since bsp has just been split
    // into two cells.
    si_->copyState(prevState, motion->parent_->state_);
    base::EuclideanProjection proj(projectionEvaluator_->getDimension()),
        prevProj(projectionEvaluator_->getDimension());
    Cell *cell = NULL, *prevCell = NULL;
    unsigned int i;

    motion->cell_ = bsp->stab(motion->projection_);
    priorityQueue_.update(motion->heapElement_);
    for (i=0; i<motion->controlDuration_; ++i)
    {
        siC_->propagate(prevState, motion->control_, 1, state);
        projectionEvaluator_->project(state, proj);
        if (!prevCell)
        {
            prevCell = bsp->stab(proj);
            // if cell at the start and end are the same, then we don't split
            if (prevCell == motion->cell_)
                break;
        }
        else
        {
            cell = bsp->stab(proj);
            if (cell != prevCell)
            {
                motion = motion->split(siC_, i, si_->cloneState(prevState), prevProj, prevCell);
                motion->heapElement_ = priorityQueue_.insert(motion);
                break;
            }
        }
        std::swap(state, prevState);
        std::swap(cell, prevCell);
        proj.swap(prevProj);
    }
}

void ompl::control::PDST::clear(void)
{
    Planner::clear();
    sampler_.reset();
    controlSampler_.reset();
    startMotions_.clear();
    iter_ = 1;
    lastGoalMotion_ = NULL;
    freeMemory();
    bsp_ = new Cell(1.0, projectionEvaluator_->getBounds(), 0);
}

void ompl::control::PDST::freeMemory(void)
{
    // Iterate over the elements in the priority queue and clear it
    std::vector<Motion*> motions;
    motions.reserve(priorityQueue_.size());
    priorityQueue_.getContent(motions);
    for (std::vector<Motion*>::iterator it = motions.begin(); it < motions.end(); ++it)
        freeMotion(*it);
    priorityQueue_.clear(); // clears the Element objects in the priority queue
    delete bsp_;
    bsp_ = NULL;
}

void ompl::control::PDST::setup(void)
{
    Planner::setup();
    tools::SelfConfig sc(si_, getName());
    sc.configureProjectionEvaluator(projectionEvaluator_);
    sc.configurePlannerRange(maxDistance_);
    bsp_ = new Cell(1.0, projectionEvaluator_->getBounds(), 0);
    lastGoalMotion_ = NULL;
}

void ompl::control::PDST::getPlannerData(ompl::base::PlannerData &data) const
{
    ompl::base::Planner::getPlannerData(data);

    double propagationStepSize = siC_->getPropagationStepSize();
    std::vector<Motion*> motions;
    priorityQueue_.getContent(motions);

    // Add goal vertex
    if (lastGoalMotion_ != NULL)
        data.addGoalVertex(lastGoalMotion_->state_);

    for (std::vector<Motion*>::iterator it = motions.begin(); it < motions.end(); ++it)
    {
        Motion *child = *it;
        Motion *parent = child->parent_;

        if (parent == NULL)
            data.addStartVertex(ompl::base::PlannerDataVertex(child->state_));
        else if (data.hasControls())
            data.addEdge(ompl::base::PlannerDataVertex(parent->state_),
                ompl::base::PlannerDataVertex(child->state_),
                ompl::control::PlannerDataEdgeControl(child->control_,
                    child->controlDuration_ * propagationStepSize));
        else
            data.addEdge(ompl::base::PlannerDataVertex(parent->state_),
                ompl::base::PlannerDataVertex(child->state_));
    }
}

void ompl::control::PDST::Cell::subdivide(unsigned int spaceDimension)
{
    double childVolume = volume_ / 2.0;
    unsigned int nextSplitDimension = (splitDimension_ + 1) % spaceDimension;
    splitValue_ = .5 * (bounds_.low[splitDimension_] + bounds_.high[splitDimension_]);

    left_ = new Cell(childVolume, bounds_, nextSplitDimension);
    left_->bounds_.high[splitDimension_] = splitValue_;
    right_ = new Cell(childVolume, bounds_, nextSplitDimension);
    right_->bounds_.low[splitDimension_] = splitValue_;
}
<commit_msg>fix possible crash with incorrect setup(), handle possible undefined goalSampler_, remove unneeded const_cast<commit_after>/*********************************************************************
* Software License Agreement (BSD License)
*
*  Copyright (c) 2011, Rice University
*  All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
*
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * 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.
*   * Neither the name of the Rice University 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 OWNER 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.
*********************************************************************/

/* Author: Jonathan Sobieski, Mark Moll */

#include "ompl/tools/config/SelfConfig.h"
#include "ompl/control/planners/pdst/PDST.h"

ompl::control::PDST::PDST(const SpaceInformationPtr &si)
    : base::Planner(si, "PDST"), siC_(si.get()), maxDistance_(0.0), bsp_(NULL),
    goalBias_(0.05), goalSampler_(NULL), iter_(1), lastGoalMotion_(NULL)
{
    Planner::declareParam<double>("range", this, &PDST::setRange, &PDST::getRange, "0.:1.:10000.");
    Planner::declareParam<double>("goal_bias", this, &PDST::setGoalBias, &PDST::getGoalBias, "0.:.05:1.");
}

ompl::control::PDST::~PDST(void)
{
    freeMemory();
}

ompl::base::PlannerStatus ompl::control::PDST::solve(const base::PlannerTerminationCondition &ptc)
{
    // Make sure the planner is configured correctly.
    // ompl::base::Planner::checkValidity checks that there is at least one
    // start state and an ompl::base::Goal object specified and throws an
    // exception if this is not the case.
    checkValidity();

    if (!bsp_)
    {
        OMPL_ERROR("PDST was not set up.");
        return base::PlannerStatus::CRASH;
    }

    // depending on how the planning problem is set up, this may be necessary
    bsp_->bounds_ = projectionEvaluator_->getBounds();
    goalSampler_ = dynamic_cast<ompl::base::GoalSampleableRegion*>(pdef_->getGoal().get());

    // Ensure that we have a state sampler AND a control sampler
    if (!sampler_)
        sampler_ = si_->allocValidStateSampler();
    if (!controlSampler_)
        controlSampler_ = siC_->allocDirectedControlSampler();

    // Initialize to correct values depending on whether or not previous calls to solve
    // generated an approximate or exact solution. If solve is being called for the first
    // time then initializes hasSolution to false and isApproximate to true.
    double distanceToGoal, closestDistanceToGoal = std::numeric_limits<double>::infinity();
    bool hasSolution = lastGoalMotion_ != NULL;
    bool isApproximate = hasSolution ? pdef_->getGoal()->isSatisfied(lastGoalMotion_->state_, &closestDistanceToGoal) : true;
    unsigned ndim = projectionEvaluator_->getDimension();

    // If an exact solution has already been found, do not run for another iteration.
    if (hasSolution && !isApproximate)
        return ompl::base::PlannerStatus::EXACT_SOLUTION;

    // Store the start states in startMotions.
    while (const base::State *st = pis_.nextStart())
    {
        Motion *startMotion = new Motion(si_->cloneState(st), projectionEvaluator_);
        bsp_->addMotion(startMotion);
        startMotion->heapElement_ = priorityQueue_.insert(startMotion);
        startMotions_.push_back(startMotion);
    }

    base::State *scratch = si_->allocState();
    base::State *scratch2 = si_->allocState();

    while (!ptc)
    {
        // Get the top priority path.
        Motion *motionSelected = priorityQueue_.top()->data;
        motionSelected->updatePriority();
        priorityQueue_.update(motionSelected->heapElement_);

        Motion *newMotion = propagateFrom(motionSelected, hasSolution, distanceToGoal, scratch);
        if (newMotion == NULL)
            continue;

        // Check if the newMotion reached the goal.
        if (hasSolution)
        {
            closestDistanceToGoal = distanceToGoal;
            lastGoalMotion_ = newMotion;
            isApproximate = false;
            break;
        }
        else if (distanceToGoal < closestDistanceToGoal)
        {
            closestDistanceToGoal = distanceToGoal;
            lastGoalMotion_ = newMotion;
        }

        // subdivide cell that contained selected motion, put motions of that
        // cell in subcells and split motions so that they contained within
        // one subcell
        Cell *cellSelected = motionSelected->cell_;
        std::vector<Motion*> motions;
        motions.swap(cellSelected->motions_);
        cellSelected->subdivide(ndim);
        for (std::vector<Motion*>::iterator m = motions.begin(); m!=motions.end(); m++)
            insertSampleIntoBsp(*m, cellSelected, scratch, scratch2);
        iter_++;
    }
    si_->freeState(scratch);
    si_->freeState(scratch2);

    if (lastGoalMotion_ != NULL)
        hasSolution = true;

    // If a solution path has been computed, save it in the problem definition object.
    if (hasSolution)
    {
        double dt = siC_->getPropagationStepSize();
        PathControl *path = new PathControl(si_);

        // Compute the path by going up the tree of motions.
        std::vector<Motion*> mpath;
        Motion *solution = lastGoalMotion_;
        while (solution->parent_ != NULL)
        {
            mpath.push_back(solution);
            solution = solution->parent_;
        }

        // Add the solution path in order from the start state to the goal.
        for (std::vector<Motion*>::reverse_iterator rIt = mpath.rbegin();
            rIt != mpath.rend(); ++rIt)
        {
            if ((*rIt)->parent_ != NULL)
                path->append((*rIt)->state_, (*rIt)->control_, (*rIt)->controlDuration_ * dt);
            else
                path->append((*rIt)->state_);
        }
        pdef_->addSolutionPath(ompl::base::PathPtr(path), isApproximate, closestDistanceToGoal);
    }

    return ompl::base::PlannerStatus(hasSolution, isApproximate);
}

ompl::control::PDST::Motion* ompl::control::PDST::propagateFrom(
    Motion* motion, bool& goalReached, double& distanceToGoal, base::State* scratch)
{
    base::Goal *goal = pdef_->getGoal().get();

    if (motion->controlDuration_ > 1)
    {
        // sample a point along the trajectory given by motion
        unsigned int duration = rng_.uniformInt(1, motion->controlDuration_);
        if (duration != motion->controlDuration_)
        {
            // split motion into two motions
            motion = motion->split(siC_, duration);
            motion->heapElement_ = priorityQueue_.insert(motion);
        }
    }

    // generate a random nearby state
    if (goalSampler_ && rng_.uniform01() < goalBias_ && goalSampler_->canSample())
        goalSampler_->sampleGoal(scratch);
    else
    {
        if (!sampler_->sampleNear(scratch, motion->state_, maxDistance_))
        {
            goalReached = false;
            distanceToGoal = std::numeric_limits<double>::infinity();
            return NULL;
        }
    }

    // generate a random control
    Control *randomControl = siC_->allocControl();
    // ignore returned duration. It's only the direction we care about; we
    // always propagate for (at most) siC_->getMaxControlDuration() steps
    controlSampler_->sampleTo(randomControl, motion->control_,
        motion->state_, scratch);

    // propagate using the random control and split when crossing cell bounds
    Motion *m = new Motion(si_->allocState(), motion, randomControl, 0, motion->projection_, iter_, NULL),
        *prevm;
    base::State* prevState = si_->cloneState(motion->state_);
    Cell *prevCell = motion->cell_;
    std::vector<Motion*> newMotions;
    unsigned i=1, totalDuration=0;

    while (true)
    {
        siC_->propagate(prevState, randomControl, 1, m->state_);
        if (!si_->isValid(m->state_))
        {
            std::swap(prevState, m->state_);
            goalReached = goal->isSatisfied(m->state_, &distanceToGoal);
            break;
        }
        projectionEvaluator_->project(m->state_, m->projection_);
        m->cell_ = bsp_->stab(m->projection_);
        if (m->cell_ != prevCell)
        {
            if (m->controlDuration_ > 0) // the first propagate call is allowed to cross a cell boundary
            {
                // start a new motion, save previous one
                prevm = m;
                m = new Motion(prevm->state_, prevm, siC_->cloneControl(randomControl),
                    1, motion->projection_, iter_, m->cell_);
                prevm->cell_= prevCell;
                prevm->state_ = si_->cloneState(prevState);
                totalDuration += prevm->controlDuration_;
                newMotions.push_back(prevm);
            }
            prevCell = m->cell_;
        }
        else
            ++(m->controlDuration_);
        if ((goalReached = goal->isSatisfied(m->state_, &distanceToGoal))
            || i == siC_->getMaxControlDuration())
            break;
        std::swap(prevState, m->state_);
        ++i;
    } // end while

    si_->freeState(prevState);
    // add last motion if it has non-zero duration
    if (m->controlDuration_ > 0)
    {
        totalDuration += m->controlDuration_;
        newMotions.push_back(m);
    }
    else
        freeMotion(m);

    // check if total motion duration is long enough
    if (totalDuration >= siC_->getMinControlDuration())
    {
        // if so, add motin segments to the priority queue and bsp.
        for (i=0; i<newMotions.size(); ++i)
        {
            newMotions[i]->heapElement_ = priorityQueue_.insert(newMotions[i]);
            newMotions[i]->cell_->motions_.push_back(newMotions[i]);
        }
        return newMotions.back();
    }
    else
    {
        // otherwise, clean up
        for (i=0; i<newMotions.size(); ++i)
            freeMotion(newMotions[i]);
        goalReached = false;
        distanceToGoal = std::numeric_limits<double>::infinity();
        return NULL;
    }
}

void ompl::control::PDST::insertSampleIntoBsp(Motion *motion, Cell *bsp, base::State* prevState, base::State* state)
{
    bsp->stab(motion->projection_)->addMotion(motion);
    priorityQueue_.update(motion->heapElement_);

    // If the motion is at most 1 step, then it cannot be split across cell
    // bounds. So we're done.
    if (motion->controlDuration_ <= 1)
        return;

    // motion can cross at most one cell boundary, since bsp has just been split
    // into two cells.
    si_->copyState(prevState, motion->parent_->state_);
    base::EuclideanProjection proj(projectionEvaluator_->getDimension()),
        prevProj(projectionEvaluator_->getDimension());
    Cell *cell = NULL, *prevCell = NULL;
    unsigned int i;

    motion->cell_ = bsp->stab(motion->projection_);
    priorityQueue_.update(motion->heapElement_);
    for (i=0; i<motion->controlDuration_; ++i)
    {
        siC_->propagate(prevState, motion->control_, 1, state);
        projectionEvaluator_->project(state, proj);
        if (!prevCell)
        {
            prevCell = bsp->stab(proj);
            // if cell at the start and end are the same, then we don't split
            if (prevCell == motion->cell_)
                break;
        }
        else
        {
            cell = bsp->stab(proj);
            if (cell != prevCell)
            {
                motion = motion->split(siC_, i, si_->cloneState(prevState), prevProj, prevCell);
                motion->heapElement_ = priorityQueue_.insert(motion);
                break;
            }
        }
        std::swap(state, prevState);
        std::swap(cell, prevCell);
        proj.swap(prevProj);
    }
}

void ompl::control::PDST::clear(void)
{
    Planner::clear();
    sampler_.reset();
    controlSampler_.reset();
    startMotions_.clear();
    iter_ = 1;
    lastGoalMotion_ = NULL;
    freeMemory();
    bsp_ = new Cell(1.0, projectionEvaluator_->getBounds(), 0);
}

void ompl::control::PDST::freeMemory(void)
{
    // Iterate over the elements in the priority queue and clear it
    std::vector<Motion*> motions;
    motions.reserve(priorityQueue_.size());
    priorityQueue_.getContent(motions);
    for (std::vector<Motion*>::iterator it = motions.begin(); it < motions.end(); ++it)
        freeMotion(*it);
    priorityQueue_.clear(); // clears the Element objects in the priority queue
    delete bsp_;
    bsp_ = NULL;
}

void ompl::control::PDST::setup(void)
{
    Planner::setup();
    tools::SelfConfig sc(si_, getName());
    sc.configureProjectionEvaluator(projectionEvaluator_);
    sc.configurePlannerRange(maxDistance_);
    bsp_ = new Cell(1.0, projectionEvaluator_->getBounds(), 0);
    lastGoalMotion_ = NULL;
}

void ompl::control::PDST::getPlannerData(ompl::base::PlannerData &data) const
{
    ompl::base::Planner::getPlannerData(data);

    double propagationStepSize = siC_->getPropagationStepSize();
    std::vector<Motion*> motions;
    priorityQueue_.getContent(motions);

    // Add goal vertex
    if (lastGoalMotion_ != NULL)
        data.addGoalVertex(lastGoalMotion_->state_);

    for (std::vector<Motion*>::iterator it = motions.begin(); it < motions.end(); ++it)
    {
        Motion *child = *it;
        Motion *parent = child->parent_;

        if (parent == NULL)
            data.addStartVertex(ompl::base::PlannerDataVertex(child->state_));
        else if (data.hasControls())
            data.addEdge(ompl::base::PlannerDataVertex(parent->state_),
                ompl::base::PlannerDataVertex(child->state_),
                ompl::control::PlannerDataEdgeControl(child->control_,
                    child->controlDuration_ * propagationStepSize));
        else
            data.addEdge(ompl::base::PlannerDataVertex(parent->state_),
                ompl::base::PlannerDataVertex(child->state_));
    }
}

void ompl::control::PDST::Cell::subdivide(unsigned int spaceDimension)
{
    double childVolume = volume_ / 2.0;
    unsigned int nextSplitDimension = (splitDimension_ + 1) % spaceDimension;
    splitValue_ = .5 * (bounds_.low[splitDimension_] + bounds_.high[splitDimension_]);

    left_ = new Cell(childVolume, bounds_, nextSplitDimension);
    left_->bounds_.high[splitDimension_] = splitValue_;
    right_ = new Cell(childVolume, bounds_, nextSplitDimension);
    right_->bounds_.low[splitDimension_] = splitValue_;
}
<|endoftext|>


<commit_before>#include "ContentWindowGraphicsItem.h"
#include "Content.h"
#include "ContentWindowManager.h"
#include "DisplayGroupManager.h"
#include "DisplayGroupGraphicsView.h"
#include "main.h"

qreal ContentWindowGraphicsItem::zCounter_ = 0;

ContentWindowGraphicsItem::ContentWindowGraphicsItem(boost::shared_ptr<ContentWindowManager> contentWindowManager) : ContentWindowInterface(contentWindowManager)
{
    // defaults
    resizing_ = false;

    // graphics items are movable and fire events on geometry changes
    setFlag(QGraphicsItem::ItemIsMovable, true);
    setFlag(QGraphicsItem::ItemSendsGeometryChanges, true);

    // default fill color / opacity
    setBrush(QBrush(QColor(0, 0, 0, 128)));

    // border based on if we're selected or not
    // use the -1 argument to force an update but not emit signals
    setSelected(selected_, (ContentWindowInterface *)-1);

    // current coordinates
    setRect(x_, y_, w_, h_);

    // new items at the front
    // we assume that interface items will be constructed in depth order so this produces the correct result...
    setZToFront();
}

void ContentWindowGraphicsItem::paint(QPainter * painter, const QStyleOptionGraphicsItem * option, QWidget * widget)
{
    QGraphicsRectItem::paint(painter, option, widget);

    boost::shared_ptr<ContentWindowManager> contentWindowManager = getContentWindowManager();

    if(contentWindowManager != NULL)
    {
        // default pen
        QPen pen;

        // button dimensions
        float buttonWidth, buttonHeight;
        contentWindowManager->getButtonDimensions(buttonWidth, buttonHeight);

        // draw close button
        QRectF closeRect(rect().x() + rect().width() - buttonWidth, rect().y(), buttonWidth, buttonHeight);
        pen.setColor(QColor(255,0,0));
        painter->setPen(pen);
        painter->drawRect(closeRect);
        painter->drawLine(QPointF(rect().x() + rect().width() - buttonWidth, rect().y()), QPointF(rect().x() + rect().width(), rect().y() + buttonHeight));
        painter->drawLine(QPointF(rect().x() + rect().width(), rect().y()), QPointF(rect().x() + rect().width() - buttonWidth, rect().y() + buttonHeight));

        // resize indicator
        QRectF resizeRect(rect().x() + rect().width() - buttonWidth, rect().y() + rect().height() - buttonHeight, buttonWidth, buttonHeight);
        pen.setColor(QColor(128,128,128));
        painter->setPen(pen);
        painter->drawRect(resizeRect);
        painter->drawLine(QPointF(rect().x() + rect().width(), rect().y() + rect().height() - buttonHeight), QPointF(rect().x() + rect().width() - buttonWidth, rect().y() + rect().height()));

        // text label

        // set the font
        float fontSize = 24.;

        QFont font;
        font.setPixelSize(fontSize);
        painter->setFont(font);

        // color the text black
        pen.setColor(QColor(0,0,0));
        painter->setPen(pen);

        // scale the text size down to the height of the graphics view
        // and, calculate the bounding rectangle for the text based on this scale
        // the dimensions of the view need to be corrected for the tiled display aspect ratio
        // recall the tiled display UI is only part of the graphics view since we show it at the correct aspect ratio
        float viewWidth = (float)scene()->views()[0]->width();
        float viewHeight = (float)scene()->views()[0]->height();

        float tiledDisplayAspect = (float)g_configuration->getTotalWidth() / (float)g_configuration->getTotalHeight();

        if(viewWidth / viewHeight > tiledDisplayAspect)
        {
            viewWidth = viewHeight * tiledDisplayAspect;
        }
        else if(viewWidth / viewHeight <= tiledDisplayAspect)
        {
            viewHeight = viewWidth / tiledDisplayAspect;
        }

        float verticalTextScale = 1. / viewHeight;
        float horizontalTextScale = viewHeight / viewWidth * verticalTextScale;

        painter->scale(horizontalTextScale, verticalTextScale);

        QRectF textBoundingRect = QRectF(rect().x() / horizontalTextScale, rect().y() / verticalTextScale, rect().width() / horizontalTextScale, rect().height() / verticalTextScale);

        // get the label and render it
        QString label(contentWindowManager->getContent()->getURI().c_str());
        QString labelSection = label.section("/", -1, -1).prepend(" ");
        painter->drawText(textBoundingRect, Qt::AlignLeft | Qt::AlignTop | Qt::TextWordWrap, labelSection);
    }
}

void ContentWindowGraphicsItem::setCoordinates(double x, double y, double w, double h, ContentWindowInterface * source)
{
    ContentWindowInterface::setCoordinates(x, y, w, h, source);

    if(source != this)
    {
        setPos(x_, y_);
        setRect(mapRectFromScene(x_, y_, w_, h_));
    }
}

void ContentWindowGraphicsItem::setPosition(double x, double y, ContentWindowInterface * source)
{
    ContentWindowInterface::setPosition(x, y, source);

    if(source != this)
    {
        setPos(x_, y_);
        setRect(mapRectFromScene(x_, y_, w_, h_));
    }
}

void ContentWindowGraphicsItem::setSize(double w, double h, ContentWindowInterface * source)
{
    ContentWindowInterface::setSize(w, h, source);

    if(source != this)
    {
        setPos(x_, y_);
        setRect(mapRectFromScene(x_, y_, w_, h_));
    }
}

void ContentWindowGraphicsItem::setSelected(bool selected, ContentWindowInterface * source)
{
    ContentWindowInterface::setSelected(selected, source);

    if(source != this)
    {
        // set the pen
        QPen p = pen();

        if(selected_ == true)
        {
            p.setColor(QColor(255,0,0));
        }
        else
        {
            p.setColor(QColor(0,0,0));
        }

        setPen(p);

        // force a redraw
        update();
    }
}

void ContentWindowGraphicsItem::setZToFront()
{
    zCounter_ = zCounter_ + 1;
    setZValue(zCounter_);
}

void ContentWindowGraphicsItem::mouseMoveEvent(QGraphicsSceneMouseEvent * event)
{
    // handle mouse movements differently depending on selected mode of item
    if(selected_ == false)
    {
        if(event->buttons().testFlag(Qt::LeftButton) == true)
        {
            if(resizing_ == true)
            {
                QRectF r = rect();
                QPointF eventPos = event->pos();

                r.setBottomRight(eventPos);

                QRectF sceneRect = mapRectToScene(r);

                double w = sceneRect.width();
                double h = sceneRect.height();

                setSize(w, h);
            }
            else
            {
                QPointF delta = event->pos() - event->lastPos();

                double x = x_ + delta.x();
                double y = y_ + delta.y();

                setPosition(x, y);
            }
        }
    }
    else
    {
        // handle zooms / pans
        QPointF delta = event->scenePos() - event->lastScenePos();

        if(event->buttons().testFlag(Qt::RightButton) == true)
        {
            // increment zoom

            // if this is a touch event, use cross-product for determining change in zoom (counterclockwise rotation == zoom in, etc.)
            // otherwise, use y as the change in zoom
            double zoomDelta;

            if(event->modifiers().testFlag(Qt::AltModifier) == true)
            {
                zoomDelta = (event->scenePos().x()-0.5) * delta.y() - (event->scenePos().y()-0.5) * delta.x();
                zoomDelta *= 2.;
            }
            else
            {
                zoomDelta = delta.y();
            }

            double zoom = zoom_ * (1. - zoomDelta);

            setZoom(zoom);
        }
        else if(event->buttons().testFlag(Qt::LeftButton) == true)
        {
            // pan (move center coordinates)
            double centerX = centerX_ + 2.*delta.x() / zoom_;
            double centerY = centerY_ + 2.*delta.y() / zoom_;

            setCenter(centerX, centerY);
        }
    }
}

void ContentWindowGraphicsItem::mousePressEvent(QGraphicsSceneMouseEvent * event)
{
    boost::shared_ptr<ContentWindowManager> contentWindowManager = getContentWindowManager();

    if(contentWindowManager != NULL)
    {
        // button dimensions
        float buttonWidth, buttonHeight;
        contentWindowManager->getButtonDimensions(buttonWidth, buttonHeight);

        // item rectangle and event position
        QRectF r = rect();
        QPointF eventPos = event->pos();

        // check to see if user clicked on the close button
        if(fabs((r.x()+r.width()) - eventPos.x()) <= buttonWidth && fabs((r.y()) - eventPos.y()) <= buttonHeight)
        {
            close();

            return;
        }

        // check to see if user clicked on the resize button
        if(fabs((r.x()+r.width()) - eventPos.x()) <= buttonWidth && fabs((r.y()+r.height()) - eventPos.y()) <= buttonHeight)
        {
            resizing_ = true;
        }
    }

    // move to the front of the GUI display
    moveToFront();

    QGraphicsItem::mousePressEvent(event);
}

void ContentWindowGraphicsItem::mouseDoubleClickEvent(QGraphicsSceneMouseEvent * event)
{
    bool selected = !selected_;

    setSelected(selected);

    QGraphicsItem::mouseDoubleClickEvent(event);
}

void ContentWindowGraphicsItem::mouseReleaseEvent(QGraphicsSceneMouseEvent * event)
{
    resizing_ = false;

    QGraphicsItem::mouseReleaseEvent(event);
}
<commit_msg>added coordinates, zoom and center information to ContentWindowGraphicsItem in the GUI.<commit_after>#include "ContentWindowGraphicsItem.h"
#include "Content.h"
#include "ContentWindowManager.h"
#include "DisplayGroupManager.h"
#include "DisplayGroupGraphicsView.h"
#include "main.h"

qreal ContentWindowGraphicsItem::zCounter_ = 0;

ContentWindowGraphicsItem::ContentWindowGraphicsItem(boost::shared_ptr<ContentWindowManager> contentWindowManager) : ContentWindowInterface(contentWindowManager)
{
    // defaults
    resizing_ = false;

    // graphics items are movable and fire events on geometry changes
    setFlag(QGraphicsItem::ItemIsMovable, true);
    setFlag(QGraphicsItem::ItemSendsGeometryChanges, true);

    // default fill color / opacity
    setBrush(QBrush(QColor(0, 0, 0, 128)));

    // border based on if we're selected or not
    // use the -1 argument to force an update but not emit signals
    setSelected(selected_, (ContentWindowInterface *)-1);

    // current coordinates
    setRect(x_, y_, w_, h_);

    // new items at the front
    // we assume that interface items will be constructed in depth order so this produces the correct result...
    setZToFront();
}

void ContentWindowGraphicsItem::paint(QPainter * painter, const QStyleOptionGraphicsItem * option, QWidget * widget)
{
    QGraphicsRectItem::paint(painter, option, widget);

    boost::shared_ptr<ContentWindowManager> contentWindowManager = getContentWindowManager();

    if(contentWindowManager != NULL)
    {
        // default pen
        QPen pen;

        // button dimensions
        float buttonWidth, buttonHeight;
        contentWindowManager->getButtonDimensions(buttonWidth, buttonHeight);

        // draw close button
        QRectF closeRect(rect().x() + rect().width() - buttonWidth, rect().y(), buttonWidth, buttonHeight);
        pen.setColor(QColor(255,0,0));
        painter->setPen(pen);
        painter->drawRect(closeRect);
        painter->drawLine(QPointF(rect().x() + rect().width() - buttonWidth, rect().y()), QPointF(rect().x() + rect().width(), rect().y() + buttonHeight));
        painter->drawLine(QPointF(rect().x() + rect().width(), rect().y()), QPointF(rect().x() + rect().width() - buttonWidth, rect().y() + buttonHeight));

        // resize indicator
        QRectF resizeRect(rect().x() + rect().width() - buttonWidth, rect().y() + rect().height() - buttonHeight, buttonWidth, buttonHeight);
        pen.setColor(QColor(128,128,128));
        painter->setPen(pen);
        painter->drawRect(resizeRect);
        painter->drawLine(QPointF(rect().x() + rect().width(), rect().y() + rect().height() - buttonHeight), QPointF(rect().x() + rect().width() - buttonWidth, rect().y() + rect().height()));

        // text label

        // set the font
        float fontSize = 24.;

        QFont font;
        font.setPixelSize(fontSize);
        painter->setFont(font);

        // color the text black
        pen.setColor(QColor(0,0,0));
        painter->setPen(pen);

        // scale the text size down to the height of the graphics view
        // and, calculate the bounding rectangle for the text based on this scale
        // the dimensions of the view need to be corrected for the tiled display aspect ratio
        // recall the tiled display UI is only part of the graphics view since we show it at the correct aspect ratio
        float viewWidth = (float)scene()->views()[0]->width();
        float viewHeight = (float)scene()->views()[0]->height();

        float tiledDisplayAspect = (float)g_configuration->getTotalWidth() / (float)g_configuration->getTotalHeight();

        if(viewWidth / viewHeight > tiledDisplayAspect)
        {
            viewWidth = viewHeight * tiledDisplayAspect;
        }
        else if(viewWidth / viewHeight <= tiledDisplayAspect)
        {
            viewHeight = viewWidth / tiledDisplayAspect;
        }

        float verticalTextScale = 1. / viewHeight;
        float horizontalTextScale = viewHeight / viewWidth * verticalTextScale;

        painter->scale(horizontalTextScale, verticalTextScale);

        QRectF textBoundingRect = QRectF(rect().x() / horizontalTextScale, rect().y() / verticalTextScale, rect().width() / horizontalTextScale, rect().height() / verticalTextScale);

        // get the label and render it
        QString label(contentWindowManager->getContent()->getURI().c_str());
        QString labelSection = label.section("/", -1, -1).prepend(" ");
        painter->drawText(textBoundingRect, Qt::AlignLeft | Qt::AlignTop, labelSection);

        // draw window info at smaller scale
        verticalTextScale *= 0.5;
        horizontalTextScale *= 0.5;

        painter->scale(0.5, 0.5);

        textBoundingRect = QRectF(rect().x() / horizontalTextScale, rect().y() / verticalTextScale, rect().width() / horizontalTextScale, rect().height() / verticalTextScale);

        QString coordinatesLabel = QString(" (") + QString::number(x_, 'f', 2) + QString(" ,") + QString::number(y_, 'f', 2) + QString(", ") + QString::number(w_, 'f', 2) + QString(", ") + QString::number(h_, 'f', 2) + QString(")\n");
        QString zoomCenterLabel = QString(" zoom = ") + QString::number(zoom_, 'f', 2) + QString(" @ (") + QString::number(centerX_, 'f', 2) + QString(", ") + QString::number(centerY_, 'f', 2) + QString(")");

        QString windowInfoLabel = coordinatesLabel + zoomCenterLabel;
        painter->drawText(textBoundingRect, Qt::AlignLeft | Qt::AlignBottom, windowInfoLabel);

    }
}

void ContentWindowGraphicsItem::setCoordinates(double x, double y, double w, double h, ContentWindowInterface * source)
{
    ContentWindowInterface::setCoordinates(x, y, w, h, source);

    if(source != this)
    {
        setPos(x_, y_);
        setRect(mapRectFromScene(x_, y_, w_, h_));
    }
}

void ContentWindowGraphicsItem::setPosition(double x, double y, ContentWindowInterface * source)
{
    ContentWindowInterface::setPosition(x, y, source);

    if(source != this)
    {
        setPos(x_, y_);
        setRect(mapRectFromScene(x_, y_, w_, h_));
    }
}

void ContentWindowGraphicsItem::setSize(double w, double h, ContentWindowInterface * source)
{
    ContentWindowInterface::setSize(w, h, source);

    if(source != this)
    {
        setPos(x_, y_);
        setRect(mapRectFromScene(x_, y_, w_, h_));
    }
}

void ContentWindowGraphicsItem::setSelected(bool selected, ContentWindowInterface * source)
{
    ContentWindowInterface::setSelected(selected, source);

    if(source != this)
    {
        // set the pen
        QPen p = pen();

        if(selected_ == true)
        {
            p.setColor(QColor(255,0,0));
        }
        else
        {
            p.setColor(QColor(0,0,0));
        }

        setPen(p);

        // force a redraw
        update();
    }
}

void ContentWindowGraphicsItem::setZToFront()
{
    zCounter_ = zCounter_ + 1;
    setZValue(zCounter_);
}

void ContentWindowGraphicsItem::mouseMoveEvent(QGraphicsSceneMouseEvent * event)
{
    // handle mouse movements differently depending on selected mode of item
    if(selected_ == false)
    {
        if(event->buttons().testFlag(Qt::LeftButton) == true)
        {
            if(resizing_ == true)
            {
                QRectF r = rect();
                QPointF eventPos = event->pos();

                r.setBottomRight(eventPos);

                QRectF sceneRect = mapRectToScene(r);

                double w = sceneRect.width();
                double h = sceneRect.height();

                setSize(w, h);
            }
            else
            {
                QPointF delta = event->pos() - event->lastPos();

                double x = x_ + delta.x();
                double y = y_ + delta.y();

                setPosition(x, y);
            }
        }
    }
    else
    {
        // handle zooms / pans
        QPointF delta = event->scenePos() - event->lastScenePos();

        if(event->buttons().testFlag(Qt::RightButton) == true)
        {
            // increment zoom

            // if this is a touch event, use cross-product for determining change in zoom (counterclockwise rotation == zoom in, etc.)
            // otherwise, use y as the change in zoom
            double zoomDelta;

            if(event->modifiers().testFlag(Qt::AltModifier) == true)
            {
                zoomDelta = (event->scenePos().x()-0.5) * delta.y() - (event->scenePos().y()-0.5) * delta.x();
                zoomDelta *= 2.;
            }
            else
            {
                zoomDelta = delta.y();
            }

            double zoom = zoom_ * (1. - zoomDelta);

            setZoom(zoom);
        }
        else if(event->buttons().testFlag(Qt::LeftButton) == true)
        {
            // pan (move center coordinates)
            double centerX = centerX_ + 2.*delta.x() / zoom_;
            double centerY = centerY_ + 2.*delta.y() / zoom_;

            setCenter(centerX, centerY);
        }

        // force a redraw to update window info label
        update();
    }
}

void ContentWindowGraphicsItem::mousePressEvent(QGraphicsSceneMouseEvent * event)
{
    boost::shared_ptr<ContentWindowManager> contentWindowManager = getContentWindowManager();

    if(contentWindowManager != NULL)
    {
        // button dimensions
        float buttonWidth, buttonHeight;
        contentWindowManager->getButtonDimensions(buttonWidth, buttonHeight);

        // item rectangle and event position
        QRectF r = rect();
        QPointF eventPos = event->pos();

        // check to see if user clicked on the close button
        if(fabs((r.x()+r.width()) - eventPos.x()) <= buttonWidth && fabs((r.y()) - eventPos.y()) <= buttonHeight)
        {
            close();

            return;
        }

        // check to see if user clicked on the resize button
        if(fabs((r.x()+r.width()) - eventPos.x()) <= buttonWidth && fabs((r.y()+r.height()) - eventPos.y()) <= buttonHeight)
        {
            resizing_ = true;
        }
    }

    // move to the front of the GUI display
    moveToFront();

    QGraphicsItem::mousePressEvent(event);
}

void ContentWindowGraphicsItem::mouseDoubleClickEvent(QGraphicsSceneMouseEvent * event)
{
    bool selected = !selected_;

    setSelected(selected);

    QGraphicsItem::mouseDoubleClickEvent(event);
}

void ContentWindowGraphicsItem::mouseReleaseEvent(QGraphicsSceneMouseEvent * event)
{
    resizing_ = false;

    QGraphicsItem::mouseReleaseEvent(event);
}
<|endoftext|>