text:
/*
 * Copyright (c) 2019 Elastos Foundation
 *
 * 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.
 */
#include "IDChainSubWallet.h"
#include "MasterWallet.h"

#include <Common/ErrorChecker.h>
#include <Common/Log.h>
#include <WalletCore/CoinInfo.h>
#include <WalletCore/Key.h>
#include <Plugin/Transaction/Payload/DIDInfo.h>
#include <Plugin/Transaction/Program.h>
#include <Plugin/Transaction/TransactionOutput.h>
#include <Plugin/Transaction/IDTransaction.h>

#include <set>
#include <boost/algorithm/string/split.hpp>
#include <boost/algorithm/string/classification.hpp>

namespace Elastos {
	namespace ElaWallet {

		IDChainSubWallet::IDChainSubWallet(const CoinInfoPtr &info,
		                                   const ChainConfigPtr &config,
		                                   MasterWallet *parent,
		                                   const std::string &netType) :
				SidechainSubWallet(info, config, parent, netType) {

			_walletManager->GetWallet()->GenerateCID();
		}

		IDChainSubWallet::~IDChainSubWallet() {

		}

		nlohmann::json
		IDChainSubWallet::CreateIDTransaction(const nlohmann::json &payloadJson, const std::string &memo) {
			WalletPtr wallet = _walletManager->GetWallet();
			ArgInfo("{} {}", wallet->GetWalletID(), GetFunName());
			ArgInfo("payload: {}", payloadJson.dump());
			ArgInfo("memo: {}", memo);

			Address receiveAddr;
			PayloadPtr payload = nullptr;
			try {
				payload = PayloadPtr(new DIDInfo());
				payload->FromJson(payloadJson, 0);

				DIDInfo *didInfo = static_cast<DIDInfo *>(payload.get());
				ErrorChecker::CheckParam(!didInfo->IsValid(0), Error::InvalidArgument, "verify did signature failed");
				std::string id = didInfo->DIDPayload().ID();
				std::vector<std::string> idSplited;
				boost::algorithm::split(idSplited, id, boost::is_any_of(":"), boost::token_compress_on);
				ErrorChecker::CheckParam(idSplited.size() != 3, Error::InvalidArgument,
										 "invalid id format in payload JSON");
				receiveAddr = Address(idSplited[2]);
				ErrorChecker::CheckParam(!receiveAddr.Valid(), Error::InvalidArgument,
										 "invalid receive addr(id) in payload JSON");
			} catch (const nlohmann::detail::exception &e) {
				ErrorChecker::ThrowParamException(Error::JsonFormatError,
												  "Create id tx param error: " + std::string(e.what()));
			}

			std::vector<OutputPtr> outputs;
			outputs.push_back(OutputPtr(new TransactionOutput(0, receiveAddr, Asset::GetELAAssetID())));
			AddressPtr fromAddr(new Address());

			TransactionPtr tx = wallet->CreateTransaction(IDTransaction::didTransaction, payload, fromAddr, outputs, memo);

			nlohmann::json result;
			EncodeTx(result, tx);

			ArgInfo("r => {}", result.dump());

			return result;
		}

		nlohmann::json IDChainSubWallet::GetAllDID(uint32_t start, uint32_t count) const {
			ArgInfo("{} {}", _walletManager->GetWallet()->GetWalletID(), GetFunName());
			ArgInfo("start: {}", start);
			ArgInfo("count: {}", count);

			nlohmann::json j;
			AddressArray cid;
			size_t maxCount = _walletManager->GetWallet()->GetAllCID(cid, start, count);

			nlohmann::json didJson;
			for (size_t i = 0; i < cid.size(); ++i) {
				Address tmp(*cid[i]);
				tmp.ConvertToDID();
				didJson.push_back(tmp.String());
			}

			j["DID"] = didJson;
			j["MaxCount"] = maxCount;

			ArgInfo("r => {}", j.dump());
			return j;
		}

		nlohmann::json IDChainSubWallet::GetAllCID(uint32_t start, uint32_t count) const {
			ArgInfo("{} {}", _walletManager->GetWallet()->GetWalletID(), GetFunName());
			ArgInfo("start: {}", start);
			ArgInfo("count: {}", count);

			nlohmann::json j;
			AddressArray cid;
			size_t maxCount = _walletManager->GetWallet()->GetAllCID(cid, start, count);

			nlohmann::json cidJosn;
			for (AddressPtr &a : cid) {
				cidJosn.push_back(a->String());
			}

			j["CID"] = cidJosn;
			j["MaxCount"] = maxCount;

			ArgInfo("r => {}", j.dump());

			return j;
		}

		std::string IDChainSubWallet::Sign(const std::string &DIDOrCID, const std::string &message,
		                                   const std::string &payPassword) const {
			ArgInfo("{} {}", _walletManager->GetWallet()->GetWalletID(), GetFunName());
			ArgInfo("DIDOrCID: {}", DIDOrCID);
			ArgInfo("message: {}", message);
			ArgInfo("payPasswd: *");

			AddressPtr didAddress(new Address(DIDOrCID));
			std::string signature = _walletManager->GetWallet()->SignWithDID(didAddress, message, payPassword);

			ArgInfo("r => {}", signature);

			return signature;
		}

		std::string IDChainSubWallet::SignDigest(const std::string &DIDOrCID, const std::string &digest,
		                                         const std::string &payPassword) const {
			ArgInfo("{} {}", _walletManager->GetWallet()->GetWalletID(), GetFunName());
			ArgInfo("DIDOrCID: {}", DIDOrCID);
			ArgInfo("digest: {}", digest);
			ArgInfo("payPasswd: *");

			ErrorChecker::CheckParam(digest.size() != 64, Error::InvalidArgument, "invalid digest");
			AddressPtr didAddress(new Address(DIDOrCID));
			std::string signature = _walletManager->GetWallet()->SignDigestWithDID(didAddress, uint256(digest), payPassword);

			ArgInfo("r => {}", signature);

			return signature;
		}

		bool IDChainSubWallet::VerifySignature(const std::string &publicKey, const std::string &message,
											   const std::string &signature) {
			ArgInfo("{} {}", _walletManager->GetWallet()->GetWalletID(), GetFunName());
			ArgInfo("pubkey: {}", publicKey);
			ArgInfo("message: {}", message);
			ArgInfo("signature: {}", signature);

			bytes_t pubkey(publicKey), sign(signature);
			Key key(pubkey);
			bool r = key.Verify(message, sign);

			ArgInfo("r => {}", r);
			return r;
		}

		std::string IDChainSubWallet::GetPublicKeyDID(const std::string &pubkey) const {
			ArgInfo("{} {}", _walletManager->GetWallet()->GetWalletID(), GetFunName());
			ArgInfo("pubkey:{}", pubkey);

			ErrorChecker::CheckParamNotEmpty(pubkey, "public key");

			std::string did = Address(PrefixIDChain, bytes_t(pubkey), true).String();

			ArgInfo("r => {}", did);
			return did;
		}

		std::string IDChainSubWallet::GetPublicKeyCID(const std::string &pubkey) const {
			ArgInfo("{} {}", _walletManager->GetWallet()->GetWalletID(), GetFunName());
			ArgInfo("pubkey: {}", pubkey);

			ErrorChecker::CheckParamNotEmpty(pubkey, "public key");

			std::string cid = Address(PrefixIDChain, bytes_t(pubkey)).String();

			ArgInfo("r => {}", cid);
			return cid;
		}

	}
}

max_forks_repo_name: yiyaowen/ela-spvwallet-desktop


text:
#define BOOST_TEST_MODULE TestSkylineLU
#include <boost/test/unit_test.hpp>

#include <amgcl/io/mm.hpp>
#include <amgcl/adapter/crs_tuple.hpp>
#include <amgcl/profiler.hpp>
#include "sample_problem.hpp"

namespace amgcl {
    profiler<> prof;
}

BOOST_AUTO_TEST_SUITE( test_io )

BOOST_AUTO_TEST_CASE(io_mm)
{
    std::vector<ptrdiff_t> ptr, ptr2;
    std::vector<ptrdiff_t> col, col2;
    std::vector<double>    val, val2;
    std::vector<double>    rhs, rhs2;

    size_t n = sample_problem(16, val, col, ptr, rhs);

    auto A = std::tie(n, ptr, col, val);

    amgcl::io::mm_write("test_io_crs.mm", A);
    amgcl::io::mm_write("test_io_vec.mm", rhs.data(), n, 1);

    size_t rows, cols;
    std::tie(rows, cols) = amgcl::io::mm_reader("test_io_crs.mm")(ptr2, col2, val2);

    BOOST_REQUIRE_EQUAL(n, rows);
    BOOST_REQUIRE_EQUAL(n, cols);
    BOOST_REQUIRE_EQUAL(ptr.back(), ptr2.back());
    for(size_t i = 0; i < n; ++i) {
        BOOST_REQUIRE_EQUAL(ptr[i], ptr2[i]);
        for(ptrdiff_t j = ptr[i], e = ptr[i+1]; j < e; ++j) {
            BOOST_CHECK_EQUAL(col[j], col2[j]);
            BOOST_CHECK_SMALL(val[j] - val2[j], 1e-12);
        }
    }

    std::tie(rows, cols) = amgcl::io::mm_reader("test_io_vec.mm")(rhs2);
    BOOST_REQUIRE_EQUAL(n, rows);
    BOOST_REQUIRE_EQUAL(1, cols);
    for(size_t i = 0; i < n; ++i) {
        BOOST_CHECK_SMALL(rhs[i] - rhs2[i], 1e-12);
    }
}

BOOST_AUTO_TEST_SUITE_END()

ext: cpp

max_stars_repo_name: tenglongcong/amgcl

max_issues_repo_name: tenglongcong/amgcl

max_forks_repo_name: tenglongcong/amgcl


text:
/*!
 * \file smppconnection.hpp
 * \author ichramm
 */
#ifndef OPENSMPP_SMPPCONNECTION_HPP_
#define OPENSMPP_SMPPCONNECTION_HPP_
#pragma once

#include <boost/asio.hpp>
#include <boost/thread.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/thread/condition.hpp>
#include <boost/enable_shared_from_this.hpp>
#include <string>

#define RESULT_OK          0
#define RESULT_TIMEOUT    -1
#define RESULT_NETERROR   -2
#define RESULT_INVRESP    -3
#define RESULT_SYSERROR   -4

namespace opensmpp
{
	class ISMPPCommand;
	class CSMPPConnection;
	typedef boost::shared_ptr<CSMPPConnection>  SMPPConnectionPtr;

	typedef boost::asio::io_service          ioservice_t;
	typedef boost::asio::ip::tcp::socket     socket_t;
	typedef boost::asio::ip::tcp::resolver   resolver_t;

	/*!
	* \brief A user server connection
	*/
	class CSMPPConnection
		: public boost::enable_shared_from_this<CSMPPConnection>
	{
	public:

		typedef boost::function<void (
				SMPPConnectionPtr /*sender*/,
				boost::shared_ptr<ISMPPCommand>
			) > NewCommandCallback;

		typedef boost::function<void (
				SMPPConnectionPtr /*sender*/
			) > ConnectionLostCallback;

		virtual ~CSMPPConnection();

		/*! \return The connection id as specified on construction */
		unsigned int GetConnectionId() const;

		unsigned int NextSequenceNumber();

		/* \brief Sends a SMPP request and wait response */
		int SendRequest(boost::shared_ptr<ISMPPCommand> cmd);

		/* \brief Sends a SMPP response for the given command */
		int SendResponse(boost::shared_ptr<ISMPPCommand> cmd);

		virtual void Close();

		socket_t& socket();

		/* \brief Starts an async read operation */
		void ReadAsync();

	protected:

		CSMPPConnection(
					unsigned int                  connectionId,
					ioservice_t&                  ioservice,
					const NewCommandCallback&     onNewData,
					const ConnectionLostCallback& onConnectionLost
			);

		/*! \brief Invoked when an async read has complete */
		void ReadHandler(const boost::system::error_code& error);

		/*
		* \brief Creates an ISMPPCommand object based on \p commandId, with
		* sequence number \p seqNumber and filled  with the data on \p buffer
		*/
		boost::shared_ptr<ISMPPCommand> CreateCommandFromBuffer(
					unsigned int       commandId,
					unsigned int       seqNumber,
					const std::string& buffer
			);

		/* \brief Sends a SMPP packet, no locking implementation */
		int SendPDU(boost::shared_ptr<ISMPPCommand> cmd, bool response);

		/*! \brief PDU Header: Basic unit of every SMPP packet */
		struct PDUHeader
		{
			unsigned int command_length;  /*!< Length in octets of the SMPP message */
			unsigned int command_id;      /*!< Type of message the SMPP PDU represents */
			unsigned int command_status;  /*!< Indicates the success or failure of an SMPP request */
			unsigned int sequence_number; /*!< Allows a response PDU to be correlated with a request PDU */
		};

		/*! \brief Holds data about a response that the user is expecting due to a call to SendPDU */
		struct PendingResponse
		{
			boost::shared_ptr<ISMPPCommand> command;  /*!< The response packet (header+body) */
			boost::condition *condition;  /*!< Activates when the response is has come */
		};

		typedef std::map<int, PendingResponse> MapPendingResponse;

		unsigned int                   m_connectionId, m_nextSequenceNumber;
		bool                           m_connectionError, m_closeRequested;
		ioservice_t&                   m_ioservice;
		socket_t                       m_socket;
		PDUHeader                      m_pduHeader;
		MapPendingResponse             m_pendingResponses;
		boost::mutex                   m_mutexCounter;
		boost::recursive_mutex         m_mutex;
		NewCommandCallback             m_onNewDataEvent;
		ConnectionLostCallback         m_onConnectionLostEvent;
	};

	/*!
	 *
	 */
	class CSMPPClientConnection
		: public CSMPPConnection
	{
	public:

		CSMPPClientConnection(
					ioservice_t&                  ioservice,
					const NewCommandCallback&     onNewData,
					const ConnectionLostCallback& onConnectionLost
			);

		~CSMPPClientConnection();

		int Connect(const std::string server, unsigned short port);
	};

	/*!
	 *
	 */
	class CSMPPServerConnection : public CSMPPConnection
	{
	public:

		CSMPPServerConnection(
					unsigned int                  connectionId,
					ioservice_t&                  ioservice,
					const NewCommandCallback&     onNewData,
					const ConnectionLostCallback& onConnectionLost
			);

		~CSMPPServerConnection();
	};

} // namespace opensmpp

#endif // OPENSMPP_SMPPCONNECTION_HPP_

ext: hpp

max_stars_repo_name: ichramm/opensmpp

max_issues_repo_name: ichramm/opensmpp

max_forks_repo_name: ichramm/opensmpp


text:
// Copyright 2015 Tim Kouters (Falcons)
// SPDX-License-Identifier: Apache-2.0
/**
 * File: fsm.hpp
 * Author: Jan Feitsma
 * Creation: 2014-12-27 based on private FSM
 * Description: FiniteStateMachine.
 *
 */

#ifndef _INCLUDED_FSM_HPP_
#define _INCLUDED_FSM_HPP_

// required headers
#include <vector>
#include <string>
#include <map>
#include <boost/function.hpp>

// define an Event? Internal vs External?
// define base class StateData?

struct State
{
	int id;
	std::string name;
	// optional: execute when entering or leaving state
	boost::function<void(void)> entryAction; // not yet used
	boost::function<void(void)> exitAction; // not yet used
	// constructor(s)
	State(int i, std::string s);
};

struct Transition
{
	int fromState;
	int toState;
	// required: test if this Transition is to be executed
	boost::function<bool(void)> condition;
	// optional: if condition holds, execute this action (normally to update some secondary data)
	boost::function<void(void)> action; // not yet used
	// constructor(s)
	Transition(int from, int to, boost::function<bool()> cond);
};

// TODO EVENT

class FiniteStateMachine
{

    private:

	// state during FSM simulation
	int m_current_state;

	// list of states
	std::vector<State> m_states;

	// reverse lookup on the state list
	std::map<std::string, int> m_state2idx;

	// all allowed transitions, indexed at the source state
	std::vector< std::vector<Transition> > m_transitions;

    public:

	// construction / destruction
	FiniteStateMachine();
	~FiniteStateMachine();
	void reset();

	// queries
	bool hasState(std::string statename) const;
	std::string getCurrentState() const;
	std::vector<State> const & getStates() const;

	// FSM setup
	void registerState(std::string statename);
	void registerTransition(std::string from, std::string to, boost::function<bool()> transitionCondition);
	void registerEntryAction(std::string state_name, boost::function<void(void)> action);
	void registerExitAction(std::string state_name, boost::function<void(void)> action);

	// FSM iteration
	void iterate();

// auxiliary stuff
    private:
	void destroy();

}; // end FiniteStateMachine

/* EXAMPLE

#include <boost/bind.hpp>
#include <boost/ref.hpp>
#include <boost/function.hpp>
#include "fsm.hpp"
using namespace std;

int main()
{
	FiniteStateMachine testFSM;
	// some random state changes between A and B, with Final as sink
	testFSM.registerState("stateA");
	testFSM.registerState("stateB");
	testFSM.registerState("stateFinal");
	testFSM.registerTransition("stateA", "stateB", boost::bind(randBool, 0.5));
	testFSM.registerTransition("stateB", "stateA", boost::bind(randBool, 0.5));
	testFSM.registerTransition("stateB", "stateFinal", boost::bind(randBool, 0.1));
	for (int i = 0; i < 10; i++)
	{
		testFSM.iterate();
	}
	return 0;
}
*/

#endif // _INCLUDED_FSM_HPP_

max_stars_repo_name: Falcons-Robocup/code

max_issues_repo_name: Falcons-Robocup/code

max_forks_repo_name: Falcons-Robocup/code


text:
/**
 * Copyright Soramitsu Co., Ltd. All Rights Reserved.
 * SPDX-License-Identifier: Apache-2.0
 */

#ifndef KAGOME_NETWORK_IMPL_LOOPBACKSTREAM
#define KAGOME_NETWORK_IMPL_LOOPBACKSTREAM

#include <boost/asio/io_context.hpp>
#include <boost/asio/streambuf.hpp>
#include <libp2p/connection/stream.hpp>

#include "common/buffer.hpp"
#include "log/logger.hpp"
#include "outcome/outcome.hpp"

namespace kagome::network {

  class LoopbackStream : public libp2p::connection::Stream,
                         public std::enable_shared_from_this<LoopbackStream> {
   public:
    enum class Error {
      INVALID_ARGUMENT = 1,
      IS_CLOSED_FOR_READS,
      IS_CLOSED_FOR_WRITES,
      IS_RESET,
      INTERNAL_ERROR
    };

    explicit LoopbackStream(libp2p::peer::PeerInfo own_peer_info);

    bool isClosedForRead() const override;
    bool isClosedForWrite() const override;
    bool isClosed() const override;

    void close(VoidResultHandlerFunc cb) override;
    void reset() override;

    void adjustWindowSize(uint32_t new_size, VoidResultHandlerFunc cb) override;

    outcome::result<bool> isInitiator() const override;

    outcome::result<libp2p::peer::PeerId> remotePeerId() const override;

    outcome::result<libp2p::multi::Multiaddress> localMultiaddr()
        const override;

    outcome::result<libp2p::multi::Multiaddress> remoteMultiaddr()
        const override;

   protected:
    void read(gsl::span<uint8_t> out,
              size_t bytes,
              ReadCallbackFunc cb) override;

    void readSome(gsl::span<uint8_t> out,
                  size_t bytes,
                  ReadCallbackFunc cb) override;

    void write(gsl::span<const uint8_t> in,
               size_t bytes,
               WriteCallbackFunc cb) override;

    void writeSome(gsl::span<const uint8_t> in,
                   size_t bytes,
                   WriteCallbackFunc cb) override;

   private:
    void read(gsl::span<uint8_t> out,
              size_t bytes,
              ReadCallbackFunc cb,
              bool some);

    libp2p::peer::PeerInfo own_peer_info_;

    log::Logger log_ = log::createLogger("LoopbackStream", "network");

    /// data, received for this stream, comes here
    boost::asio::streambuf buffer_;

    /// when a new data arrives, this function is to be called
    std::function<void(outcome::result<size_t>)> data_notifyee_;
    bool data_notified_ = false;

    /// is the stream opened for reads?
    bool is_readable_ = true;

    /// is the stream opened for writes?
    bool is_writable_ = true;

    /// was the stream reset?
    bool is_reset_ = false;
  };

}  // namespace kagome::network

OUTCOME_HPP_DECLARE_ERROR(kagome::network, LoopbackStream::Error);

#endif  // KAGOME_NETWORK_IMPL_LOOPBACKSTREAM

max_stars_repo_name: igor-egorov/kagome

max_issues_repo_name: igor-egorov/kagome

max_forks_repo_name: igor-egorov/kagome


text:
// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2012 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2012 Mateusz Loskot, London, UK.

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//
// Polygon Example

#include <algorithm> // for reverse, unique
#include <iostream>
#include <string>

#include <boost/geometry/geometry.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include <boost/geometry/geometries/adapted/c_array.hpp>
#include <boost/geometry/multi/geometries/multi_polygon.hpp>

BOOST_GEOMETRY_REGISTER_C_ARRAY_CS(cs::cartesian)

std::string boolstr(bool v)
{
    return v ? "true" : "false";
}

int main(void)
{
    using namespace boost::geometry;

    typedef model::d2::point_xy<double> point_2d;
    typedef model::polygon<point_2d> polygon_2d;
    typedef model::box<point_2d> box_2d;

    // Define a polygon and fill the outer ring.
    // In most cases you will read it from a file or database
    polygon_2d poly;
    {
        const double coor[][2] = {
            {2.0, 1.3}, {2.4, 1.7}, {2.8, 1.8}, {3.4, 1.2}, {3.7, 1.6},
            {3.4, 2.0}, {4.1, 3.0}, {5.3, 2.6}, {5.4, 1.2}, {4.9, 0.8}, {2.9, 0.7},
            {2.0, 1.3} // closing point is opening point
            };
        assign_points(poly, coor);
    }

    // Polygons should be closed, and directed clockwise. If you're not sure if that is the case,
    // call the correct algorithm
    correct(poly);

    // Polygons can be streamed as text
    // (or more precisely: as DSV (delimiter separated values))
    std::cout << dsv(poly) << std::endl;

    // As with lines, bounding box of polygons can be calculated
    box_2d b;
    envelope(poly, b);
    std::cout << dsv(b) << std::endl;

    // The area of the polygon can be calulated
    std::cout << "area: " << area(poly) << std::endl;

    // And the centroid, which is the center of gravity
    point_2d cent;
    centroid(poly, cent);
    std::cout << "centroid: " << dsv(cent) << std::endl;

    // The number of points can be requested per ring (using .size())
    // or per polygon (using num_points)
    std::cout << "number of points in outer ring: " << poly.outer().size() << std::endl;

    // Polygons can have one or more inner rings, also called holes, islands, interior rings.
    // Let's add one
    {
        poly.inners().resize(1);
        model::ring<point_2d>& inner = poly.inners().back();

        const double coor[][2] = { {4.0, 2.0}, {4.2, 1.4}, {4.8, 1.9}, {4.4, 2.2}, {4.0, 2.0} };
        assign_points(inner, coor);
    }

    correct(poly);

    std::cout << "with inner ring:" << dsv(poly) << std::endl;
    // The area of the polygon is changed of course
    std::cout << "new area of polygon: " << area(poly) << std::endl;
    centroid(poly, cent);
    std::cout << "new centroid: " << dsv(cent) << std::endl;

    // You can test whether points are within a polygon
    std::cout << "point in polygon:"
        << " p1: "  << boolstr(within(make<point_2d>(3.0, 2.0), poly))
        << " p2: "  << boolstr(within(make<point_2d>(3.7, 2.0), poly))
        << " p3: "  << boolstr(within(make<point_2d>(4.4, 2.0), poly))
        << std::endl;

    // As with linestrings and points, you can derive from polygon to add, for example,
    // fill color and stroke color. Or SRID (spatial reference ID). Or Z-value. Or a property map.
    // We don't show this here.

    // Clip the polygon using a box
    box_2d cb(make<point_2d>(1.5, 1.5), make<point_2d>(4.5, 2.5));
    typedef std::vector<polygon_2d> polygon_list;
    polygon_list v;

    intersection(cb, poly, v);
    std::cout << "Clipped output polygons" << std::endl;
    for (polygon_list::const_iterator it = v.begin(); it != v.end(); ++it)
    {
        std::cout << dsv(*it) << std::endl;
    }

    typedef model::multi_polygon<polygon_2d> polygon_set;
    polygon_set ps;
    union_(cb, poly, ps);

    polygon_2d hull;
    convex_hull(poly, hull);
    std::cout << "Convex hull:" << dsv(hull) << std::endl;

    // If you really want:
    //   You don't have to use a vector, you can define a polygon with a deque
    //   You can specify the container for the points and for the inner rings independantly

    typedef model::polygon<point_2d, true, true, std::deque, std::deque> deque_polygon;
    deque_polygon poly2;
    ring_type<deque_polygon>::type& ring = exterior_ring(poly2);
    append(ring, make<point_2d>(2.8, 1.9));
    append(ring, make<point_2d>(2.9, 2.4));
    append(ring, make<point_2d>(3.3, 2.2));
    append(ring, make<point_2d>(3.2, 1.8));
    append(ring, make<point_2d>(2.8, 1.9));
    std::cout << dsv(poly2) << std::endl;

    return 0;
}

max_stars_repo_name: randolphwong/mcsema

max_issues_repo_name: randolphwong/mcsema

max_forks_repo_name: randolphwong/mcsema


text:
#pragma once
#ifndef _MATHUTIL_H
#define _MATHUTIL_H

#include <assert.h>

#include <random>
#include <limits>
#include <memory>
#include <iostream>
#include <iomanip>

#include <Eigen/src/Core/util/DisableStupidWarnings.h>
#include <Eigen/Core>
#include <Eigen/Geometry>

#include <opencv2/core/core.hpp>
#include <opencv2/core/eigen.hpp>
#include <opencv2/calib3d.hpp>

namespace mut
{
   const double PI = 3.14159265358979323846;
   const float PI_F = 3.14159265358979f;
   const double NaN = std::numeric_limits<double>::quiet_NaN();
   const double NaN_F = std::numeric_limits<float>::quiet_NaN();

   static Eigen::Quaterniond rotate_between(const Eigen::Vector3d &from, const Eigen::Vector3d &to,
                                            const Eigen::Vector3d &fallbackAxis = Eigen::Vector3d(0, 0, 0))
   //-----------------------------------------------------------------------------------------------
   {
      Eigen::Quaterniond q;
      Eigen::Vector3d v0 = from;
      Eigen::Vector3d v1 = to;
      v0.normalize();
      v1.normalize();

      double d = v0.dot(v1);
      if (d >= 1.0f)
         return Eigen::Quaterniond(1, 0, 0, 0);

      if (d < (1e-6f - 1.0f))
      {
         if (fallbackAxis != Eigen::Vector3d(0, 0, 0))
            q = Eigen::AngleAxis<double>(PI, fallbackAxis);
         else
         {
            // Generate an axis
            Eigen::Vector3d axis = Eigen::Vector3d(1, 0, 0).cross(from);
            if (axis.norm() < 0.000000001) // pick another if colinear
               axis = Eigen::Vector3d(0, 1, 0).cross(from);
            axis.normalize();
            q = Eigen::AngleAxis<double>(PI, axis);
         }
      }
      else
      {
         double s = sqrt((1 + d) * 2);
         double invs = 1 / s;

         Eigen::Vector3d c = v0.cross(v1);

         q.x() = c.x() * invs;
         q.y() = c.y() * invs;
         q.z() = c.z() * invs;
         q.w() = s * 0.5f;
         q.normalize();
      }
      return q;
   }

   template<typename T> static inline bool near_zero(T v, T epsilon);

   template<> bool near_zero(long double v, long double epsilon) { return (fabsl(v) <= epsilon); }

   template<> bool near_zero(double v, double epsilon) { return (fabs(v) <= epsilon); }

   template<> bool near_zero(float v, float epsilon) { return (fabsf(v) <= epsilon); }

   inline static double radiansToDegrees(double radians) { return (radians * 180.0 / PI); }

   inline static float radiansToDegrees(float radians) { return (radians * 180.0f / PI_F); }

   inline static double degreesToRadians(double degrees) { return (degrees * PI / 180.0); }

   inline static float degreesToRadians(float degrees) { return (degrees * PI_F / 180.0f); }

   template<typename T>
   inline static void normalize(T &v)
//-----------------------------------------------
   {
      auto magnitude = v.dot(v);
      if (magnitude > 1.0e-8)
         v /= sqrt(magnitude);
   }

   template<typename T>
   inline static void unchecked_normalize(T &v)
   { v /= sqrt(v.dot(v)); }

   inline static Eigen::Matrix3d skew33d(Eigen::Vector3d v3)
  //-----------------------------------------------
   {
      double a = v3[0], b = v3[1], c = v3[2];
      Eigen::Matrix3d X;
      X << 0, -c, b, c, 0, -a, -b, a, 0;
      return X;
   }

   inline static Eigen::Vector3d solve_homogeneous(Eigen::Matrix3d A)
//---------------------------------------------------------------------------------------------------------------------------
   {
//   std::cout << "Ax = 0:" << std::endl << A << std::endl;
      Eigen::JacobiSVD<Eigen::MatrixXd, Eigen::FullPivHouseholderQRPreconditioner>
            svd(A, Eigen::ComputeFullU | Eigen::ComputeFullV);
      auto U = svd.matrixU();
      auto V = svd.matrixV();
//   std::cout << "U: " << U << std::endl << "S: " << svd.singularValues() << std::endl << "V: " << V << std::endl;
      return V.col(V.cols() - 1);
   }

   inline static Eigen::Vector3d ray(const Eigen::Matrix3d &KI, double x, double y, double z = 1,
                                                bool is_homogenouous = false, bool is_normalize = false)
//----------------------------------------------------------------------------------
   {
      Eigen::Vector3d v(x, y, z);
      Eigen::Vector3d ray = KI * v;
      if (is_homogenouous)
         ray /= ray[2];
      if (is_normalize)
         ray.normalize();
      return ray;
   }

   inline static Eigen::Vector3d ray(const Eigen::Matrix3d &KI, Eigen::Vector3d v, bool is_homogenouous = false,
                                                bool is_normalize = false)
//--------------------------------------------------------------------------------------------------
   {
      Eigen::Vector3d ray = KI * v;
      if (is_homogenouous)
         ray /= ray[2];
      if (is_normalize)
         ray.normalize();
      //Eigen::Vector3d p  = KI.inverse()*Eigen::Vector3d(1128,592);
      return ray;
   }

   inline static Eigen::Vector3d ray(const Eigen::Matrix3d &KI, cv::Point3d pt, bool is_homogenouous = false,
                                                bool is_normalize = false)
   //-----------------------------------------------------------------------------------------------------------------------
   {
      Eigen::Vector3d v(pt.x, pt.y, pt.z);
      Eigen::Vector3d ray = KI * v;
      if (is_homogenouous)
         ray /= ray[2];
      if (is_normalize)
         ray.normalize();
      return ray;
   }

   inline static void yawpitchroll(const Eigen::Vector3d train, const Eigen::Vector3d query,
                                   double& roll_degrees, double& pitch_degrees, double &yaw_degrees)
   //----------------------------------------------------------------------------------------
   {
      Eigen::Quaterniond Q = rotate_between(query, train);
      Eigen::Matrix3d R = Q.toRotationMatrix();
      Eigen::Vector3d euler = R.eulerAngles(0, 1, 2);
      roll_degrees = radiansToDegrees(euler[0]);
      pitch_degrees = radiansToDegrees(euler[1]);
      yaw_degrees = radiansToDegrees(euler[2]);

      std::cout << std::fixed << std::setprecision(8) << train.transpose() << " -> "  << query.transpose()
                << ": Roll " << mut::radiansToDegrees(euler[0]) << " Pitch " << mut::radiansToDegrees(euler[1])
                << " Yaw " << mut::radiansToDegrees(euler[2]) << std::endl << R << std::endl;

      Q.setFromTwoVectors(query, train);
      R = Q.toRotationMatrix();
      euler = R.eulerAngles(0, 1, 2);
      std::cout << std::fixed << std::setprecision(8) << train.transpose() << " -> "  << query.transpose()
                << ": Roll " << mut::radiansToDegrees(euler[0]) << " Pitch " << mut::radiansToDegrees(euler[1])
                << " Yaw " << mut::radiansToDegrees(euler[2]) << std::endl << R << std::endl;
   }

   static Eigen::Vector3d quaternion_to_euler_angle(Eigen::Quaterniond Q)
   //--------------------------------------------------------------------
   {
      double w = Q.w();
      double x = Q.x();
      double y = Q.y();
      double z = Q.z();

      double ysqr = y * y;

      double t0 = +2.0 * (w * x + y * z);
      double t1 = +1.0 - 2.0 * (x * x + ysqr);
      double X = atan2(t0, t1);

      double t2 = +2.0 * (w * y - z * x);
      t2 = (t2 > +1.0) ? +1.0 : t2;
      t2 = (t2 < -1.0) ? -1.0 : t2;
      double Y = asin(t2);

      double t3 = +2.0 * (w * z + x * y);
      double t4 = +1.0 - 2.0 * (ysqr + z * z);
      double Z = atan2(t3, t4);

      return Eigen::Vector3d(X, Y, Z);
   }

   static cv::Vec3f rotation2Euler(cv::Mat &R)
   //-----------------------------------------
   {
      Eigen::Matrix3d ER;
      cv::cv2eigen(R, ER);
      Eigen::Vector3d euler = ER.eulerAngles(0, 1, 2);
      return cv::Vec3f(euler[0], euler[1], euler[2]);
   }

   static Eigen::Quaterniond euler2Quaternion(const double roll, const double pitch, const double yaw )
   //-------------------------------------------------------------------------------------------
   {
      Eigen::AngleAxisd rollAngle(roll, Eigen::Vector3d::UnitX());
      Eigen::AngleAxisd pitchAngle(pitch, Eigen::Vector3d::UnitY());
      Eigen::AngleAxisd yawAngle(yaw, Eigen::Vector3d::UnitZ());
      Eigen::Quaterniond q = yawAngle * pitchAngle * rollAngle;
      return q;
   }

   static cv::Vec3d ocvMultMatVec(const cv::Mat &P, double x, double y, double z)
   //---------------------------------------------------------------------------
   {
      cv::Mat V(1, 3, CV_64FC1);
      double *pV = V.ptr<double>(0);
      pV[0] = x;
      pV[1] = y;
      pV[2] = z;
      cv::Mat R = P * V.t();
      R = R.t();
      pV = R.ptr<double>(0);
      return cv::Vec3d(pV[0], pV[1], pV[2]);
   }

   static cv::Vec3d ocvMultMatVec(const cv::Mat &P, cv::Vec3d v)
   //------------------------------------------------------------
   {
      cv::Mat V(1, 3, CV_64FC1);
      double *pV = V.ptr<double>(0);
      pV[0] = v[0];
      pV[1] = v[1];
      pV[2] = v[2];
      cv::Mat R = P * V.t();
      R = R.t();
      pV = R.ptr<double>(0);
      return cv::Vec3d(pV[0], pV[1], pV[2]);
   }

   static inline cv::Vec3d toOcvRotationVec(Eigen::Quaterniond Q)
   //-----------------------------------------------------
   {
      Eigen::AngleAxisd aa(Q);
      Eigen::Vector3d v = aa.angle()*aa.axis();
/*
      auto R = Q.toRotationMatrix();
      cv::Mat OR;
      cv::eigen2cv(R, OR);
      cv::Vec3d vv;
      cv::Rodrigues(OR, vv);
      std::cout << v.transpose() << " " << vv.t() << std::endl;
*/
      return cv::Vec3d(v[0], v[1], v[2]);
   }

   static inline Eigen::Quaterniond toQuaternion(cv::InputArray m)
   //-----------------------------------------------------
   {
      cv::Mat R;
      cv::Rodrigues(m, R);
      Eigen::Matrix3d ER;
      cv::cv2eigen(R, ER);
      return Eigen::Quaterniond(ER);
   }

   static void decode_quaternion(const Eigen::Quaterniond qin, Eigen::Vector3d& axis, double& angle)
   //-------------------------------------------------------------------------------------
   {
      Eigen::Quaterniond Q = qin;
      if (Q.w() > 1)
         Q.normalize(); // if w>1 acos and sqrt will produce errors, this cant happen if quaternion is normalised
      angle = 2 * acos(Q.w());
      double s = sqrt(1 - Q.w()*Q.w()); // assuming quaternion normalised then w is less than 1, so term always positive.
      if (s >= 0.001)
      { // test to avoid divide by zero, s is always positive due to sqrt
         // if s close to zero then direction of axis not important
         axis[0] = Q.x(); // if it is important that axis is normalised then replace with x=1; y=z=0;
         axis[1] = Q.y();
         axis[2] = Q.z();
      }
      else
      {
         axis[0] = Q.x() / s;
         axis[1] = Q.y() / s;
         axis[2] = Q.z() / s;
         axis.normalize();
      }
   }

   static void essential_decompose(const Eigen::Matrix3d& essential_matrix, Eigen::Matrix3d* rotation1,
                            Eigen::Matrix3d* rotation2, Eigen::Vector3d* translation)
   //------------------------------------------------------------------------------------------
   {
      Eigen::Matrix3d d;
      d << 0, 1, 0, -1, 0, 0, 0, 0, 1;

      const Eigen::JacobiSVD<Eigen::Matrix3d> svd(
            essential_matrix, Eigen::ComputeFullU | Eigen::ComputeFullV);
      Eigen::Matrix3d U = svd.matrixU();
      Eigen::Matrix3d V = svd.matrixV();
      if (U.determinant() < 0) {
         U.col(2) *= -1.0;
      }

      if (V.determinant() < 0) {
         V.col(2) *= -1.0;
      }

      // Possible configurations.
      *rotation1 = U * d * V.transpose();
      *rotation2 = U * d.transpose() * V.transpose();
      *translation = U.col(2).normalized();
   }

   inline double essential_check(const Eigen::Matrix3d R, const double tx, const double ty, const double tz,
                                 const double xt, const double yt, const double xq,  const double yq)
   {
      return  -R(0, 0)*ty*xt - R(0, 1)*ty*yt - R(0, 2)*ty + R(1, 0)*tx*xt + R(1, 1)*tx*yt +
               R(1, 2)*tx + xq*(-R(1, 0)*tz*xt - R(1, 1)*tz*yt - R(1, 2)*tz + R(2, 0)*ty*xt +
               R(2, 1)*ty*yt + R(2, 2)*ty) - yq*(-R(0, 0)*tz*xt - R(0, 1)*tz*yt - R(0, 2)*tz +
               R(2, 0)*tx*xt + R(2, 1)*tx*yt + R(2, 2)*tx);
   }

   static bool check_essential(Eigen::Matrix3d R, Eigen::Vector3d T, double epsilon =0.0000001)
   //------------------------------------------------------------------------------------
   {
      Eigen::Matrix3d E = skew33d(T)*R;
//      std::cout << R << std::endl << E << std::endl;
      if (! near_zero(E.determinant(), epsilon))
         return false;
      Eigen::Matrix3d EET = E*E.transpose();
      Eigen::Matrix3d constraint = 2*EET*E - EET.trace()*E;
//      std::cout << constraint << std::endl;
      struct ZeroVisit
      {
         double total = 0;
         void init(const double& value, Eigen::Index i, Eigen::Index j) {}
         void operator() (const double& value, Eigen::Index i, Eigen::Index j) { total += value; }
      } visitor;
      constraint.visit(visitor);
      if (! near_zero(visitor.total, epsilon))
         return false;

      Eigen::JacobiSVD<Eigen::MatrixXd, Eigen::FullPivHouseholderQRPreconditioner>
            svd(E, Eigen::ComputeFullU | Eigen::ComputeFullV);
      auto U = svd.matrixU();
      auto V = svd.matrixV();
      auto S = svd.singularValues().asDiagonal().toDenseMatrix();
//      std::cout << "U: " << U << std::endl << "S: " << S << std::endl << "V: " << V << std::endl;
      if (!near_zero(S(2, 2), epsilon))
         return false;
   //   if ( (!near_zero(S(0, 0) - 1, 0.0000001)) || (!near_zero(S(1, 1) - 1, 0.0000001)) )
   //   {
   //      S(0, 0) = S(1, 1) = 1.0;
   //      S(2, 2) = 0.0;
   //      auto A = U*S*V.transpose();
   //      std::cout << A << std::endl;
   //   }
      return true;
   }

   // or just R = Eigen::Quaterniond::UnitRandom().toRotationMatrix();
   static void randomRotationMatrix(std::mt19937* generator, std::uniform_real_distribution<double>* distribution,
                                    Eigen::Matrix3d& R)
//-------------------------------------------------------------------------------------------------------------
   {
      double x = (*distribution)(*generator);
      double y = (*distribution)(*generator);
      double z = (*distribution)(*generator);
      double theta = std::acos(2*x - 1);
      double phi = 2*mut::PI*y;

      double r  = sqrt(z);
      double Vx = sin(phi) * r;
      double Vy = cos(phi) * r;
      double Vz = sqrt(2.f - z);

      double st = sin(theta);
      double ct = cos(theta);
      double Sx = Vx * ct - Vy * st;
      double Sy = Vx * st + Vy * ct;

      R(0,0) = Vx * Sx - ct;
      R(0,1) = Vx * Sy - st;
      R(0,2) = Vx * Vz;
      R(1,0) = Vy * Sx + st;
      R(1,1) = Vy * Sy - ct;
      R(1,2) = Vy * Vz;
      R(2,0) = Vz * Sx;
      R(2,1) = Vz * Sy;
      R(2,2) = 1.0 - z;

      assert(mut::near_zero(R.determinant() - 1.0, 0.000001));
   }

   static void random_vector3d_fixed_norm_kludge(const double norm_sqrt, Eigen::Vector3d& v)
   //-------------------------------------------------------------------------------
   {
      std::random_device rand_device;
      std::mt19937 rand_generator;
      std::unique_ptr<std::uniform_real_distribution<double>> distribution;
      std::uniform_int_distribution<int> index_distr(0,2);
      std::vector<double> pot;

      distribution.reset(new std::uniform_real_distribution<double>(0, norm_sqrt));
      double one = ((*distribution)(rand_generator));
      distribution.reset(new std::uniform_real_distribution<double>(0, std::max(norm_sqrt - one, 0.0)));
      double two = ((*distribution)(rand_generator));
      double three = std::max(norm_sqrt - one - two, 0.0);
      pot.clear();
      pot.push_back(one); pot.push_back(two); pot.push_back(three);
      int i1 = index_distr(rand_generator), i2 = index_distr(rand_generator), i3;
      while (i2 == i1) i2 = index_distr(rand_generator);
      switch (i1)
      {
         case 0: if (i2 == 1) i3 = 2; else i3 = 1; break;
         case 1: if (i2 == 0) i3 = 2; else i3 = 0; break;
         case 2: if (i2 == 0) i3 = 1; else i3 = 0; break;
      }
      v[0] = pot[i1]; v[1] = pot[i2]; v[2] = pot[i3];
   }

/*
int essential_best_pose(const Eigen::Matrix3d& essential_matrix,
                        const std::vector<FeatureCorrespondence>& normalized_correspondences,
                        Eigen::Matrix3d* rotation, Eigen::Vector3d* position)
{
   Eigen::Matrix3d rotation1, rotation2;
   Eigen::Vector3d translation;
   essential_decompose(essential_matrix, &rotation1, &rotation2, &translation);
   const std::vector<Eigen::Matrix3d> rotations = {rotation1, rotation1, rotation2, rotation2};
   const std::vector<Eigen::Vector3d> positions = {
         -rotations[0].transpose() * translation,
         -rotations[1].transpose() * -translation,
         -rotations[2].transpose() * translation,
         -rotations[3].transpose() * -translation};

   // From the 4 candidate poses, find the one with the most triangulated points
   // in front of the camera.
   std::vector<int> points_in_front_of_cameras(4, 0);
   for (int i = 0; i < 4; i++) {
      for (const auto& correspondence : normalized_correspondences) {
         if (IsTriangulatedPointInFrontOfCameras(
               correspondence, rotations[i], positions[i])) {
            ++points_in_front_of_cameras[i];
         }
      }
   }

   // Find the pose with the most points in front of the camera.
   const auto& max_element = std::max_element(points_in_front_of_cameras.begin(),
                                              points_in_front_of_cameras.end());
   const int max_index =
         std::distance(points_in_front_of_cameras.begin(), max_element);

   // Set the pose.
   *rotation = rotations[max_index];
   *position = positions[max_index];
   return *max_element;
}
*/
}
#endif

ext: hh

max_stars_repo_name: donaldmunro/PlanarTrainer

max_issues_repo_name: donaldmunro/PlanarTrainer

max_forks_repo_name: donaldmunro/PlanarTrainer


text:
/*
 * Copyright (c) 2016, Vanderbilt University
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 * Author: Addisu Z. Taddese
 */

#include <boost/bind.hpp>
#include <boost/thread.hpp>
#include <boost/thread/mutex.hpp>

#include <ros/callback_queue.h>
#include <ros/advertise_options.h>
#include <ros/ros.h>

#include <iostream>
#include <vector>
#include <cstdint>

#include "storm_gazebo_ros_magnet/dipole_magnet.h"

namespace gazebo {

DipoleMagnet::DipoleMagnet(): ModelPlugin() {
  this->connect_count = 0;
}

DipoleMagnet::~DipoleMagnet() {
  event::Events::DisconnectWorldUpdateBegin(this->update_connection);
  if (this->should_publish) {
    this->queue.clear();
    this->queue.disable();
    this->rosnode->shutdown();
    this->callback_queue_thread.join();
    delete this->rosnode;
  }
  if (this->mag){
    DipoleMagnetContainer::Get().Remove(this->mag);
  }
}

void DipoleMagnet::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf) {
  // Store the pointer to the model
  this->model = _parent;
  this->world = _parent->GetWorld();
  gzdbg << "Loading DipoleMagnet plugin" << std::endl;

  this->mag = std::make_shared<DipoleMagnetContainer::Magnet>();

  // load parameters
  this->robot_namespace = "";
  if (_sdf->HasElement("robotNamespace"))
    this->robot_namespace = _sdf->GetElement("robotNamespace")->Get<std::string>() + "/";

  if(!_sdf->HasElement("bodyName")) {
    gzerr << "DipoleMagnet plugin missing <bodyName>, cannot proceed" << std::endl;
    return;
  }else {
    this->link_name = _sdf->GetElement("bodyName")->Get<std::string>();
  }

  this->link = this->model->GetLink(this->link_name);
  if(!this->link){
    gzerr << "Error: link named " << this->link_name << " does not exist" << std::endl;
    return;
  }

  this->should_publish = false;
  if (_sdf->HasElement("shouldPublish"))
  {
    this->should_publish = _sdf->GetElement("shouldPublish")->Get<bool>();
  }

  if (!_sdf->HasElement("updateRate"))
  {
    gzmsg << "DipoleMagnet plugin missing <updateRate>, defaults to 0.0"
        " (as fast as possible)" << std::endl;
    this->update_rate = 0;
  }
  else
    this->update_rate = _sdf->GetElement("updateRate")->Get<double>();

  if (_sdf->HasElement("calculate")){
    this->mag->calculate = _sdf->Get<bool>("calculate");
  } else
    this->mag->calculate = true;

  if (_sdf->HasElement("dipole_moment")){
    this->mag->moment = _sdf->Get<math::Vector3>("dipole_moment");
  }

  if (_sdf->HasElement("xyzOffset")){
    this->mag->offset.pos = _sdf->Get<math::Vector3>("xyzOffset");
  }

  if (_sdf->HasElement("rpyOffset")){
    math::Vector3 rpy_offset = _sdf->Get<math::Vector3>("rpyOffset");
    this->mag->offset.rot = math::Quaternion(rpy_offset);
  }

  if (this->should_publish) {
    if (!_sdf->HasElement("topicNs"))
    {
      gzmsg << "DipoleMagnet plugin missing <topicNs>,"
          "will publish on namespace " << this->link_name << std::endl;
    }
    else {
      this->topic_ns = _sdf->GetElement("topicNs")->Get<std::string>();
    }

    if (!ros::isInitialized())
    {
      gzerr << "A ROS node for Gazebo has not been initialized, unable to load "
        "plugin. Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in "
        "the gazebo_ros package. If you want to use this plugin without ROS, "
        "set <shouldPublish> to false" << std::endl;
      return;
    }

    this->rosnode = new ros::NodeHandle(this->robot_namespace);
    this->rosnode->setCallbackQueue(&this->queue);

    this->wrench_pub = this->rosnode->advertise<geometry_msgs::WrenchStamped>(
        this->topic_ns + "/wrench", 1,
        boost::bind( &DipoleMagnet::Connect,this),
        boost::bind( &DipoleMagnet::Disconnect,this), ros::VoidPtr(), &this->queue);
    this->mfs_pub = this->rosnode->advertise<sensor_msgs::MagneticField>(
        this->topic_ns + "/mfs", 1,
        boost::bind( &DipoleMagnet::Connect,this),
        boost::bind( &DipoleMagnet::Disconnect,this), ros::VoidPtr(), &this->queue);

    // Custom Callback Queue
    this->callback_queue_thread = boost::thread( boost::bind( &DipoleMagnet::QueueThread,this ) );
  }

  this->mag->model_id = this->model->GetId();

  gzmsg << "Loaded Gazebo dipole magnet plugin on " << this->model->GetName() << std::endl;

  DipoleMagnetContainer::Get().Add(this->mag);

  // Listen to the update event. This event is broadcast every
  // simulation iteration.
  this->update_connection = event::Events::ConnectWorldUpdateBegin(
      boost::bind(&DipoleMagnet::OnUpdate, this, _1));
}

void DipoleMagnet::Connect() {
  this->connect_count++;
}

void DipoleMagnet::Disconnect() {
  this->connect_count--;
}

void DipoleMagnet::QueueThread() {
  static const double timeout = 0.01;

  while (this->rosnode->ok())
  {
    this->queue.callAvailable(ros::WallDuration(timeout));
  }
}

// Called by the world update start event
void DipoleMagnet::OnUpdate(const common::UpdateInfo & /*_info*/) {

  // Calculate the force from all other magnets
  math::Pose p_self = this->link->GetWorldPose();
  p_self.pos += -p_self.rot.RotateVector(this->mag->offset.pos);
  p_self.rot *= this->mag->offset.rot.GetInverse();

  this->mag->pose = p_self;

  if (!this->mag->calculate)
    return;

  DipoleMagnetContainer& dp = DipoleMagnetContainer::Get();

  math::Vector3 moment_world = p_self.rot.RotateVector(this->mag->moment);

  math::Vector3 force(0, 0, 0);
  math::Vector3 torque(0, 0, 0);
  math::Vector3 mfs(0, 0, 0);
  for(DipoleMagnetContainer::MagnetPtrV::iterator it = dp.magnets.begin(); it < dp.magnets.end(); it++){
    std::shared_ptr<DipoleMagnetContainer::Magnet> mag_other = *it;
    if (mag_other->model_id != this->mag->model_id) {
      math::Pose p_other = mag_other->pose;
      math::Vector3 m_other = p_other.rot.RotateVector(mag_other->moment);

      math::Vector3 force_tmp;
      math::Vector3 torque_tmp;
      GetForceTorque(p_self, moment_world, p_other, m_other, force_tmp, torque_tmp);

      force += force_tmp;
      torque += torque_tmp;

      math::Vector3 mfs_tmp;
      GetMFS(p_self, p_other, m_other, mfs_tmp);

      mfs += mfs_tmp;

      this->link->AddForce(force_tmp);
      this->link->AddTorque(torque_tmp);
    }
  }

  this->PublishData(force, torque, mfs);
}

void DipoleMagnet::PublishData(
    const math::Vector3& force,
    const math::Vector3& torque,
    const math::Vector3& mfs){
  if(this->should_publish && this->connect_count > 0) {
    // Rate control
    common::Time cur_time = this->world->GetSimTime();
    if (this->update_rate > 0 &&
        (cur_time-this->last_time).Double() < (1.0/this->update_rate))
      return;

    this->lock.lock();
    // copy data into wrench message
    this->wrench_msg.header.frame_id = "world";
    this->wrench_msg.header.stamp.sec = cur_time.sec;
    this->wrench_msg.header.stamp.nsec = cur_time.nsec;

    this->wrench_msg.wrench.force.x    = force.x;
    this->wrench_msg.wrench.force.y    = force.y;
    this->wrench_msg.wrench.force.z    = force.z;
    this->wrench_msg.wrench.torque.x   = torque.x;
    this->wrench_msg.wrench.torque.y   = torque.y;
    this->wrench_msg.wrench.torque.z   = torque.z;

    // now mfs
    this->mfs_msg.header.frame_id = this->link_name;
    this->mfs_msg.header.stamp.sec = cur_time.sec;
    this->mfs_msg.header.stamp.nsec = cur_time.nsec;

    this->mfs_msg.magnetic_field.x = mfs.x;
    this->mfs_msg.magnetic_field.y = mfs.y;
    this->mfs_msg.magnetic_field.z = mfs.z;

    this->wrench_pub.publish(this->wrench_msg);
    this->mfs_pub.publish(this->mfs_msg);

    this->lock.unlock();
  }
}

void DipoleMagnet::GetForceTorque(const math::Pose& p_self,
    const math::Vector3& m_self,
    const math::Pose& p_other,
    const math::Vector3& m_other,
    math::Vector3& force,
    math::Vector3& torque) {

  bool debug = false;
  math::Vector3 p = p_self.pos - p_other.pos;
  math::Vector3 p_unit = p/p.GetLength();

  math::Vector3 m1 = m_other;
  math::Vector3 m2 = m_self;
  if (debug)
    std::cout << "p: " << p << " m1: " << m1 << " m2: " << m2 << std::endl;

  double K = 3.0*1e-7/pow(p.GetLength(), 4);
  force = K * (m2 * (m1.Dot(p_unit)) +  m1 * (m2.Dot(p_unit)) +
      p_unit*(m1.Dot(m2)) - 5*p_unit*(m1.Dot(p_unit))*(m2.Dot(p_unit)));

  double Ktorque = 1e-7/pow(p.GetLength(), 3);
  math::Vector3 B1 = Ktorque*(3*(m1.Dot(p_unit))*p_unit - m1);
  torque = m2.Cross(B1);
  if (debug)
    std::cout << "B: " << B1 << " K: " << Ktorque << " t: " << torque << std::endl;
}

void DipoleMagnet::GetMFS(const math::Pose& p_self,
    const math::Pose& p_other,
    const math::Vector3& m_other,
    math::Vector3& mfs) {

  // sensor location
  math::Vector3 p = p_self.pos - p_other.pos;
  math::Vector3 p_unit = p/p.GetLength();

  // Get the field at the sensor location
  double K = 1e-7/pow(p.GetLength(), 3);
  math::Vector3 B = K*(3*(m_other.Dot(p_unit))*p_unit - m_other);

  // Rotate the B vector into the capsule/body frame
  math::Vector3 B_body = p_self.rot.RotateVectorReverse(B);

  // Assign vector
  mfs.x = B_body[0];
  mfs.y = B_body[1];
  mfs.z = B_body[2];
}

// Register this plugin with the simulator
GZ_REGISTER_MODEL_PLUGIN(DipoleMagnet)

}

ext: cc


text:
/*
 * Copyright (c) 2012, 2013 Aldebaran Robotics. All rights reserved.
 * Use of this source code is governed by a BSD-style license that can be
 * found in the COPYING file.
 */

#include "utils.hpp"

#include <algorithm>
#include <iterator>
#include <random>

#include <boost/filesystem.hpp>
#include <boost/utility/string_ref.hpp>

#include <qi/os.hpp>
#include <qi/path.hpp>

namespace {

  static auto randomSource = [] {
    std::random_device trueRand;
    std::seed_seq seed{ trueRand(), trueRand(), trueRand(), trueRand()
                      , trueRand(), trueRand(), trueRand(), trueRand()
                      };
    return std::default_random_engine{ seed };
  }();

  // Returns a random number inside `[min, max]` (`min` and `max` are included).
  //
  // Precondition: min <= max
  //
  // N is one of {short, int, long, long long,
  //  unsigned short, unsigned int, unsigned long, unsigned long long}
  template<typename N>
  N randomNumber(N min, N max)
  {
    std::uniform_int_distribution<N> dice(min, max);
    return dice(randomSource);
  }

  // Returns a random character inside `[0-9a-zA-Z]`.
  char randomAlphaNum()
  {
    static const boost::string_ref alphanums
      = "0123456789abcdefghijklmnopqrstuvwxyzABCDEFGHIJKLMNOPQRSTUVWXYZ";

    const auto randomIdx = randomNumber<int>(0, alphanums.size() - 1);
    return alphanums[randomIdx];
  }

}

std::string randomstr(std::string::size_type sz)
{
  std::string s;
  s.reserve(sz);
  generate_n(std::back_inserter(s), sz, randomAlphaNum);
  return s;
}

std::wstring wrandomstr(std::wstring::size_type sz)
{
  std::wstring s;
  s.reserve(sz);
  generate_n(std::back_inserter(s), sz, randomAlphaNum);
  return s;
}

std::string fsconcat(const std::vector<std::string>& parts)
{
  boost::filesystem::path p;
  for (const auto& part : parts)
  {
    if (!part.empty())
    {
      p.append(part, qi::unicodeFacet());
    }
  }
  return p.make_preferred().string(qi::unicodeFacet());
}

max_stars_repo_name: arntanguy/libqi

max_issues_repo_name: arntanguy/libqi

max_forks_repo_name: arntanguy/libqi


text:
/**
 * @file VNode.cpp
 * @author Ken.Matsushima <ken.ma24ma@yahoo.com>
 * @date 2015.4.13
 */

#include "Host.h"
#include <vector>
#include <boost/algorithm/string.hpp>
namespace gpds {

struct FIND_SUCCESSOR_DATA{
	NodeId id;
	const char address[30];
};

void NodePosition::add(const NodeId &key, std::shared_ptr<const RNode> node) {
	auto pair = std::make_shared<std::pair<NodeId, std::shared_ptr<const RNode>>>();
	std::memcpy(pair->first, key, sizeof(NodeId));
	pair->second = node;
	m_nodes.push_back(pair);
}

void NodePosition::add(const std::vector<NodePair> &list) {
	if (list.size()==0) return;
	m_nodes.insert(m_nodes.end(), list.begin(), list.end());
}

void NodePosition::ready() {
	std::sort(m_nodes.begin(), m_nodes.end(), [=](const NodePair &c1, const NodePair &c2) {
		return c1 > c2;
	});
}

NodePair NodePosition::searchAfter(const NodeId key) {
	for ( auto itr=m_nodes.begin(), end=m_nodes.end(); itr!=end; ++itr ) {
		if (std::memcmp(key, (*itr)->first, sizeof(NodeId)) < 0 ) return (*itr);
	}
	return (*m_nodes.begin());
}

NodePair NodePosition::searchBefore(const NodeId key) {
	for ( auto itr=m_nodes.begin(), end=m_nodes.end(); itr!=end; ++itr ) {
		if (std::memcmp(key, (*itr)->first, sizeof(NodeId)) < 0 ) return (*--itr);
	}
	return (*m_nodes.end());
}

void VNode::init(Host *host, uint8_t ringid, uint8_t nodeid[NODEID_LEN]){
	m_host = host;
	m_ringid = ringid;
	std::memcpy(m_nodeid, nodeid, NODEID_LEN);
}

bool VNode::interpret(std::shared_ptr<RNode> node, const h256 cnctid, uint8_t msgid, const uint8_t *data, uint16_t len) {
	switch (msgid) {
	case JOIN:
		procJoin(node, cnctid, data, len);
		break;
	case SEND_SUCCESSOR:
		procSendSuccessor(node, cnctid, data, len);
	}
	return true;
}

void VNode::onconnected(std::shared_ptr<RNode> node) {
	if (m_host==nullptr) return;
	h256 cnctid;
	m_host->makeCnctId(cnctid);
	auto wait = std::make_shared<std::pair<h256, std::shared_ptr<RNode>>>();
	std::memcpy(wait->first, cnctid, sizeof(h256));
	wait->second = nullptr;
	m_waitres.push_back(wait);
	m_host->send(*(node.get()), cnctid, m_ringid, JOIN, m_nodeid, NODEID_LEN);
}

void VNode::procJoin(std::shared_ptr<RNode> node, const h256 cnctid, const uint8_t *data, uint16_t len) {
	std::cout << "JOIN" << std::endl;
	NodePosition np;
	NodeId *recvid = *((NodeId**)&data);
	std::shared_ptr<std::string> rip = node->getAddress();
	std::cout<<rip<<std::endl;
	if (!m_host->checkNodeID(*recvid, m_ringid, *rip)) {
		std::cout << "JOIN:Invalid NodeId" << std::endl;
		return;
	}
	np.add(m_nodeid, nullptr);
	np.add(m_suclist);
	np.ready();
	NodePair pair = np.searchAfter(*recvid);
	if (pair->second==nullptr) {
		const std::string address = m_host->getAddress();
		std::vector<uint8_t> buf;
		uint16_t len;
		len = sizeof(NodeId) + address.size() +1;
		buf.resize(len);
		std::memcpy(buf.data(), m_nodeid, sizeof(NodeId));
		std::memcpy(buf.data()+sizeof(NodeId), address.c_str(), address.size());
		buf[len-1] = 0x0;
		m_host->send(*node.get(), cnctid, m_ringid, SEND_SUCCESSOR, buf.data(),len);
	} else {
		auto wait = std::make_shared<std::pair<h256, std::shared_ptr<RNode>>>();
		memcpy(wait->first, cnctid, sizeof(h256));
		wait->second = node;
		m_waitres.push_back(wait);
		m_host->send(*pair->second, cnctid, m_ringid, JOIN, data, len);
	}
}

void VNode::procSendSuccessor(std::shared_ptr<RNode> node, const h256 cnctid, const uint8_t *data, uint16_t len) {
	std::cout << "SEND_SUCCESSOR" << std::endl;
	for ( auto itr=m_waitres.begin(), end=m_waitres.end(); itr!=end; ++itr) {
		if (std::memcmp(cnctid, (*itr)->first, sizeof(h256))==0) {
			if ((*itr)->second==nullptr) {
				std::cout << "FOUND:SUCCESSOR" << std::endl;
				FIND_SUCCESSOR_DATA *buf;
				buf = (FIND_SUCCESSOR_DATA*)data;
				std::vector<char> address;
				address.resize(len);
				std::memcpy(address.data(), buf->address, len-sizeof(NodeId));
				int portpos = 0;
				for ( ;portpos<len; portpos++) {
					if (address[portpos]==':') {
						address[portpos++] = 0x0;
						break;
					}
				}
				m_host->cnct(address.data(), address.data()+portpos, [=](std::shared_ptr<RNode> node) {
					auto pair = std::make_shared<std::pair<NodeId, std::shared_ptr<const RNode>>>();
					std::memcpy((void*)pair->first, buf->id, sizeof(NodeId));
					pair->second = node;
					m_suclist.push_back(pair);
				});
			} else {
				m_host->send(*(*itr)->second, cnctid, m_ringid, SEND_SUCCESSOR, data, len);
			}
			return;
		}
	}

	std::cout << "VNode::procSendSuccessor - can't find join message" << std::endl;
}

}

max_stars_repo_name: kenmazsyma/GPDS

max_issues_repo_name: kenmazsyma/GPDS

max_forks_repo_name: kenmazsyma/GPDS


text:
/*
 * This file is part of the DUDS project. It is subject to the BSD-style
 * license terms in the LICENSE file found in the top-level directory of this
 * distribution and at https://github.com/jjackowski/duds/blob/master/LICENSE.
 * No part of DUDS, including this file, may be copied, modified, propagated,
 * or distributed except according to the terms contained in the LICENSE file.
 *
 * Copyright (C) 2017  Jeff Jackowski
 */
#ifndef SMBUS_HPP
#define SMBUS_HPP

#include <cstdint>
#include <vector>
#include <boost/noncopyable.hpp>

namespace duds { namespace hardware { namespace interface {

/**
 * An interface for communication with a
 * [SMBus](http://en.wikipedia.org/wiki/System_Management_Bus) device.
 * This is intended to communicate with a single device. Use one for each
 * device, even if they use the same bus. As a result, implementations need
 * not be thread-safe since it makes little sense to attempt multiple
 * communications with the same device. However, the bus should be handled in
 * a thread-safe manner; multiple simultaneous attempts to use the same bus
 * to communicate with different devices should work.
 *
 * Some I2C devices communicate in a manner that allows them to be handled as
 * SMBus devices. Such devices should use Smbus instead of I2c to take
 * advantage of the easier-to-use interface of Smbus. The Linux kernel
 * documentation implies that SMBus master hardware exists, so favoring the
 * use of Smbus may also increase compatibility without requiring the use of
 * a software master using GPIOs.
 *
 * Whenever possible, use Packet Error Checking (PEC) with communications. It
 * will help prevent bad data on the bus from causing trouble by noticing
 * the bad data and generating an error. It is not part of the I2C
 * specification, so it should be disabled for use with SMBus-like I2C devices.
 *
 * All word, two byte, std::uint16_t data is in host endianness. The SMBus
 * protocol specifies that words are sent in little endian order. This must
 * work on both little and big endian hosts. To assist in dealing with devices
 * that are not SMBus compliant and use big-endian data, the receiveWordBe()
 * and transmitWordBe() functions will do an endianness conversion on the data.
 * They allow the code to use host endianness while communicating with
 * big-endian data.
 *
 * There isn't a matching access object because SMBus is specified in such a
 * way that it shouldn't be required.
 *
 * @todo  Add ability to query SMBus master's capabilities.
 *
 * @author  Jeff Jackowski
 */
class Smbus : boost::noncopyable {
public:
	virtual ~Smbus() = 0;
	// PEC
	/**
	 * Use with constructors to explicitly specify that Packet Error Checking
	 * (PEC) be used. This should normally be the default.
	 */
	struct UsePec { };
	/**
	 * Use with constructors to specify that Packet Error Checking (PEC) will
	 * not be used. This should be avoided unless it is unsupported, as is the
	 * case with I2C devices.
	 */
	struct NoPec { };
	// some flag for big-endian data; standard is little
	/**
	 * Read a single byte from the device without sending a command/register
	 * byte first.
	 * @return  The byte from the device.
	 * @throw SmbusErrorPec          The PEC checksum was not valid for the data.
	 * @throw SmbusErrorBusy         The bus was in use for an inordinate length
	 *                               of time. This is not caused by scheduling
	 *                               on the same host computer.
	 * @throw SmbusErrorNoDevice     The device did not respond to its address.
	 * @throw SmbusErrorUnsupported  This operation is unsupported by the master.
	 * @throw SmbusErrorProtocol     Data from the device does not conform to
	 *                               the SMBus protocol.
	 * @throw SmbusErrorTimeout      The operation took too long resulting in a
	 *                               bus timeout.
	 * @throw SmbusError             A general error that doesn't fit one of the
	 *                               other exceptions.
	 */
	virtual std::uint8_t receiveByte() = 0;
	/**
	 * Sends a command byte, then reads a single byte from the device.
	 * @param cmd  The command or register byte.
	 * @return     The byte from the device.
	 * @throw SmbusErrorPec          The PEC checksum was not valid for the data.
	 * @throw SmbusErrorBusy         The bus was in use for an inordinate length
	 *                               of time. This is not caused by scheduling
	 *                               on the same host computer.
	 * @throw SmbusErrorNoDevice     The device did not respond to its address.
	 * @throw SmbusErrorUnsupported  This operation is unsupported by the master.
	 * @throw SmbusErrorProtocol     Data from the device does not conform to
	 *                               the SMBus protocol.
	 * @throw SmbusErrorTimeout      The operation took too long resulting in a
	 *                               bus timeout.
	 * @throw SmbusError             A general error that doesn't fit one of the
	 *                               other exceptions.
	 */
	virtual std::uint8_t receiveByte(std::uint8_t cmd) = 0;
	/**
	 * Sends a command byte, then reads a word, two bytes, from the device.
	 * @param cmd  The command or register byte.
	 * @return     The word from the device.
	 * @throw SmbusErrorPec          The PEC checksum was not valid for the data.
	 * @throw SmbusErrorBusy         The bus was in use for an inordinate length
	 *                               of time. This is not caused by scheduling
	 *                               on the same host computer.
	 * @throw SmbusErrorNoDevice     The device did not respond to its address.
	 * @throw SmbusErrorUnsupported  This operation is unsupported by the master.
	 * @throw SmbusErrorProtocol     Data from the device does not conform to
	 *                               the SMBus protocol.
	 * @throw SmbusErrorTimeout      The operation took too long resulting in a
	 *                               bus timeout.
	 * @throw SmbusError             A general error that doesn't fit one of the
	 *                               other exceptions.
	 */
	virtual std::uint16_t receiveWord(std::uint8_t cmd) = 0;
	/**
	 * Sends a command byte, then reads a big-endian word from the device.
	 * @param cmd  The command or register byte.
	 * @return     The word from the device in host endianness.
	 * @note  This function is only useful for devices that are not actually
	 *        SMBus compliant. Some I2C devices that can be used as SMBus
	 *        devices may use big-endian data.
	 * @throw SmbusErrorPec          The PEC checksum was not valid for the data.
	 * @throw SmbusErrorBusy         The bus was in use for an inordinate length
	 *                               of time. This is not caused by scheduling
	 *                               on the same host computer.
	 * @throw SmbusErrorNoDevice     The device did not respond to its address.
	 * @throw SmbusErrorUnsupported  This operation is unsupported by the master.
	 * @throw SmbusErrorProtocol     Data from the device does not conform to
	 *                               the SMBus protocol.
	 * @throw SmbusErrorTimeout      The operation took too long resulting in a
	 *                               bus timeout.
	 * @throw SmbusError             A general error that doesn't fit one of the
	 *                               other exceptions.
	 */
	std::uint16_t receiveWordBe(std::uint8_t cmd) {
		std::uint16_t result = receiveWord(cmd);
		return (result << 8) | (result >> 8);
	}
	/**
	 * Sends a command byte, then reads a block of data from the device.
	 * @param cmd     The command or register byte.
	 * @param in      The input buffer.
	 * @param maxlen  The maximum amount of data to write into @a in.
	 * @return        The number of bytes received from the device.
	 * @throw SmbusErrorMessageLength The buffer @a in could not hold all the
	 *                                received data.
	 * @throw SmbusErrorPec           The PEC checksum was not valid for the data.
	 * @throw SmbusErrorBusy          The bus was in use for an inordinate length
	 *                                of time. This is not caused by scheduling
	 *                                on the same host computer.
	 * @throw SmbusErrorNoDevice      The device did not respond to its address.
	 * @throw SmbusErrorUnsupported   This operation is unsupported by the master.
	 * @throw SmbusErrorProtocol      Data from the device does not conform to
	 *                                the SMBus protocol.
	 * @throw SmbusErrorTimeout       The operation took too long resulting in a
	 *                                bus timeout.
	 * @throw SmbusError              A general error that doesn't fit one of the
	 *                                other exceptions.
	 */
	virtual int receive(std::uint8_t cmd, std::uint8_t *in,
		const int maxlen) = 0;
	/**
	 * Sends a command byte, then reads a block of data from the device.
	 * @param cmd  The command or register byte.
	 * @param in   The input vector. Upon success, the vector will be
	 *             resized to match the received data.
	 * @throw SmbusErrorPec          The PEC checksum was not valid for the data.
	 * @throw SmbusErrorBusy         The bus was in use for an inordinate length
	 *                               of time. This is not caused by scheduling
	 *                               on the same host computer.
	 * @throw SmbusErrorNoDevice     The device did not respond to its address.
	 * @throw SmbusErrorUnsupported  This operation is unsupported by the master.
	 * @throw SmbusErrorProtocol     Data from the device does not conform to
	 *                               the SMBus protocol.
	 * @throw SmbusErrorTimeout      The operation took too long resulting in a
	 *                               bus timeout.
	 * @throw SmbusError             A general error that doesn't fit one of the
	 *                               other exceptions.
	 */
	virtual void receive(std::uint8_t cmd, std::vector<std::uint8_t> &in) = 0;
	/**
	 * Sends a single bit to the device.
	 * @throw SmbusErrorPec          The PEC checksum was not valid for the data.
	 * @throw SmbusErrorBusy         The bus was in use for an inordinate length
	 *                               of time. This is not caused by scheduling
	 *                               on the same host computer.
	 * @throw SmbusErrorNoDevice     The device did not respond to its address.
	 * @throw SmbusErrorUnsupported  This operation is unsupported by the master.
	 * @throw SmbusErrorProtocol     Data from the device does not conform to
	 *                               the SMBus protocol.
	 * @throw SmbusErrorTimeout      The operation took too long resulting in a
	 *                               bus timeout.
	 * @throw SmbusError             A general error that doesn't fit one of the
	 *                               other exceptions.
	 */
	virtual void transmitBool(bool out) = 0;
	/**
	 * Sends a single byte to the device.
	 * @param byte  The byte to send.
	 * @throw SmbusErrorPec          The PEC checksum was not valid for the data.
	 * @throw SmbusErrorBusy         The bus was in use for an inordinate length
	 *                               of time. This is not caused by scheduling
	 *                               on the same host computer.
	 * @throw SmbusErrorNoDevice     The device did not respond to its address.
	 * @throw SmbusErrorUnsupported  This operation is unsupported by the master.
	 * @throw SmbusErrorProtocol     Data from the device does not conform to
	 *                               the SMBus protocol.
	 * @throw SmbusErrorTimeout      The operation took too long resulting in a
	 *                               bus timeout.
	 * @throw SmbusError             A general error that doesn't fit one of the
	 *                               other exceptions.
	 */
	virtual void transmitByte(std::uint8_t byte) = 0;
	/**
	 * Sends a command byte and a data byte to the device.
	 * @param cmd   The command or register byte.
	 * @param byte  The data byte to send.
	 * @throw SmbusErrorPec          The PEC checksum was not valid for the data.
	 * @throw SmbusErrorBusy         The bus was in use for an inordinate length
	 *                               of time. This is not caused by scheduling
	 *                               on the same host computer.
	 * @throw SmbusErrorNoDevice     The device did not respond to its address.
	 * @throw SmbusErrorUnsupported  This operation is unsupported by the master.
	 * @throw SmbusErrorProtocol     Data from the device does not conform to
	 *                               the SMBus protocol.
	 * @throw SmbusErrorTimeout      The operation took too long resulting in a
	 *                               bus timeout.
	 * @throw SmbusError             A general error that doesn't fit one of the
	 *                               other exceptions.
	 */
	virtual void transmitByte(std::uint8_t cmd, std::uint8_t byte) = 0;
	/**
	 * Sends a command byte and a data word to the device.
	 * @param cmd   The command or register byte.
	 * @param word  The data word to send.
	 * @throw SmbusErrorPec          The PEC checksum was not valid for the data.
	 * @throw SmbusErrorBusy         The bus was in use for an inordinate length
	 *                               of time. This is not caused by scheduling
	 *                               on the same host computer.
	 * @throw SmbusErrorNoDevice     The device did not respond to its address.
	 * @throw SmbusErrorUnsupported  This operation is unsupported by the master.
	 * @throw SmbusErrorProtocol     Data from the device does not conform to
	 *                               the SMBus protocol.
	 * @throw SmbusErrorTimeout      The operation took too long resulting in a
	 *                               bus timeout.
	 * @throw SmbusError             A general error that doesn't fit one of the
	 *                               other exceptions.
	 */
	virtual void transmitWord(std::uint8_t cmd, std::uint16_t word) = 0;
	/**
	 * Sends a command byte and a big-endian data word to the device.
	 * @param cmd   The command or register byte.
	 * @param word  The data word to send in host endianness.
	 * @note  This function is only useful for devices that are not actually
	 *        SMBus compliant. Some I2C devices that can be used as SMBus
	 *        devices may use big-endian data.
	 * @throw SmbusErrorPec          The PEC checksum was not valid for the data.
	 * @throw SmbusErrorBusy         The bus was in use for an inordinate length
	 *                               of time. This is not caused by scheduling
	 *                               on the same host computer.
	 * @throw SmbusErrorNoDevice     The device did not respond to its address.
	 * @throw SmbusErrorUnsupported  This operation is unsupported by the master.
	 * @throw SmbusErrorProtocol     Data from the device does not conform to
	 *                               the SMBus protocol.
	 * @throw SmbusErrorTimeout      The operation took too long resulting in a
	 *                               bus timeout.
	 * @throw SmbusError             A general error that doesn't fit one of the
	 *                               other exceptions.
	 */
	void transmitWordBe(std::uint8_t cmd, std::uint16_t word) {
		transmitWord(cmd, (word << 8) | (word >> 8));
	}
	/**
	 * Sends a command byte and a block of data to the device.
	 * @param cmd   The command or register byte.
	 * @param out   The data to send.
	 * @param len   The number of bytes in the buffer @a out to send. It must be
	 *              between 1 and 32, inclusive.
	 * @throw SmbusErrorMessageLength A request was made to send a block of less
	 *                                than 1 or more than 32 bytes.
	 * @throw SmbusErrorPec           The PEC checksum was not valid for the data.
	 * @throw SmbusErrorBusy          The bus was in use for an inordinate length
	 *                                of time. This is not caused by scheduling
	 *                                on the same host computer.
	 * @throw SmbusErrorNoDevice      The device did not respond to its address.
	 * @throw SmbusErrorUnsupported   This operation is unsupported by the master.
	 * @throw SmbusErrorProtocol      Data from the device does not conform to
	 *                                the SMBus protocol.
	 * @throw SmbusErrorTimeout       The operation took too long resulting in a
	 *                                bus timeout.
	 * @throw SmbusError              A general error that doesn't fit one of the
	 *                                other exceptions.
	 */
	virtual void transmit(
		std::uint8_t cmd,
		const std::uint8_t *out,
		const int len
	) = 0;
	/**
	 * Sends a command byte and a block of data to the device.
	 * @param cmd   The command or register byte.
	 * @param out   The data to send. It must have between 1 and 32 bytes,
	 *              inclusive.
	 * @throw SmbusErrorMessageLength A request was made to send a block of less
	 *                                than 1 or more than 32 bytes.
	 * @throw SmbusErrorPec           The PEC checksum was not valid for the data.
	 * @throw SmbusErrorBusy          The bus was in use for an inordinate length
	 *                                of time. This is not caused by scheduling
	 *                                on the same host computer.
	 * @throw SmbusErrorNoDevice      The device did not respond to its address.
	 * @throw SmbusErrorUnsupported   This operation is unsupported by the master.
	 * @throw SmbusErrorProtocol      Data from the device does not conform to
	 *                                the SMBus protocol.
	 * @throw SmbusErrorTimeout       The operation took too long resulting in a
	 *                                bus timeout.
	 * @throw SmbusError              A general error that doesn't fit one of the
	 *                                other exceptions.
	 */
	void transmit(std::uint8_t cmd, const std::vector<std::uint8_t> &out) {
		transmit(cmd, &(out[0]), out.size());
	}
	/**
	 * Does a process call operation. Sends a command byte and a word to the
	 * device, then receives a word.
	 * @param cmd   The command or register byte.
	 * @param word  The word to send.
	 * @return      The word received from the device.
	 * @throw SmbusErrorPec          The PEC checksum was not valid for the data.
	 * @throw SmbusErrorBusy         The bus was in use for an inordinate length
	 *                               of time. This is not caused by scheduling
	 *                               on the same host computer.
	 * @throw SmbusErrorNoDevice     The device did not respond to its address.
	 * @throw SmbusErrorUnsupported  This operation is unsupported by the master.
	 * @throw SmbusErrorProtocol     Data from the device does not conform to
	 *                               the SMBus protocol.
	 * @throw SmbusErrorTimeout      The operation took too long resulting in a
	 *                               bus timeout.
	 * @throw SmbusError             A general error that doesn't fit one of the
	 *                               other exceptions.
	 */
	virtual std::uint16_t call(std::uint8_t cmd, std::uint16_t word) = 0;
	/**
	 * Does a block process call operation. Sends a command byte and block of
	 * data to the device, then receives a block of data.
	 * @param cmd   The command or register byte.
	 * @param out   The data block to send. It must not have more than 32 bytes.
	 * @param in    The data block received from the device.
	 * @throw SmbusErrorMessageLength A request was made to send a block of more
	 *                                than 32 bytes.
	 * @throw SmbusErrorPec           The PEC checksum was not valid for the data.
	 * @throw SmbusErrorBusy          The bus was in use for an inordinate length
	 *                                of time. This is not caused by scheduling
	 *                                on the same host computer.
	 * @throw SmbusErrorNoDevice      The device did not respond to its address.
	 * @throw SmbusErrorUnsupported   This operation is unsupported by the master.
	 * @throw SmbusErrorProtocol      Data from the device does not conform to
	 *                                the SMBus protocol.
	 * @throw SmbusErrorTimeout       The operation took too long resulting in a
	 *                                bus timeout.
	 * @throw SmbusError              A general error that doesn't fit one of the
	 *                                other exceptions.
	 */
	virtual void call(
		std::uint8_t cmd,
		const std::vector<std::uint8_t> &out,
		std::vector<std::uint8_t> &in
	) = 0;
	/**
	 * Returns the address of the device that this object will attempt to
	 * communicate with.
	 */
	virtual int address() const = 0;
};

} } }

#endif        //  #ifndef SMBUS_HPP

max_stars_repo_name: jjackowski/duds

max_issues_repo_name: jjackowski/duds

max_forks_repo_name: jjackowski/duds


text:
/*
 * Copyright 2018 James Dyer
 *
 * Licensed under the Apache License, Version 2.0 (the "License"); you may not
 * use this file except in compliance with the License. You may obtain a copy of
 * the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
 * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
 * License for the specific language governing permissions and limitations under
 * the License.
 *
 * This is being developed for the TANGO Project: http://tango-project.eu
 */

#include <sstream>
#include <NTL/RR.h>
#include <json/json.h>
#include "HE2Encrypter.h"
#include "Random.h"

HE2Encrypter::HE2Encrypter(int lambda, int eta){
	generateParameters(lambda,eta);
}

HE2Encrypter::HE2Encrypter(int n, int d, int rho)
{
    /* Compute lower bound for p */
	NTL::RR rtwo(2);
	NTL::RR exp1(rho*d);
	NTL::RR nplusone(n+1);
	NTL::RR exp2(d);
	NTL::RR ploBound = pow(rtwo,exp1)*pow(nplusone,exp2);
	NTL::ZZ pLowerBound = to_ZZ(ploBound);

	/* Derive lambda and eta */
	long lambda = NumBits(pLowerBound) + 1;
	long eta = ((lambda * lambda / rho) - lambda);
	generateParameters(lambda,eta);
}

NTL::vec_ZZ_p HE2Encrypter::encrypt(NTL::ZZ& plaintext)
{
    NTL::ZZ_p r = to_ZZ_p(rng->nextBigInteger(q));
    NTL::ZZ_p s = to_ZZ_p(rng->nextBigInteger(modulus));
    NTL::ZZ_p ptext = to_ZZ_p(plaintext);
    NTL::ZZ_p c = ptext + r*(*pmod);
    return ONE_VECTOR*c + (*a)*s;
}

std::string HE2Encrypter::writeSecretsToJSON()
{
	Json::Value root;
	std::ostringstream pStr,modStr,gammaStr;
	pStr << p;
	root["p"] = pStr.str();
	modStr << modulus;
	root["modulus"] = modStr.str();
	gammaStr << (*gamma);
    root["gamma"]=gammaStr.str();
	Json::FastWriter writer;
	return writer.write(root);
}

max_stars_repo_name: TANGO-Project/cryptsdc

max_issues_repo_name: TANGO-Project/cryptsdc

max_forks_repo_name: TANGO-Project/cryptsdc


text:
/*
    Copyright 2013 Adobe
    Distributed under the Boost Software License, Version 1.0.
    (See accompanying file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
*/
/**************************************************************************************************/

#ifndef ADOBE_EMPTY_HPP
#define ADOBE_EMPTY_HPP

/**************************************************************************************************/

#include <adobe/config.hpp>

#include <boost/mpl/bool.hpp>
#include <boost/operators.hpp>
#include <boost/type_traits/has_trivial_constructor.hpp>
#include <boost/type_traits/has_trivial_destructor.hpp>
#include <boost/type_traits/is_empty.hpp>
#include <boost/type_traits/is_pod.hpp>

#if defined(ADOBE_STD_SERIALIZATION)
#include <iosfwd>
#endif

/**************************************************************************************************/

namespace adobe {
namespace version_1 {

/**************************************************************************************************/

/*!
\ingroup abi_misc

\model_of
    - \ref concept_regular_type
    - \ref stldoc_LessThanComparable

\brief An empty regular- and less-than-comparable- type.

\rationale
    empty_t is useful for default values. For example, value_t() will construct a value_t
containing
an empty_t by default.
*/

struct empty_t : private boost::totally_ordered<empty_t> {
    friend inline bool operator==(const empty_t&, const empty_t&) { return true; }
    friend inline bool operator<(const empty_t&, const empty_t&) { return false; }
    friend inline void swap(empty_t&, empty_t&) {}
};

#if defined(ADOBE_STD_SERIALIZATION)
//!\ingroup abi_misc
std::ostream& operator<<(std::ostream& stream, const empty_t&);
#endif

/**************************************************************************************************/

} // namespace version_1

using version_1::empty_t;

/**************************************************************************************************/

/*!
    empty_base is used to provide a unique empty base class when base class chaining is used to
    reduce object size overhead such as with Boost operators in Boost 1.31.0
*/

//!\ingroup util_misc
template <typename Derived>
struct empty_base {};

/**************************************************************************************************/

} // namespace adobe

/**************************************************************************************************/

namespace boost {

template <>
struct is_pod<adobe::version_1::empty_t> : boost::mpl::true_ {};
template <>
struct is_empty<adobe::version_1::empty_t> : boost::mpl::true_ {};
template <>
struct has_trivial_constructor<adobe::version_1::empty_t> : boost::mpl::true_ {};
template <>
struct has_trivial_destructor<adobe::version_1::empty_t> : boost::mpl::true_ {};
} // namespace boost

/**************************************************************************************************/

#endif

/**************************************************************************************************/