text:
#include "wheel_odometry.h"

#include <queue>
#include <jsoncpp/json/json.h>
#include <Eigen/Core>
#include <fstream>
#include <iostream>
#include <algorithm>

namespace odometry
{
    LookupTable::LookupTable(const int n, const double *break_points, const double *values)
    {
        _break_points.resize(n);
        _values.resize(n);
        for (int i = 0; i < n; ++i)
        {
            _break_points[i] = *break_points++;
            _values[i] = *values++;
        }
    }

    double LookupTable::Lookup(double x) const
    {
        auto loc = std::lower_bound(_break_points.begin(), _break_points.end(), x) - _break_points.begin();
        if (loc == 0)
            loc = 1;
        if (loc == _break_points.size())
            --loc;

        const auto x1 = _break_points[loc - 1];
        const auto y1 = _values[loc - 1];
        const auto x2 = _break_points[loc];
        const auto y2 = _values[loc];

        return (y1 * (x2 - x) + y2 * (x - x1)) / (x2 - x1);
    }

    WheelOdometry::WheelOdometry(const std::string &json_params_file_path)
    {
        Json::Value params;
        Json::Reader param_reader;
        std::ifstream fs(json_params_file_path);
        if (!param_reader.parse(fs, params, false))
        {
            std::cerr << "params file opening error in WheelOdometry" << std::endl;
            fs.close();
            exit(1);
        }
        fs.close();
        _vehicle_param.wheel_base = params.get("wheel_base", -1).asDouble();
        _vehicle_param.front_wheel_count_full = params.get("front_wheel_count_full", -1).asInt();
        _vehicle_param.rear_wheel_count_full = params.get("rear_wheel_count_full", -1).asInt();
        _vehicle_param.count_period = params.get("count_period", -1).asDouble();
        _vehicle_param.front_wheel_radius = params.get("front_wheel_radius", -1).asDouble();
        _vehicle_param.rear_wheel_radius = params.get("rear_wheel_radius", -1).asDouble();
        _vehicle_param.front_wheel_track = params.get("front_wheel_track", -1).asDouble();
        _vehicle_param.rear_wheel_track = params.get("rear_wheel_track", -1).asDouble();
        const auto enable_vec_fl_fr_rl_rr = params.get("enable_vec_fl_fr_rl_rr", Json::Value::null);
        const auto steering_angle_forward = params.get("steering_angle_forward", Json::Value::null);
        const auto curvature_forward = params.get("curvature_forward", Json::Value::null);
        const auto steering_angle_backward = params.get("steering_angle_backward", Json::Value::null);
        const auto curvature_backward = params.get("curvature_backward", Json::Value::null);
        if (!_vehicle_param.check() || steering_angle_forward.size() != curvature_forward.size() || curvature_forward.size() < 2 || steering_angle_backward.size() != curvature_backward.size() || curvature_backward.size() < 2 || enable_vec_fl_fr_rl_rr.size() != 4)
        {
            std::cerr << "Vehicle Params Error" << std::endl;
            exit(1);
        }

        for (int i = 0; i < 4; ++i)
        {
            _vehicle_param.enable_vec_fl_fr_rl_rr[i] = enable_vec_fl_fr_rl_rr[i].asDouble();
        }

        std::vector<double> c_f(curvature_forward.size());
        std::vector<double> s_f(steering_angle_forward.size());
        std::vector<double> c_b(curvature_backward.size());
        std::vector<double> s_b(steering_angle_backward.size());

        for (int i = 0; i < c_f.size(); ++i)
        {
            s_f[i] = steering_angle_forward[i].asDouble();
            c_f[i] = curvature_forward[i].asDouble();
        }

        for (int i = 0; i < c_b.size(); ++i)
        {
            s_b[i] = steering_angle_backward[i].asDouble();
            c_b[i] = curvature_backward[i].asDouble();
        }

        _vehicle_param.lookup_curvature_forward = new LookupTable(c_f.size(), s_f.data(), c_f.data());
        _vehicle_param.lookup_curvature_backward = new LookupTable(c_b.size(), s_b.data(), c_b.data());
    }

    void WheelOdometry::updateWheelingCount(WheelCount FL, WheelCount FR, WheelCount RL, WheelCount RR, const uint64_t stamp)
    {
        if (!(_status_flag & WHEELING_COUNT_READY))
        {
            _status_flag |= WHEELING_COUNT_READY;
            _chasis_status.wheeling_count_last[0] = FL;
            _chasis_status.wheeling_count_last[1] = FR;
            _chasis_status.wheeling_count_last[2] = RL;
            _chasis_status.wheeling_count_last[3] = RR;
            _stamp = stamp;
            _stamp_mid = stamp;
            return;
        }
        _chasis_status.wheeling_count_curr[0] = FL;
        _chasis_status.wheeling_count_curr[1] = FR;
        _chasis_status.wheeling_count_curr[2] = RL;
        _chasis_status.wheeling_count_curr[3] = RR;

        if (_status_flag != ALL_READY)
        {
            return;
        }
        Eigen::Map<Eigen::Array<WheelCount, 4, 1>> curr_count{_chasis_status.wheeling_count_curr};
        Eigen::Map<Eigen::Array<WheelCount, 4, 1>> last_count{_chasis_status.wheeling_count_last};

        Eigen::Array<double, 4, 1> count_diff = (curr_count - last_count).cast<double>();

        double dt = (stamp - _stamp) * 1e-9;

        if((count_diff.sum() == 0) && (dt < 0.1)) return;

        const Eigen::Map<Eigen::Array<double, 4, 1>> enable_vec{&_vehicle_param.enable_vec_fl_fr_rl_rr[0]};

        const LookupTable *ltable = _vehicle_param.lookup_curvature_backward;
        if (_chasis_status.is_forward)
        {
            ltable = _vehicle_param.lookup_curvature_forward;
        }

        const auto curvatue = ltable->Lookup(_chasis_status.steering_angle);
        const auto half_Df_c = 0.5 * _vehicle_param.front_wheel_track * curvatue;
        const auto half_Dr_c = 0.5 * _vehicle_param.rear_wheel_track * curvatue;
        const auto wheelbase_c = _vehicle_param.wheel_base * curvatue;
        const auto coeff_front = 2 * M_PI / _vehicle_param.front_wheel_count_full * _vehicle_param.front_wheel_radius;
        const auto coeff_rear = 2 * M_PI / _vehicle_param.rear_wheel_count_full * _vehicle_param.rear_wheel_radius;

        for (int i = 0; i < 4; ++i)
        {
            if (count_diff[i] > _vehicle_param.count_period / 2)
                count_diff[i] -= _vehicle_param.count_period;
            else if (count_diff[i] < -_vehicle_param.count_period / 2)
                count_diff[i] += _vehicle_param.count_period;
        }

        // FL FR RL RR
        const Eigen::Array<double, 4, 1> factor{coeff_front / Eigen::Vector2d{1.0 - half_Df_c, wheelbase_c}.norm(),
                                                coeff_front / Eigen::Vector2d{1.0 + half_Df_c, wheelbase_c}.norm(),
                                                coeff_rear / (1.0 - half_Dr_c),
                                                coeff_rear / (1.0 + half_Dr_c)};

        double ds = (enable_vec * count_diff * factor).sum() / enable_vec.sum();

        if (!_chasis_status.is_forward)
            ds *= -1;

        const double dyaw = ds * curvatue;
        _x += ds * std::cos(_yaw);
        _y += ds * std::sin(_yaw);
        _yaw += dyaw;
        _distance += std::abs(ds);

        static std::queue<std::array<double, 3>> diffrencial;
        static double integrated_yaw = 0, integrated_s = 0, integrated_t = 0;

        diffrencial.push({dyaw, ds ,dt});
        integrated_yaw += dyaw;
        integrated_s += ds;
        integrated_t += dt;
        if(integrated_t > _sliding_time)
        {
            _local_yaw_rate = integrated_yaw / integrated_t;
            _local_v = integrated_s / integrated_t;
            _stamp_mid = stamp - 5e8 * integrated_t;
            _is_new_twist = true;
            while(integrated_t > _sliding_time)
            {
                const auto old_data = diffrencial.front();
                diffrencial.pop();
                integrated_yaw -= old_data[0];
                integrated_s -= old_data[1];
                integrated_t -= old_data[2];
            }
        }
        // _local_yaw_rate = dyaw / dt;
        // _local_v = ds / dt;
        // _stamp_mid = (stamp - _stamp) / 2 + _stamp;

        last_count = curr_count;
        _stamp = stamp;
        _is_new_pose = true;
    }
} // namespace odometry

max_stars_repo_name: Saki-Chen/W-LOAM

max_issues_repo_name: xingchengzhi/W-LOAM

max_forks_repo_name: xingchengzhi/W-LOAM


text:
/*
 * Copyright (c) 2014, Stanislav Vorobiov
 * 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.
 *
 * 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.
 */

#include "af/TPS.h"
#include "af/Utils.h"
#include "Logger.h"
#include <boost/make_shared.hpp>
#include <json/json.h>
#include <log4cplus/ndc.h>

namespace af
{
    static const TPSEntry dummyEntry;

    TPS::TPS(const std::string& imageFileName,
             UInt32 width,
             UInt32 height)
    : imageFileName_(imageFileName),
      width_(width),
      height_(height)
    {
    }

    TPS::~TPS()
    {
    }

    TPSPtr TPS::fromStream(const std::string& fileName, std::istream& is)
    {
        std::string json;

        {
            log4cplus::NDCContextCreator ndc(fileName);

            if (!is) {
                LOG4CPLUS_ERROR(afutil::logger(), "Cannot open file");

                return TPSPtr();
            }

            if (!readStream(is, json)) {
                LOG4CPLUS_ERROR(afutil::logger(), "Error reading file");

                return TPSPtr();
            }
        }

        return fromString(fileName, json);
    }

    TPSPtr TPS::fromString(const std::string& fileName, const std::string& json)
    {
        Json::Value jsonValue;
        Json::Reader reader;

        log4cplus::NDCContextCreator ndc(fileName);

        if (!reader.parse(json, jsonValue)) {
            LOG4CPLUS_ERROR(afutil::logger(),
                            "Failed to parse JSON: " << reader.getFormattedErrorMessages());
            return TPSPtr();
        }

        LOG4CPLUS_DEBUG(afutil::logger(), "Processing...");

        return fromJsonValue(jsonValue);
    }

    TPSPtr TPS::fromJsonValue(const Json::Value& jsonValue)
    {
        std::string imageFileName = jsonValue["meta"]["image"].asString();
        UInt32 width = jsonValue["meta"]["size"]["w"].asUInt();
        UInt32 height = jsonValue["meta"]["size"]["h"].asUInt();

        TPSPtr tps = boost::make_shared<TPS>(imageFileName, width, height);

        int i = 0;
        Json::Value tmp = jsonValue["frames"][i];
        while (!tmp.isNull()) {
            std::ostringstream os;
            os << "frame " << i++;
            log4cplus::NDCContextCreator ndc(os.str());

            std::string fileName = tmp["filename"].asString();
            UInt32 x = tmp["frame"]["x"].asUInt();
            UInt32 y = tmp["frame"]["y"].asUInt();
            UInt32 width = tmp["frame"]["w"].asUInt();
            UInt32 height = tmp["frame"]["h"].asUInt();

            TPSEntry entry(x, y, width, height);

            tps->addEntry(fileName, entry);

            tmp = jsonValue["frames"][i];
        }

        return tps;
    }

    void TPS::addEntry(const std::string& fileName, const TPSEntry& entry)
    {
        if (!entries_.insert(std::make_pair(fileName, entry)).second) {
            LOG4CPLUS_WARN(afutil::logger(),
                           "File \"" << fileName << "\" already in TPS \"" << imageFileName_ << "\"");
        }
    }

    const TPSEntry& TPS::entry(const std::string& fileName) const
    {
        Entries::const_iterator it = entries_.find(fileName);

        if (it == entries_.end()) {
            LOG4CPLUS_WARN(afutil::logger(),
                           "File \"" << fileName << "\" not found in TPS \"" << imageFileName_ << "\"");
            return dummyEntry;
        }

        return it->second;
    }
}

ext: cpp

max_stars_repo_name: Sheph/TriggerTime

max_issues_repo_name: Sheph/TriggerTime

max_forks_repo_name: Sheph/TriggerTime


text:
// Copyright 2017, 2018 Peter Dimov.
// Distributed under the Boost Software License, Version 1.0.

#include <boost/hash2/fnv1a.hpp>
#include <boost/hash2/siphash.hpp>
#include <boost/hash2/xxhash.hpp>
#include <boost/hash2/spooky2.hpp>
#include <boost/hash2/murmur3.hpp>
#include <boost/hash2/md5.hpp>
#include <boost/hash2/sha1.hpp>
#include <boost/core/lightweight_test.hpp>
#include <algorithm>
#include <cstddef>

template<class H> void test()
{
    H h;

    typename H::result_type r1 = h.result();

    typename H::result_type r2 = h.result();
    BOOST_TEST( r1 != r2 );

    typename H::result_type r3 = h.result();
    BOOST_TEST( r2 != r3 );

    typename H::result_type r4 = h.result();
    BOOST_TEST( r3 != r4 );
}

int main()
{
    test<boost::hash2::fnv1a_32>();
    test<boost::hash2::fnv1a_64>();
    test<boost::hash2::murmur3_32>();
    test<boost::hash2::murmur3_128>();
    test<boost::hash2::xxhash_32>();
    test<boost::hash2::xxhash_64>();
    test<boost::hash2::spooky2_128>();
    test<boost::hash2::siphash_32>();
    test<boost::hash2::siphash_64>();
    test<boost::hash2::md5_128>();
    test<boost::hash2::sha1_160>();
    test<boost::hash2::hmac_md5_128>();
    test<boost::hash2::hmac_sha1_160>();

    return boost::report_errors();
}


text:
/* CirKit: A circuit toolkit
 * Copyright (C) 2009-2015  University of Bremen
 * Copyright (C) 2015-2017  EPFL
 *
 * 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 "demo.hpp"

#include <iostream>

#include <boost/format.hpp>

#include <classical/xmg/xmg.hpp>    /* XMG data structure */

namespace cirkit
{

/******************************************************************************
 * XMG demo                                                                   *
 ******************************************************************************/

void xmg_demo( bool verbose )
{
  /* create a new empty XMG */
  xmg_graph xmg;

  /* the plan is to create a full adder, first we need three inputs */
  const auto a   = xmg.create_pi( "a" );
  const auto b   = xmg.create_pi( "b" );
  const auto cin = xmg.create_pi( "cin" );

  /* now we create the expressions */
  const auto sum  = xmg.create_xor( xmg.create_xor( a, b ), cin );
  const auto cout = xmg.create_maj( a, b, cin );

  /* sum and cout are values of type xmg_function. These represent pointers
   * to node with a complement flag.  We need to make them outputs of the
   * XMG explicitly
   */
  xmg.create_po( sum, "sum" );
  xmg.create_po( cout, "cout" );

  /* the XMG should now have 7 nodes, which are 3 inputs, 1 MAJ gate, 2 XOR gates,
   * and also one constant 0 node, which is always there, even if we don't use it;
   * let's print some statistics
   */
  if ( verbose )
  {
    std::cout << "[i] number of nodes:   " << xmg.size() << std::endl
              << "[i] number of inputs:  " << xmg.inputs().size() << std::endl
              << "[i] number of outputs: " << xmg.outputs().size() << std::endl
              << "[i] number of MAJ:     " << xmg.num_maj() << std::endl
              << "[i] number of XOR:     " << xmg.num_xor() << std::endl
              << "[i] number of gates:   " << xmg.num_gates() << std::endl;
  }

  /* let's now print all the input and output names */
  if ( verbose )
  {
    /* xmg.inputs() returns pairs of input nodes (xmg_node) and names
     */
    std::cout << "[i] input names:      ";
    for ( const auto& input : xmg.inputs() )
    {
      std::cout << " " << input.second;
    }
    std::cout << std::endl;

    /* xmg.outputs() returns pairs of output pointers (xmg_function) and names
     */
    std::cout << "[i] output names:     ";
    for ( const auto& output : xmg.outputs() )
    {
      std::cout << " " << output.second;
    }
    std::cout << std::endl;
  }

  /* next we compute the level of each node.  This information is not directly
   * available, and we first must compute it with `compute_levels', afterwards
   * we can access it with `level'.
   */
  xmg.compute_levels();
  if ( verbose )
  {
    for ( auto node : xmg.nodes() )
    {
      std::cout << boost::format( "[i] node %d is at level %d" ) % node % xmg.level( node ) << std::endl;
    }
  }
}

/******************************************************************************
 * implementation of the command                                              *
 ******************************************************************************/

demo_command::demo_command( const environment::ptr& env )
  : cirkit_command( env, "Demonstrate some of the API" )
{
  /* Options can be added one by one using opts.add_options()
   * There are in principle 3 types of options:
   * - Boolean flags:
   *
   *   ( "long_name,f", "description")
   *
   *   Here long_name is passed as --long_name to the command
   *   the flag f is a shorthand one character alternative passed
   *   as -f . The flag is optional. You can check whether in
   *   option has been set in execute() function using
   *   is_set( "long_name" )
   *
   * - Values without default value:
   *
   *   ( "long_name,f", value( &variable ), "description" )
   *
   *   Here variable is a private parameter in the demo_command
   *   class. It can be any string representable type.  This
   *   interface is based on Boost.ProgramOptions. Check their
   *   documentation for details.  You can use the variable then
   *   directly in the execute() function, but you should check whether
   *   it has been set using is_set( "long_name" ) . In order to
   *   use value, you add these two lines to the top of the file:
   *
   *   #include <boost/program_options.hpp>
   *   using boost::program_options::value;
   *
   * - Values with default value:
   *
   *   ( "long_name,f", value_with_default( &variable ), "description" )
   *
   *   Like the case without default value, but make sure that your
   *   variable is assigned a default value when being declared in the
   *   class. Also add these two lines:
   *
   *   #include <core/utils/program_options.hpp>
   *
   *   And you don't need to check whether the variable is set before
   *   using it.
   */
  opts.add_options()
    ( "xmg,x", "demonstration of XMG features" )
    ;

  /* this adds a verbose option to the command */
  be_verbose();
}

bool demo_command::execute()
{
  if ( is_set( "xmg" ) )
  {
    xmg_demo( is_verbose() );
  }

  /* command should always return true, false leads to program termination */
  return true;
}

}

// Local Variables:
// c-basic-offset: 2
// eval: (c-set-offset 'substatement-open 0)
// eval: (c-set-offset 'innamespace 0)
// End:

max_stars_repo_name: eletesta/cirkit

max_issues_repo_name: eletesta/cirkit

max_forks_repo_name: eletesta/cirkit


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

#ifndef KAGOME_BLOCK_TREE_HPP
#define KAGOME_BLOCK_TREE_HPP

#include <cstdint>
#include <vector>

#include <boost/optional.hpp>
#include <outcome/outcome.hpp>
#include "primitives/block.hpp"
#include "primitives/block_id.hpp"
#include "primitives/common.hpp"
#include "primitives/justification.hpp"

namespace kagome::blockchain {
  /**
   * Storage for blocks, which has a form of tree; it serves two functions:
   *    - keep tracking of all finalized blocks (they are kept in the
   *      non-volatile storage)
   *    - work with blocks, which participate in the current round of BABE block
   *      production (handling forks, pruning the blocks, resolving child-parent
   *      relations, etc)
   */
  struct BlockTree {
    using BlockHashVecRes = outcome::result<std::vector<primitives::BlockHash>>;

    virtual ~BlockTree() = default;

    /**
     * Get block header by provided block id
     * @param block id of the block header we are looking for
     * @return result containing block header if it exists, error otherwise
     */
    virtual outcome::result<primitives::BlockHeader> getBlockHeader(
        const primitives::BlockId &block) const = 0;

    /**
     * Get a body (extrinsics) of the block (if present)
     * @param block - id of the block to get body for
     * @return body, if the block exists in our storage, error in case it does
     * not exist in our storage, or actual error happens
     */
    virtual outcome::result<primitives::BlockBody> getBlockBody(
        const primitives::BlockId &block) const = 0;

    /**
     * Get a justification of the block (if present)
     * @param block - id of the block to get justification for
     * @return body, if the block exists in our storage, error in case it does
     * not exist in our storage, or actual error happens
     */
    virtual outcome::result<primitives::Justification> getBlockJustification(
        const primitives::BlockId &block) const = 0;

    /**
     * Adds header to the storage
     * @param header that we are adding
     * @return result with success if header's parent exists on storage and new
     * header was added. Error otherwise
     */
    virtual outcome::result<void> addBlockHeader(
        const primitives::BlockHeader &header) = 0;

    /**
     * Adds block body to the storage
     * @param block_number that corresponds to the block which body we are
     * adding
     * @param block_hash that corresponds to the block which body we are adding
     * @param block_body that we are adding
     * @return result with success if block body was inserted. Error otherwise
     */
    virtual outcome::result<void> addBlockBody(
        primitives::BlockNumber block_number,
        const primitives::BlockHash &block_hash,
        const primitives::BlockBody &block_body) = 0;

    /**
     * Add a new block to the tree
     * @param block to be added
     * @return nothing or error; if error happens, no changes in the tree are
     * made
     *
     * @note if block, which is specified in PARENT_HASH field of (\param block)
     * is not in our local storage, corresponding error is returned. It is
     * suggested that after getting that error, the caller would ask another
     * peer for the parent block and try to insert it; this operation is to be
     * repeated until a successful insertion happens
     */
    virtual outcome::result<void> addBlock(const primitives::Block &block) = 0;

    /**
     * Mark the block as finalized and store a finalization justification
     * @param block to be finalized
     * @param justification of the finalization
     * @return nothing or error
     */
    virtual outcome::result<void> finalize(
        const primitives::BlockHash &block,
        const primitives::Justification &justification) = 0;

    /**
     * Get a chain of blocks from the specified as a (\param block) up to the
     * closest finalized one
     * @param block to get a chain from
     * @return chain of blocks in top-to-bottom order (from the last finalized
     * block to the provided one) or error
     */
    virtual BlockHashVecRes getChainByBlock(
        const primitives::BlockHash &block) = 0;

    /**
     * Get a chain of blocks from the (\param block)
     * @param block, from which the chain is started
     * @param ascending - if true, the chain will grow up from the provided
     * block (it is the lowest one); if false, down
     * @param maximum number of blocks to be retrieved
     * @return chain or blocks or error
     */
    virtual BlockHashVecRes getChainByBlock(const primitives::BlockHash &block,
                                            bool ascending,
                                            uint64_t maximum) = 0;

    /**
     * Get a chain of blocks
     * @param top_block - block, which is at the top of the chain
     * @param bottom_block - block, which is the bottom of the chain
     * @return chain of blocks in top-to-bottom order or error
     */
    virtual BlockHashVecRes getChainByBlocks(
        const primitives::BlockHash &top_block,
        const primitives::BlockHash &bottom_block) = 0;

    /**
     * Check if one block is ancestor of second one (direct chain exists)
     * @param ancestor - block, which is at the top of the chain
     * @param descendant - block, which is the bottom of the chain
     * @return true if \param ancestor is ancestor of \param descendant
     */
    virtual bool hasDirectChain(const primitives::BlockHash &ancestor,
                                const primitives::BlockHash &descendant) = 0;

    /**
     * Get a longest path (chain of blocks) from the last finalized block down
     * to the deepest leaf
     * @return chain of blocks or error
     *
     * @note this function is equivalent to "getChainByBlock(deepestLeaf())"
     */
    virtual BlockHashVecRes longestPath() = 0;

    /**
     * Get a deepest leaf of the tree
     * @return deepest leaf
     *
     * @note deepest leaf is also a result of "SelectBestChain": if we are the
     * leader, we connect a block, which we constructed, to that deepest leaf
     */
    virtual primitives::BlockInfo deepestLeaf() const = 0;

    /**
     * @brief Get the most recent block of the best (longest) chain among those
     * that contain a block with \param target_hash
     * @param target_hash is a hash of a block that the chosen chain must
     * contain
     * @param max_number is the max block number that the resulting block (and
     * the target one) may possess
     */
    virtual outcome::result<primitives::BlockInfo> getBestContaining(
        const primitives::BlockHash &target_hash,
        const boost::optional<primitives::BlockNumber> &max_number) const = 0;

    /**
     * Get all leaves of our tree
     * @return collection of the leaves
     */
    virtual std::vector<primitives::BlockHash> getLeaves() const = 0;

    /**
     * Get children of the block with specified hash
     * @param block to get children of
     * @return collection of children hashes or error
     */
    virtual BlockHashVecRes getChildren(const primitives::BlockHash &block) = 0;

    /**
     * Get the last finalized block
     * @return hash of the block
     */
    virtual primitives::BlockInfo getLastFinalized() const = 0;
  };
}  // namespace kagome::blockchain

#endif  // KAGOME_BLOCK_TREE_HPP

ext: hpp

max_stars_repo_name: FlorianFranzen/kagome

max_issues_repo_name: FlorianFranzen/kagome

max_forks_repo_name: FlorianFranzen/kagome


text:
///////////////////////////////////////////////////////////////////////////////
// repeat_end_matcher.hpp
//
//  Copyright 2008 Eric Niebler. 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 BOOST_XPRESSIVE_DETAIL_CORE_MATCHER_REPEAT_END_MATCHER_HPP_EAN_10_04_2005
#define BOOST_XPRESSIVE_DETAIL_CORE_MATCHER_REPEAT_END_MATCHER_HPP_EAN_10_04_2005

// MS compatible compilers support #pragma once
#if defined(_MSC_VER) && (_MSC_VER >= 1020)
# pragma once
#endif

#include <boost/mpl/bool.hpp>
#include <boost/xpressive/detail/detail_fwd.hpp>
#include <boost/xpressive/detail/core/quant_style.hpp>
#include <boost/xpressive/detail/core/state.hpp>

namespace boost { namespace xpressive { namespace detail
{

    ///////////////////////////////////////////////////////////////////////////////
    // repeat_end_matcher
    //
    template<typename Greedy>
    struct repeat_end_matcher
      : quant_style<quant_none, 0, false>
    {
        typedef Greedy greedy_type;
        int mark_number_;
        unsigned int min_, max_;
        mutable void const *back_;

        repeat_end_matcher(int mark_number, unsigned int min, unsigned int max)
          : mark_number_(mark_number)
          , min_(min)
          , max_(max)
          , back_(0)
        {
        }

        template<typename BidiIter, typename Next>
        bool match(match_state<BidiIter> &state, Next const &next) const
        {
            // prevent repeated zero-width sub-matches from causing infinite recursion
            sub_match_impl<BidiIter> &br = state.sub_match(this->mark_number_);

            if(br.zero_width_ && br.begin_ == state.cur_)
            {
                return next.skip_match(state);
            }

            bool old_zero_width = br.zero_width_;
            br.zero_width_ = (br.begin_ == state.cur_);

            if(this->match_(state, next, greedy_type()))
            {
                return true;
            }

            br.zero_width_ = old_zero_width;
            return false;
        }

        // greedy, variable-width quantifier
        template<typename BidiIter, typename Next>
        bool match_(match_state<BidiIter> &state, Next const &next, mpl::true_) const
        {
            sub_match_impl<BidiIter> &br = state.sub_match(this->mark_number_);

            if(this->max_ > br.repeat_count_)
            {
                ++br.repeat_count_;
                // loop back to the expression "pushed" in repeat_begin_matcher::match
                if(next.top_match(state, this->back_))
                {
                    return true;
                }
                else if(--br.repeat_count_ < this->min_)
                {
                    return false;
                }
            }

            // looping finished, continue matching the rest of the pattern
            return next.skip_match(state);
        }

        // non-greedy, variable-width quantifier
        template<typename BidiIter, typename Next>
        bool match_(match_state<BidiIter> &state, Next const &next, mpl::false_) const
        {
            sub_match_impl<BidiIter> &br = state.sub_match(this->mark_number_);

            if(this->min_ <= br.repeat_count_)
            {
                if(next.skip_match(state))
                {
                    return true;
                }
            }

            if(this->max_ > br.repeat_count_)
            {
                ++br.repeat_count_;
                if(next.top_match(state, this->back_))
                {
                    return true;
                }
                --br.repeat_count_;
            }

            return false;
        }
    };

}}}

#endif


text:
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2019, WVU Interactive Robotics Laboratory
*                       https://web.statler.wvu.edu/~irl/
* 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 Willow Garage 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.
*********************************************************************/

#include <stdio.h>
#include <math.h>
#include <boost/array.hpp>
#include <ros/ros.h>
#include <ros/time.h>
#include <vicon_bridge/Markers.h>
#include <vicon_bridge/Marker.h>
#include <geometry_msgs/TransformStamped.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_ros/buffer_interface.h>
#include <tf2/convert.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>
#include <wvu_swarm_std_msgs/vicon_bot.h>
#include <wvu_swarm_std_msgs/vicon_bot_array.h>
#include <wvu_swarm_std_msgs/robot_command.h>
#include <wvu_swarm_std_msgs/robot_command_array.h>
#include "robot_id.h"
#include <thread>

#define DEBUG 0
#include <iostream>

bool g_charging = false;

std::thread cinLooper;

void cinLoop(void)
{
    while(true){
        std::cin.clear();
        std::cin >> g_charging;
        std::cout << g_charging << std::endl;
    }
}

void msgCallback(const wvu_swarm_std_msgs::vicon_bot_array &msg) {}

// Method to find distance between two points
double getDist(const geometry_msgs::Point first, const geometry_msgs::Point second);

// Method to populate a vector with a Gerono Lemniscate
//   Params: a vector to fill with the points, a value 'a' defining the curve,
//   and a value for an increment in degrees to iterate through 360 degrees
void genGeronoLemn(std::vector<geometry_msgs::Point> &target, const double a, const double incr);

void butterflyLemn(std::vector<geometry_msgs::Point> &target, const double a, const double incr);

// Method to process a bot's ideal vector
//   Params: a vector to add the bot's command to, the bot to use, the point to
//   compare the bot against.
//   Return: the distance of the bot
double processBot(wvu_swarm_std_msgs::robot_command_array &outputMsg, std::string tfPrefix,
        const wvu_swarm_std_msgs::vicon_bot bot, const geometry_msgs::Point point, tf2_ros::Buffer &tfBuff);

int main(int argc, char **argv)
{
    // Start a cin thread for charge switching
    cinLooper = std::thread(cinLoop);
    bool prevCharge = false;

    // Initialize a node
    ros::init(argc, argv, "tracker");

    // Generates nodehandle, publisher, subscribers
    ros::NodeHandle n;
    ros::NodeHandle n_priv("~"); // private handle
    ros::Publisher pub;
    ros::Subscriber sub;

    //  Where to get the array of bots, what topic to advertise, where to get transform of target
    std::string viconArrayTopic, advertiseTopic, transformPrefix;

    //  Value of 'a' in formula in cm, value to iterate degrees, distance to "find" points at
    int lemniscateConstant, lemniscateInterval, cutoffRadius;

    // Import parameters from launchfile
    n_priv.param<std::string>("vicon_array_topic", viconArrayTopic, "/viconArray");
    n_priv.param<std::string>("advertise_topic", advertiseTopic, "execute");
    n_priv.param<std::string>("transform_prefix", transformPrefix, "cardbot_");
    n_priv.param<int>("lemniscate_constant", lemniscateConstant, 50);
    n_priv.param<int>("lemniscate_interval", lemniscateInterval, 10);
    n_priv.param<int>("cutoff_radius", cutoffRadius, 10);

    // Sets loop rate at 100Hz
    ros::Rate rate(10);

    // Subscribe to tracker's vicon topic, advertise result vector
    sub = n.subscribe(viconArrayTopic, 10, &msgCallback);
    pub = n.advertise<wvu_swarm_std_msgs::robot_command_array>(advertiseTopic, 1000);

    // Set up transform buffer
    tf2_ros::Buffer tfBuffer;

    // Set up transform listener
    tf2_ros::TransformListener tfListener(tfBuffer);

    // Generate a Gerono Lemniscate (figure 8) as discrete points
    std::vector<geometry_msgs::Point> path;
    genGeronoLemn(path, lemniscateConstant, lemniscateInterval);

    // Pick one point out of the path
    geometry_msgs::Point currentPoint = path.at(0);
    int currentIndex = 0;
    ROS_INFO("Point initialized at %f, %f\n", currentPoint.x, currentPoint.y);

    // Get the information of the vicon array
    wvu_swarm_std_msgs::vicon_bot_array viconArray =
            *(ros::topic::waitForMessage<wvu_swarm_std_msgs::vicon_bot_array>(viconArrayTopic));

    // Generate a vector of robot commands to publish
    wvu_swarm_std_msgs::robot_command_array output;

    double minimumPointDistance = 10000.0; // Trivially large number, in cm

    // Run while ros functions
    while(ros::ok())
    {
        output.commands.clear();
        viconArray = *(ros::topic::waitForMessage<wvu_swarm_std_msgs::vicon_bot_array>(viconArrayTopic));
        minimumPointDistance = 10000.0;

        // Iterate through all bots
        for(wvu_swarm_std_msgs::vicon_bot iteratorBot : viconArray.poseVect)
        {
            // Process bot's distance from the point and add its command vector to the output
            double thisDist = processBot(output, transformPrefix, iteratorBot, currentPoint, tfBuffer);

            // Change minimum if necessary
            if(thisDist < minimumPointDistance) minimumPointDistance = thisDist;
        }

        // If charging now, move the point
        if(g_charging && !prevCharge) {
            prevCharge = true;

            geometry_msgs::Point pt;
            pt.x = -50;
            pt.y = 0;
            pt.z = 0;
            currentPoint = pt;

            ROS_INFO("Point moved to %f, %f\n", currentPoint.x, currentPoint.y);
        }
        // If no longer charging, go back to path
        else if(!g_charging && prevCharge) {
            prevCharge = false;

            currentPoint = path.at(currentIndex);

            ROS_INFO("Point moved to %f, %f\n", currentPoint.x, currentPoint.y);
        }
        // Move the point if any bot has come too close
        else if(!g_charging && minimumPointDistance < cutoffRadius) {
            // Rotate through vector if needed
            if(currentIndex + 1 >= path.size())
                currentIndex = 0;
            else
                currentIndex++;

            currentPoint = path.at(currentIndex);

            ROS_INFO("Point moved to %f, %f\n", currentPoint.x, currentPoint.y);
        }

        // Publish vector of robot commands
        pub.publish(output);

        ros::spinOnce();
        rate.sleep();
    }

    cinLooper.join();
}

double getDist(const geometry_msgs::Point first, const geometry_msgs::Point second)
{
    double xDiff = first.x - second.x;
    double yDiff = first.y - second.y;
    double zDiff = first.z - second.z;

    return sqrt(xDiff*xDiff + yDiff*yDiff + zDiff*zDiff);
}

void genGeronoLemn(std::vector<geometry_msgs::Point> &target, const double a, const double incr)
{
    // Iterate across 360 degrees
    for(int t = 0; t < 360; t += incr)
    {
        // Create a point
        geometry_msgs::Point temp;

        double rads = t * 3.14156265 / 180;

        // Do math to find its location
        //   This lemniscate is rotated 90 deg
        temp.x = a * cos(rads) * sin(rads);
        temp.y = a * sin(rads);
        temp.z = 0; //flatworld

        // Add to vector
        target.push_back(temp);

        ROS_INFO("Added point at %d degrees, or %f rads, at %f, %f, 0\n", t, rads, temp.x, temp.y);
    }
}

void butterflyLemn(std::vector<geometry_msgs::Point> &target, const double a, const double incr)
{
    // Iterate across 360 degrees
    for(int t = 0; t < 360; t += incr)
    {
        // Create a point
        geometry_msgs::Point temp;

        double rads = t * 3.14156265 / 180;

        // Do math to find its location
        //   This lemniscate is rotated 90 deg
        temp.x = a/3.0 * sin(rads) * (exp(cos(rads))-2*cos(4*rads)-(pow(sin(rads/12),5)));
        temp.y = a/3.0 * cos(rads) * (exp(cos(rads))-2*cos(4*rads)-(pow(sin(5/12),5)));
        temp.z = 0; //flatworld

        // Add to vector
        target.push_back(temp);

        ROS_INFO("Added point at %d degrees, or %f rads, at %f, %f, 0\n", t, rads, temp.x, temp.y);
    }
}

double processBot(wvu_swarm_std_msgs::robot_command_array &outputMsg, std::string tfPrefix,
        const wvu_swarm_std_msgs::vicon_bot bot, const geometry_msgs::Point point, tf2_ros::Buffer &tfBuff)
{
    // Generate a point for this bot's location
    geometry_msgs::Point botPt;
    botPt.x = bot.botPose.transform.translation.x;
    botPt.y = bot.botPose.transform.translation.y;
    botPt.z = 0;

    // Find distance between bot and point
    double dist = getDist(botPt, point);

    // Convert the bot's pose to a string
    char rid[3] = {'\0'};
    char rid2[3] = {'\0'};
    rid[0] = bot.botId[0];
    rid[1] = bot.botId[1];
    rid2[0] = bot.botId[0];
    rid2[1] = bot.botId[1];
    std::string idStr(rid);
    int idNum = rid_map[idStr];

#if DEBUG
    std::cout << "\033[32;1mGot id string: " << idStr << " converts to : " << rid_map[idStr] << "\033[0m" << std::endl;
#endif

    // Build a string for the robot's transformation
    std::string transformString = "vicon/";
    transformString += tfPrefix + idStr + "/" + tfPrefix + idStr;

    // Find transform from bot to target point
    geometry_msgs::TransformStamped trackerTarget;
    try{
        trackerTarget = tfBuff.lookupTransform(transformString,
                "world", ros::Time(0));
    } catch(tf2::LookupException) {
        ROS_ERROR("Can't find transform %s!", transformString.c_str());
        return 10000.0; // Arbitrarily large distance, cm
    }

    // Transform the point
    geometry_msgs::Point pointTransformed;
    tf2::doTransform<geometry_msgs::Point>(point, pointTransformed, trackerTarget);
    pointTransformed.z = 0; // why not

    // Find angle between bot and point
    double degrees = 0;
    if(pointTransformed.x < 0)
        degrees = atan(pointTransformed.y / pointTransformed.x) * 180 / 3.14159265;
    else
        degrees = (atan(pointTransformed.y / pointTransformed.x) * 180 / 3.14159265) + 180;
    degrees -= 180;
    if(degrees < 0) degrees += 360;

    // Build a command for this specific bot
    wvu_swarm_std_msgs::robot_command thisCmd;
    thisCmd.rid = idNum;
    thisCmd.r = dist;
    thisCmd.theta = degrees;

    // Add command to the vector
    outputMsg.commands.push_back(thisCmd);

    return dist;
}

max_stars_repo_name: wvu-irl/reu-swarm-ros

max_issues_repo_name: wvu-irl/reu-swarm-ros

max_forks_repo_name: wvu-irl/reu-swarm-ros


text:
/// @file   DomeAdapterDiskCatalog.cpp
/// @brief  Dome adapter catalog
/// @author Georgios Bitzes <georgios.bitzes@cern.ch>

#include <dmlite/cpp/dmlite.h>
#include <iostream>

#include "DomeAdapterDiskCatalog.h"
#include "DomeAdapter.h"
#include "utils/DomeTalker.h"
#include "utils/DomeUtils.h"
#include "DomeAdapterUtils.h"

#include <boost/property_tree/json_parser.hpp>
#include <boost/optional/optional.hpp>

using namespace dmlite;

DomeAdapterDiskCatalog::DomeAdapterDiskCatalog(DomeAdapterFactory *factory) throw (DmException) {
  factory_ = factory;
  sec_ = NULL;
}

std::string DomeAdapterDiskCatalog::getImplId() const throw() {
  return std::string("DomeAdapterDiskCatalog");
}

void DomeAdapterDiskCatalog::setStackInstance(StackInstance* si) throw (DmException)
{
  this->si_ = si;
}

void DomeAdapterDiskCatalog::setSecurityContext(const SecurityContext* secCtx) throw (DmException)
{
  this->sec_ = secCtx;
}

void DomeAdapterDiskCatalog::getChecksum(const std::string& path,
                                         const std::string& csumtype,
                                         std::string& csumvalue,
                                         const std::string& pfn,
                                         const bool forcerecalc,
                                         const int waitsecs) throw (DmException) {
  Log(Logger::Lvl3, domeadapterlogmask, domeadapterlogname, " Entering, path '"
                                                            << path << "', csumtype '"
                                                            << csumtype << "'");
  time_t start = time(0);
  bool recalc = forcerecalc;

  int waitsecs1 = waitsecs;
  if (waitsecs1 == 0) waitsecs1 = 1800;

  while(true) {
    DomeTalker talker(factory_->davixPool_, sec_, factory_->domehead_,
                      "GET", "dome_chksum");

    boost::property_tree::ptree params;
    params.put("checksum-type", csumtype);
    params.put("lfn", path);
    params.put("force-recalc", DomeUtils::bool_to_str(recalc));
    recalc = false; // no force-recalc in subsequent requests

    if(!talker.execute(params)) {
      throw DmException(EINVAL, talker.err());
    }

    // checksum calculation in progress
    if(talker.status() == 202) {
      if(time(0) - start >= waitsecs1)
        throw DmException(EAGAIN, SSTR(waitsecs << "s were not sufficient to checksum '" << csumtype << ":" << path << "'. Try again later."));
      sleep(5);
      continue;
    }

    try {
      csumvalue = talker.jresp().get<std::string>("checksum");
      return;
    }
    catch(boost::property_tree::ptree_error &e) {
      throw DmException(EINVAL, SSTR("Error when parsing json response: " << talker.response()));
    }
  }
}

Replica DomeAdapterDiskCatalog::getReplicaByRFN(const std::string& rfn) throw (DmException) {
  Log(Logger::Lvl4, domeadapterlogmask, domeadapterlogname, "rfn: " << rfn);
  DomeTalker talker(factory_->davixPool_, sec_, factory_->domehead_,
                    "GET", "dome_getreplicainfo");

  if(!talker.execute("rfn", rfn)) {
    throw DmException(talker.dmlite_code(), talker.err());
  }

  try {
    Replica replica;
    ptree_to_replica(talker.jresp(), replica);
    return replica;
  }
  catch(boost::property_tree::ptree_error &e) {
    throw DmException(EINVAL, SSTR("Error when parsing json response: '" << e.what() << "'. Contents: '" << talker.response() << "'"));
  }
}

// taken from built-in catalog and slightly modified
bool DomeAdapterDiskCatalog::accessReplica(const std::string& rfn, int mode) throw (DmException) {
  try {
    Replica      replica = this->getReplicaByRFN(rfn);
    // ExtendedStat xstat   = this->extendedStat(replica.fileid);

    bool replicaAllowed = true;
    mode_t perm = 0;

    if (mode & R_OK)
      perm  = S_IREAD;

    if (mode & W_OK) {
      perm |= S_IWRITE;
      replicaAllowed = (replica.status == Replica::kBeingPopulated);
    }

    if (mode & X_OK)
      perm |= S_IEXEC;

    // bool metaAllowed    = (checkPermissions(sec_, xstat.acl, xstat.stat, perm) == 0);
    return replicaAllowed;
  }
  catch (DmException& e) {
    if (e.code() != EACCES) throw;
    return false;
  }
}

ExtendedStat DomeAdapterDiskCatalog::extendedStat(const std::string& path, bool follow) throw (DmException) {
  Log(Logger::Lvl4, domeadapterlogmask, domeadapterlogname, "path: " << path << " follow (ignored) :" << follow);

  DomeTalker talker(factory_->davixPool_, sec_, factory_->domehead_,
                    "GET", "dome_getstatinfo");

  if(!talker.execute("lfn", path)) {
    throw DmException(talker.dmlite_code(), talker.err());
  }

  try {
    ExtendedStat xstat;
    ptree_to_xstat(talker.jresp(), xstat);
    return xstat;
  }
  catch(boost::property_tree::ptree_error &e) {
    throw DmException(EINVAL, SSTR("Error when parsing json response: " << talker.response()));
  }
}

ExtendedStat DomeAdapterDiskCatalog::extendedStatByRFN(const std::string& rfn)  throw (DmException) {
  Log(Logger::Lvl4, domeadapterlogmask, domeadapterlogname, "rfn: " << rfn );

  DomeTalker talker(factory_->davixPool_, sec_, factory_->domehead_,
                    "GET", "dome_getstatinfo");

  if(!talker.execute("rfn", rfn)) {
    throw DmException(talker.dmlite_code(), talker.err());
  }

  try {
    ExtendedStat xstat;
    ptree_to_xstat(talker.jresp(), xstat);
    return xstat;
  }
  catch(boost::property_tree::ptree_error &e) {
    throw DmException(EINVAL, SSTR("Error when parsing json response: " << talker.response()));
  }
}

Directory* DomeAdapterDiskCatalog::openDir(const std::string& path) throw (DmException) {
  Log(Logger::Lvl4, domeadapterlogmask, domeadapterlogname, "Entering. Path: " << path);
  using namespace boost::property_tree;
  Log(Logger::Lvl4, domeadapterlogmask, domeadapterlogname, "path: " << path);
  DomeTalker talker(factory_->davixPool_, sec_, factory_->domehead_,
                    "GET", "dome_getdir");

  ptree params;
  params.put("path", path);
  params.put("statentries", "true");

  if(!talker.execute(params)) {
    throw DmException(EINVAL, talker.err());
  }

  try {
    DomeDir *domedir = new DomeDir(path);

    ptree entries = talker.jresp().get_child("entries");
    for(ptree::const_iterator it = entries.begin(); it != entries.end(); it++) {
      ExtendedStat xstat;
      xstat.name = it->second.get<std::string>("name");

      Log(Logger::Lvl4, domeadapterlogmask, domeadapterlogname, "entry " << xstat.name);

      ptree_to_xstat(it->second, xstat);
      domedir->entries_.push_back(xstat);
    }
    return domedir;
  }
  catch(ptree_error &e) {
    throw DmException(EINVAL, SSTR("Error when parsing json response - " << e.what() << " : " << talker.response()));
  }
}

void DomeAdapterDiskCatalog::closeDir(Directory* dir) throw (DmException) {
  Log(Logger::Lvl4, domeadapterlogmask, domeadapterlogname, "Entering.");
  DomeDir *domedir = static_cast<DomeDir*>(dir);
  delete domedir;
}

ExtendedStat* DomeAdapterDiskCatalog::readDirx(Directory* dir) throw (DmException) {
  Log(Logger::Lvl4, domeadapterlogmask, domeadapterlogname, "Entering.");
  if (dir == NULL) {
    throw DmException(DMLITE_SYSERR(EFAULT), "Tried to read a null dir");
  }

  DomeDir *domedir = static_cast<DomeDir*>(dir);
  if(domedir->pos_ >= domedir->entries_.size()) {
    return NULL;
  }

  domedir->pos_++;
  return &domedir->entries_[domedir->pos_ - 1];
}

void DomeAdapterDiskCatalog::updateExtendedAttributes(const std::string& lfn,
                                                  const Extensible& ext) throw (DmException) {
  Log(Logger::Lvl4, domeadapterlogmask, domeadapterlogname, "Entering.");

  DomeTalker talker(factory_->davixPool_, sec_, factory_->domehead_,
                    "POST", "dome_updatexattr");

  if(!talker.execute("lfn", lfn, "xattr", ext.serialize())) {
    throw DmException(EINVAL, talker.err());
  }
}

max_stars_repo_name: EricKTCheung/dmlite

max_issues_repo_name: EricKTCheung/dmlite

max_forks_repo_name: EricKTCheung/dmlite


text:
#include <boost/program_options.hpp>
#include <iostream>
#include <memory>

#include <application/application.hpp>
#include <conf_manager/configuration_manager.hpp>
#include <logger/logger.hpp>
#include <logger/logger_exception.hpp>
#include <common/utility.hpp>

namespace po = boost::program_options;

int main(int argc, char* argv[])
{
    po::options_description opts("service options");

    opts.add_options()
            ("help", "produce a help message")
            ("console", "run on console")
            ("daemon", "run as a service");

    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, opts), vm);

    if (vm.count("help") || argc == 1)
    {
        std::cout << opts << std::endl;
        return 0;
    }

    try
    {
        utility::initialization_config("/etc/http-server/http-server.yaml");
        utility::initialization_logger(configuration_manager::instance_ptr()->get_config()->paths_.log_.logfile_);
    }
    catch(logger_exception &ex)
    {
        std::cout << ex.what() << std::endl;
        exit(EXIT_FAILURE);
    }
    catch(YAML::Exception &ex)
    {
        std::cout << "configuration manager: " << ex.what() << std::endl;
        exit(EXIT_FAILURE);
    }

    std::unique_ptr<application> app = std::make_unique<application>();

    if (vm.count("console"))
    {
        app->run();
    }
    else if (vm.count("daemon"))
    {
        app->start_background();
    }

    return 0;
}

max_stars_repo_name: pashinov/http-server

max_issues_repo_name: pashinov/http-server

max_forks_repo_name: pashinov/server


text:
/*
 (c) 2014 Glen Joseph Fernandes
 glenjofe at gmail dot com

 Distributed under the Boost Software
 License, Version 1.0.
 http://boost.org/LICENSE_1_0.txt
*/
#ifndef BOOST_ALIGN_DETAIL_ALIGNMENT_OF_MSVC_HPP
#define BOOST_ALIGN_DETAIL_ALIGNMENT_OF_MSVC_HPP

#include <boost/align/detail/min_size.hpp>
#include <boost/align/detail/offset_object.hpp>

namespace boost {
    namespace alignment {
        namespace detail {
            template<class T>
            struct alignment_of
                : min_size<sizeof(T),
                    offsetof(offset_object<T>, object)>::type {
            };
        }
    }
}

#endif

max_stars_repo_name: multi-os-engine/cinder-natj-binding

max_issues_repo_name: multi-os-engine/cinder-natj-binding

max_forks_repo_name: multi-os-engine/cinder-natj-binding


text:
/*
 * Copyright (c) 2010, Anima Games, Benjamin Karaban, Laurent Schneider,
 * Jérémie Comarmond, Didier Colin.
 * 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 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 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.
 */
#include "stdafx.h"

#include "EnumGenerator.h"
#include <iomanip>

#include <vector>
#include <fstream>
#include <iostream>
#include <sstream>
#include <boost/current_function.hpp>
#include <string>
#include <ctime>

static int nbSpace = 200;
static char Tag [] = "    ";

void writeHeader(std::ofstream& file)
{

	file<<"typedef std::string String_t;\n\n";
	file<<"#define CASE_ENUM_TO_STRING(str, value)"<<std::setw(nbSpace)<<"\\\n"<<Tag<<Tag<<"case value:"<<std::setw(nbSpace)<<"\\\n"<<Tag<<Tag<<Tag<<"str = #value;"<<std::setw(nbSpace)<<"\\\n"<<Tag<<Tag<<Tag<<"break;\n\n\n";
}

void writeEnum(std::ofstream& file, const int nbParam)
{
	int ii;

/////////////////////////////
//le define
/////////////////////////////
	file<<"#define LM_ENUM_"<<nbParam<<"(name, ";
	for (ii=0; ii<nbParam-1; ++ii)
		file<<"param_"<<ii<<", ";
	file<<"param_"<<ii<<')';
	file<<std::setw(nbSpace)<<"\\\n";

/////////////////////////////
//l'enum
/////////////////////////////
	file<<std::setw(nbSpace)<<"\\\n";
	file<<Tag<<"enum name {";
	for (ii=0; ii<nbParam-1; ++ii)
		file<<"param_"<<ii<<", ";
	file<<"param_"<<ii;
	file<<"};";

/////////////////////////////
//toString
/////////////////////////////
	file<<std::setw(nbSpace)<<"\\\n";
	file<<Tag<<"String_t toString(const name e)"<<std::setw(nbSpace)<<"\\\n";
	file<<Tag<<"{"<<std::setw(nbSpace)<<"\\\n";
	file<<Tag<<Tag<<"String_t str;"<<std::setw(nbSpace)<<"\\\n";
	file<<Tag<<Tag<<"switch(e)"<<std::setw(nbSpace)<<"\\\n";
	file<<Tag<<Tag<<"{"<<std::setw(nbSpace)<<"\\\n";
	for (ii=0; ii<nbParam; ++ii)
	{
		file<<Tag<<Tag<<Tag<<"CASE_ENUM_TO_STRING(str, "<<"param_"<<ii<<");"<<std::setw(nbSpace)<<"\\\n";
	}
	file<<Tag<<Tag<<"}"<<std::setw(nbSpace)<<"\\\n";
	file<<Tag<<Tag<<"return str;"<<std::setw(nbSpace)<<"\\\n";
	file<<Tag<<"}"<<std::setw(nbSpace)<<"\\\n";

/////////////////////////////
//isValid
/////////////////////////////
	file<<Tag<<"bool isValid(const name e)"<<std::setw(nbSpace)<<"\\\n";
	file<<Tag<<"{"<<std::setw(nbSpace)<<"\\\n";
	file<<Tag<<Tag<<"bool result = false;"<<std::setw(nbSpace)<<"\\\n";
    file<<Tag<<Tag<<"switch(e)"<<std::setw(nbSpace)<<"\\\n";
	file<<Tag<<Tag<<"{"<<std::setw(nbSpace)<<"\\\n";
	for (ii=0; ii<nbParam; ++ii)
	{
		file<<Tag<<Tag<<Tag<<"case "<<"param_"<<ii<<":"<<std::setw(nbSpace)<<"\\\n";
    }
	file<<Tag<<Tag<<Tag<<Tag<<"result = true;"<<std::setw(nbSpace)<<"\\\n";
	file<<Tag<<Tag<<"}"<<std::setw(nbSpace)<<std::setw(nbSpace)<<"\\\n";
    file<<Tag<<Tag<<"return result;"<<std::setw(nbSpace)<<"\\\n";
	file<<Tag<<"}"<<std::setw(nbSpace)<<"\\\n";
/////////////////////////////
//Count
/////////////////////////////
	file<<Tag<<"const int32 "<<"name"<<"##_COUNT"<<" = "<<nbParam<<";";
/////////////////////////////
//fin
/////////////////////////////
	file<<"\n\n\n";
}

void GenerateEnum(std::string fileName)
{
	const int nbMaxEnum = 50;

	std::ofstream file ( fileName.c_str(), std::ios_base::beg );

	writeHeader(file);

	for (int idEnum=1; idEnum<nbMaxEnum; ++idEnum)
		writeEnum(file, idEnum);
}

max_stars_repo_name: benkaraban/anima-games-engine

max_issues_repo_name: benkaraban/anima-games-engine

max_forks_repo_name: benkaraban/anima-games-engine


text:
#include <Common/ZooKeeper/ZooKeeper.h>
#include <IO/ReadHelpers.h>
#include <IO/ReadBufferFromFileDescriptor.h>
#include <boost/program_options.hpp>

int main(int argc, char ** argv)
try
{
    boost::program_options::options_description desc("Allowed options");
    desc.add_options()
        ("help,h", "produce help message")
        ("address,a", boost::program_options::value<std::string>()->required(),
        "addresses of ZooKeeper instances, comma separated. Example: example01e.yandex.ru:2181")
        ;

    boost::program_options::variables_map options;
    boost::program_options::store(boost::program_options::parse_command_line(argc, argv, desc), options);

    if (options.count("help"))
    {
        std::cout << "Remove nodes in ZooKeeper by list passed in stdin." << std::endl;
        std::cout << "Usage: " << argv[0] << " [options] < list_of_nodes_on_each_line" << std::endl;
        std::cout << desc << std::endl;
        return 1;
    }

    zkutil::ZooKeeper zookeeper(options.at("address").as<std::string>());

    DB::ReadBufferFromFileDescriptor in(STDIN_FILENO);
    std::list<zkutil::ZooKeeper::RemoveFuture> futures;

    std::cerr << "Requested: ";
    while (!in.eof())
    {
        std::string path;
        DB::readEscapedString(path, in);
        DB::assertString("\n", in);
        futures.push_back(zookeeper.asyncRemove(path));
        std::cerr << ".";
    }
    std::cerr << "\n";

    std::cerr << "Done: ";
    for (auto & future : futures)
    {
        try
        {
            future.get();
        }
        catch (...)
        {
            std::cerr << DB::getCurrentExceptionMessage(true) << '\n';
        }
        std::cerr << ".";
    }
    std::cerr << "\n";

    return 0;
}
catch (const Poco::Exception & e)
{
    std::cerr << DB::getCurrentExceptionMessage(true) << '\n';
    throw;
}

max_stars_repo_name: ArielLily/ClickHouse

max_issues_repo_name: ArielLily/ClickHouse

max_forks_repo_name: ArielLily/ClickHouse


text:
/**************************************************************
 *
 * Licensed to the Apache Software Foundation (ASF) under one
 * or more contributor license agreements.  See the NOTICE file
 * distributed with this work for additional information
 * regarding copyright ownership.  The ASF licenses this file
 * to you 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.
 *
 *************************************************************/

#ifndef INCLUDED_SLIDESHOW_SHAPELISTENEREVENTHANDLER_HXX
#define INCLUDED_SLIDESHOW_SHAPELISTENEREVENTHANDLER_HXX

#include <com/sun/star/uno/Reference.hxx>
#include <boost/shared_ptr.hpp>

namespace com { namespace sun { namespace star { namespace drawing
{
    class XShape;
} namespace presentation {
    class XShapeEventListener;
} } } }

/* Definition of ShapeListenerEventHandler interface */

namespace slideshow
{
    namespace internal
    {

        /** Interface for handling view events.

        	Classes implementing this interface can be added to an
        	EventMultiplexer object, and are called from there to
        	handle view events.
         */
        class ShapeListenerEventHandler
        {
        public:
            virtual ~ShapeListenerEventHandler() {}

            virtual bool listenerAdded( const ::com::sun::star::uno::Reference<
                                           ::com::sun::star::presentation::XShapeEventListener>& xListener,
                                        const ::com::sun::star::uno::Reference<
                                           ::com::sun::star::drawing::XShape>&                   xShape ) = 0;

            virtual bool listenerRemoved( const ::com::sun::star::uno::Reference<
                                             ::com::sun::star::presentation::XShapeEventListener>& xListener,
                                          const ::com::sun::star::uno::Reference<
                                             ::com::sun::star::drawing::XShape>&                   xShape ) = 0;
        };

        typedef ::boost::shared_ptr< ShapeListenerEventHandler > ShapeListenerEventHandlerSharedPtr;

    }
}

#endif /* INCLUDED_SLIDESHOW_SHAPELISTENEREVENTHANDLER_HXX */

ext: hxx

max_stars_repo_name: Grosskopf/openoffice

max_issues_repo_name: Grosskopf/openoffice

max_forks_repo_name: Grosskopf/openoffice


text:
#include <testsuite.h>
#include <blitz/array.h>

BZ_USING_NAMESPACE(blitz)

// test that stencil extent detection works

BZ_DECLARE_STENCIL2(test, P1, P2)
P2 = forward11_stencilop(P1,0)+forward21_stencilop(P1,1);
BZ_END_STENCIL

typedef Array<double,2> array2d;

int main()
{
const int N=5;
array2d A(N,N);
array2d B(N,N);
// Fill a three-dimensional array with a Gaussian function
firstIndex i;
secondIndex j;
thirdIndex k;
float midpoint = 3.;
float c = - 1.;

 A = exp(c * (sqr(i-midpoint) + sqr(j-midpoint)));
 B = 42;
 //cout << A << endl;
 //cout << B << endl;

applyStencil(test(),A,B);

 BZTEST(B(0,0)!=42);
 BZTEST(B(4,1)==42);
 BZTEST(B(2,2)!=42);
 BZTEST(B(1,3)==42);
 BZTEST(B(1,4)==42);
 //cout << B << endl;

}

max_stars_repo_name: fraguela/depspawn

max_issues_repo_name: MSV-Project/IBAMR

max_forks_repo_name: MSV-Project/IBAMR


text:
// Copyright 2015 by BBN Technologies Corp.
// All Rights Reserved.

#include "Generic/common/leak_detection.h" // This must be the first #include
#include "Generic/common/FeatureModule.h"
#include "Generic/common/ParamReader.h"
#include "Generic/test/UnitTester.h"
#include <boost/test/unit_test.hpp>

boost::unit_test::test_suite* init_unit_test_suite(int argc, char* argv[]) {
	if (argc < 2) {
		std::cerr << "Error: expected a parameter file" << std::endl;
		throw new std::exception();
	}

	try {
		ParamReader::readParamFile(argv[1]);
		FeatureModule::load();
	} catch (UnexpectedInputException &e) {
		std::cerr << e.getMessage() << std::endl;
		throw e;
	}

	UnitTester *unitTester = UnitTester::build();
	return unitTester->initUnitTests();
}

max_stars_repo_name: BBN-E/serif

max_issues_repo_name: BBN-E/serif

max_forks_repo_name: BBN-E/serif


text:
/*
* drakeUtil.cpp
 *
 *  Created on: Jun 19, 2013
 *      Author: russt
 */

#include "drakeUtil.h"
#include <string>
#include <math.h>
#include <limits>
#include <Eigen/Dense>
#include <stdexcept>
#include <algorithm>
#include <functional>

using namespace std;
using namespace Eigen;

void baseZeroToBaseOne(std::vector<int>& vec)
{
  for (std::vector<int>::iterator iter=vec.begin(); iter!=vec.end(); iter++)
    (*iter)++;
}

double angleAverage(double theta1, double theta2) {
  // Computes the average between two angles by averaging points on the unit
  // circle and taking the arctan of the result.
  //   see: http://en.wikipedia.org/wiki/Mean_of_circular_quantities
  // theta1 is a scalar or column vector of angles (rad)
  // theta2 is a scalar or column vector of angles (rad)

  double x_mean = 0.5 * (cos(theta1) + cos(theta2));
  double y_mean = 0.5 * (sin(theta1) + sin(theta2));

  double angle_mean = atan2(y_mean, x_mean);

  return angle_mean;
}

template <typename DerivedTorque, typename DerivedForce, typename DerivedNormal, typename DerivedPoint>
std::pair<Eigen::Vector3d, double> resolveCenterOfPressure(const Eigen::MatrixBase<DerivedTorque> & torque, const Eigen::MatrixBase<DerivedForce> & force, const Eigen::MatrixBase<DerivedNormal> & normal, const Eigen::MatrixBase<DerivedPoint> & point_on_contact_plane)
{
  // TODO: implement multi-column version
  using namespace Eigen;

  if (abs(normal.squaredNorm() - 1.0) > 1e-12) {
    throw std::runtime_error("Drake:resolveCenterOfPressure:BadInputs: normal should be a unit vector");
  }

  Vector3d cop;
  double normal_torque_at_cop;

  double fz = normal.dot(force);
  bool cop_exists = abs(fz) > 1e-12;

  if (cop_exists) {
    auto torque_at_point_on_contact_plane = torque - point_on_contact_plane.cross(force);
    double normal_torque_at_point_on_contact_plane = normal.dot(torque_at_point_on_contact_plane);
    auto tangential_torque = torque_at_point_on_contact_plane - normal * normal_torque_at_point_on_contact_plane;
    cop = normal.cross(tangential_torque) / fz + point_on_contact_plane;
    auto torque_at_cop = torque - cop.cross(force);
    normal_torque_at_cop = normal.dot(torque_at_cop);
  }
  else {
    cop.setConstant(std::numeric_limits<double>::quiet_NaN());
    normal_torque_at_cop = std::numeric_limits<double>::quiet_NaN();
  }
  return std::pair<Vector3d, double>(cop, normal_torque_at_cop);
}

//Based on the Matrix Sign Function method outlined in this paper:
//http://www.engr.iupui.edu/~skoskie/ECE684/Riccati_algorithms.pdf
template <typename DerivedA, typename DerivedB, typename DerivedQ, typename DerivedR, typename DerivedX>
void care(MatrixBase<DerivedA> const& A, MatrixBase<DerivedB> const& B, MatrixBase<DerivedQ> const& Q, MatrixBase<DerivedR> const& R,  MatrixBase<DerivedX> & X)
{
  const size_t n = A.rows();

  LLT<MatrixXd> R_cholesky(R);

  MatrixXd H(2 * n, 2 * n);
  H << A, B * R_cholesky.solve(B.transpose()), Q, -A.transpose();

  MatrixXd Z = H;
  MatrixXd Z_old;

  //these could be options
  const double tolerance = 1e-9;
  const double max_iterations = 100;

  double relative_norm;
  size_t iteration = 0;

  const double p = static_cast<double>(Z.rows());

  do {
    Z_old = Z;
    //R. Byers. Solving the algebraic Riccati equation with the matrix sign function. Linear Algebra Appl., 85:267–279, 1987
    //Added determinant scaling to improve convergence (converges in rough half the iterations with this)
    double ck = pow(abs(Z.determinant()), -1.0/p);
    Z *= ck;
    Z = Z - 0.5 * (Z - Z.inverse());
    relative_norm = (Z - Z_old).norm();
    iteration ++;
  } while(iteration < max_iterations && relative_norm > tolerance);

  MatrixXd W11 = Z.block(0, 0, n, n);
  MatrixXd W12 = Z.block(0, n, n, n);
  MatrixXd W21 = Z.block(n, 0, n, n);
  MatrixXd W22 = Z.block(n, n, n, n);

  MatrixXd lhs(2 * n, n);
  MatrixXd rhs(2 * n, n);
  MatrixXd eye = MatrixXd::Identity(n, n);
  lhs << W12, W22 + eye;
  rhs << W11 + eye, W21;

  JacobiSVD<MatrixXd> svd(lhs, ComputeThinU | ComputeThinV);

  X = svd.solve(rhs);
}

template <typename DerivedA, typename DerivedB, typename DerivedQ, typename DerivedR, typename DerivedK, typename DerivedS>
void lqr(MatrixBase<DerivedA> const& A, MatrixBase<DerivedB> const& B, MatrixBase<DerivedQ> const& Q, MatrixBase<DerivedR> const& R, MatrixBase<DerivedK> & K, MatrixBase<DerivedS> & S)
{
  LLT<MatrixXd> R_cholesky(R);
  care(A, B, Q, R, S);
  K = R_cholesky.solve(B.transpose() * S);
}

template DLLEXPORT void lqr(MatrixBase<MatrixXd> const& A, MatrixBase<MatrixXd> const& B, MatrixBase<MatrixXd> const& Q, MatrixBase<MatrixXd> const& R, MatrixBase<MatrixXd> & K, MatrixBase<MatrixXd> & S);
template DLLEXPORT void lqr(MatrixBase< Map<MatrixXd> > const& A, MatrixBase< Map<MatrixXd> > const& B, MatrixBase< Map<MatrixXd> > const& Q, MatrixBase< Map<MatrixXd> > const& R, MatrixBase< Map<MatrixXd> > & K, MatrixBase< Map<MatrixXd> > & S);
template DLLEXPORT void lqr(MatrixBase<MatrixXd> const& A, MatrixBase<MatrixXd> const& B, MatrixBase<MatrixXd> const& Q, MatrixBase<MatrixXd> const& R, MatrixBase< Map<MatrixXd> > & K, MatrixBase< Map<MatrixXd> > & S);
template DLLEXPORT void lqr<Eigen::Block<Eigen::Matrix<double, -1, -1, 0, -1, -1>, -1, -1, true>, Eigen::Block<Eigen::Matrix<double, -1, -1, 0, -1, -1>, -1, -1, true>, Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1>, Eigen::Matrix<double, -1, -1, 0, -1, -1> >(Eigen::MatrixBase<Eigen::Block<Eigen::Matrix<double, -1, -1, 0, -1, -1>, -1, -1, true> > const&, Eigen::MatrixBase<Eigen::Block<Eigen::Matrix<double, -1, -1, 0, -1, -1>, -1, -1, true> > const&, Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> > const&, Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&, Eigen::MatrixBase<Eigen::Matrix<double, -1, -1, 0, -1, -1> >&);

template DLLEXPORT void care(MatrixBase<MatrixXd> const& A, MatrixBase<MatrixXd> const& B, MatrixBase<MatrixXd> const& Q, MatrixBase<MatrixXd> const& R, MatrixBase<MatrixXd> & X);
template DLLEXPORT void care(MatrixBase< Map<MatrixXd> > const& A, MatrixBase< Map<MatrixXd> > const& B, MatrixBase< Map<MatrixXd> > const& Q, MatrixBase< Map<MatrixXd> > const& R, MatrixBase< Map<MatrixXd> > & X);

template DLLEXPORT std::pair<Eigen::Matrix<double, 3, 1, 0, 3, 1>, double> resolveCenterOfPressure<Eigen::Map<Eigen::Matrix<double, 3, 1, 0, 3, 1> const, 0, Eigen::Stride<0, 0> >, Eigen::Map<Eigen::Matrix<double, 3, 1, 0, 3, 1> const, 0, Eigen::Stride<0, 0> >, Eigen::Map<Eigen::Matrix<double, 3, 1, 0, 3, 1> const, 0, Eigen::Stride<0, 0> >, Eigen::Map<Eigen::Matrix<double, 3, 1, 0, 3, 1> const, 0, Eigen::Stride<0, 0> > >(Eigen::MatrixBase<Eigen::Map<Eigen::Matrix<double, 3, 1, 0, 3, 1> const, 0, Eigen::Stride<0, 0> > > const&, Eigen::MatrixBase<Eigen::Map<Eigen::Matrix<double, 3, 1, 0, 3, 1> const, 0, Eigen::Stride<0, 0> > > const&, Eigen::MatrixBase<Eigen::Map<Eigen::Matrix<double, 3, 1, 0, 3, 1> const, 0, Eigen::Stride<0, 0> > > const&, Eigen::MatrixBase<Eigen::Map<Eigen::Matrix<double, 3, 1, 0, 3, 1> const, 0, Eigen::Stride<0, 0> > > const&);
template DLLEXPORT std::pair<Eigen::Matrix<double, 3, 1, 0, 3, 1>, double> resolveCenterOfPressure<Eigen::Matrix<double, 3, 1, 0, 3, 1>, Eigen::Matrix<double, 3, 1, 0, 3, 1>, Eigen::Matrix<double, 3, 1, 0, 3, 1>, Eigen::Matrix<double, 3, 1, 0, 3, 1> >(Eigen::MatrixBase<Eigen::Matrix<double, 3, 1, 0, 3, 1> > const&, Eigen::MatrixBase<Eigen::Matrix<double, 3, 1, 0, 3, 1> > const&, Eigen::MatrixBase<Eigen::Matrix<double, 3, 1, 0, 3, 1> > const&, Eigen::MatrixBase<Eigen::Matrix<double, 3, 1, 0, 3, 1> > const&);
template DLLEXPORT std::pair<Eigen::Matrix<double, 3, 1, 0, 3, 1>, double> resolveCenterOfPressure<Eigen::Block<Eigen::Matrix<double, 6, 1, 0, 6, 1>, 3, 1, false>, Eigen::Block<Eigen::Matrix<double, 6, 1, 0, 6, 1>, 3, 1, false>, Eigen::Matrix<double, 3, 1, 0, 3, 1>, Eigen::Matrix<double, 3, 1, 0, 3, 1> >(Eigen::MatrixBase<Eigen::Block<Eigen::Matrix<double, 6, 1, 0, 6, 1>, 3, 1, false> > const&, Eigen::MatrixBase<Eigen::Block<Eigen::Matrix<double, 6, 1, 0, 6, 1>, 3, 1, false> > const&, Eigen::MatrixBase<Eigen::Matrix<double, 3, 1, 0, 3, 1> > const&, Eigen::MatrixBase<Eigen::Matrix<double, 3, 1, 0, 3, 1> > const&);


text:
#pragma once

#include <Eigen/Geometry>
#include <memory>
#include <vector>

#include "../JointId/jointid.hh"
#include "../Math/math.hh"

namespace bold
{
  /**
   * Body part information
   */
  class BodyPart
  {
  public:
    BodyPart(std::string name)
    : name(name)
    {}

    virtual ~BodyPart() = default;

    /// Name of body part
    std::string name;

    // Needed when having fixed sized Eigen member
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  };

  class Joint;

  /**
   * Limb information
   */
  class Limb : public BodyPart
  {
  public:
    Limb(std::string name)
    : BodyPart(name)
    {}

    /// Mass of limb, in KG
    double mass;

    // Position of the centre of mass
    Eigen::Vector3d com;

    /// Vector of joints attached to this limb
    std::vector<std::shared_ptr<Joint>> joints;

    // Needed when having fixed sized Eigen member
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  };

  /**
   * Joint information
   */
  class Joint : public BodyPart
  {
  public:
    Joint(JointId id, std::string name)
    : BodyPart(name),
      id(id),
      rotationOrigin(0)
    {}

    /// This joint's identifier
    JointId id;

    /// Axis of joint in local coordinate system
    Eigen::Vector3d axis;

    /** An offset for the joint's rotation.
    *
    * This is relative to the description in the kinematic model, and exists
    * in cases where the model is simpler to lay out using non-zero hinge
    * positions. For example, the DARwIn-OPs shoulder roll joints push the
    * elbows out to 45 degrees when at zero. The model used describes the
    * robot with arms pointing down, parallel to one another, and the origin
    * for rotation is used to adjust.
    */
    double rotationOrigin;

//    /// Minimum and maximum angle bounds
//    std::pair<double, double> bounds;

    /// Child part connected by this joint. May be a Limb, or another Joint.
    std::shared_ptr<BodyPart const> childPart;

    /// Anchor points of joint on body parts, relative to their center
    std::pair<Eigen::Vector3d,Eigen::Vector3d> anchors;

    // Needed when having fixed sized Eigen member
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  };
}

ext: hh

max_stars_repo_name: drewnoakes/bold-humanoid

max_issues_repo_name: drewnoakes/bold-humanoid

max_forks_repo_name: drewnoakes/bold-humanoid


text:
// think-cell public library
//
// Copyright (C) 2016-2021 think-cell Software GmbH
//
// 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

#include "range.h"
#include "container.h" // tc::vector
#include "range.t.h"

MODIFY_WARNINGS_BEGIN(((disable)(4018)))
#include <boost/range/category.hpp>
#include <boost/range/iterator_range.hpp>
MODIFY_WARNINGS_END

namespace lookup {
	struct NoBegin final {};
}

namespace {
	using HasStdBegin = tc::vector<int>;
	using HasNoBegin = lookup::NoBegin;
	using HasBoostBegin = boost::iterator_range< boost::range_iterator<std::array<unsigned long,1> const>::type >;

	STATIC_ASSERT(tc::is_range_with_iterators<HasStdBegin>::value);
	STATIC_ASSERT(tc::is_range_with_iterators<HasBoostBegin>::value);
	STATIC_ASSERT(!tc::is_range_with_iterators<HasNoBegin>::value);

	struct TransFilterTest final {
		template <typename T>
		struct wrapped final {
			explicit wrapped(T const& t) noexcept : m_t(t) {}
			wrapped(wrapped<T> const& t) noexcept : m_t(t.m_t) {}
			T getT() const& noexcept { return m_t; }

			T m_t;
		};

		using wrapped_long = wrapped<long>;
		static bool filter35( wrapped_long const& wl ) noexcept { return wl.getT() == 3 || wl.getT() == 5; }
		static std::size_t transf_times_100( wrapped_long const& wl ) noexcept { std::size_t color = wl.getT() * 100; return color; }

		using WlList = tc::vector< wrapped_long > const&;
		//using WlList = tc::vector< wrapped_long >;        // works!
		WlList getWlList() const& noexcept { return m_list; }

		using WlFilterdList = tc::filter_adaptor<decltype(&filter35), WlList>;
		auto getWlFilterdList() const& return_decltype_noexcept( tc::filter(getWlList(), &filter35) )

		// This is where it gets weird, as soon as you somehow use has_range_iterator<WlFilterdList> (here at class scope)
		// things go crashing down, even though has_range_iterator<WlFilterdList> is perfectly fine one line later at function scope

		using WlFilterdTransformedList = tc::transform_adaptor<decltype(&transf_times_100), WlFilterdList, true>; STATIC_ASSERT(tc::is_range_with_iterators<WlFilterdList>::value);
		//using WlFilterdTransformedList = transform_adaptor<decltype(&transf_times_100), WlFilterdList, tc::is_range_with_iterators<WlFilterdList>::value>;
		//using WlFilterdTransformedList = transform_adaptor<decltype(&transf_times_100), WlFilterdList>;
		WlFilterdTransformedList getWlFilterdTransformedList() const& noexcept {
			STATIC_ASSERT(tc::is_range_with_iterators<WlFilterdList>::value);
			STATIC_ASSERT(tc::is_range_with_iterators<WlFilterdTransformedList>::value);

			return  tc::transform(getWlFilterdList(), &transf_times_100 );
		}

		TransFilterTest() noexcept {
			TEST_init_hack(tc::vector, wrapped_long, list, {wrapped_long(1),wrapped_long(2),wrapped_long(3),wrapped_long(4),wrapped_long(5),wrapped_long(6)});
			m_list = list;
		}

		private:
			tc::vector<wrapped_long> m_list;
	};

UNITTESTDEF( TransFilterTest ) {

	TransFilterTest t;
	auto_cref( res, t.getWlFilterdTransformedList() );

	UNUSED_TEST_VARIABLE(res);
	TEST_init_hack(tc::vector, std::size_t, original, {std::size_t(300), std::size_t(500)});
}

UNITTESTDEF( boost_iterator_range_compat ) {

	TEST_init_hack(tc::vector, unsigned long, original, {1,2,3,4,5,6,7,8});
	tc::vector<unsigned long> v = original;

	TEST_init_hack(tc::vector, unsigned long, baul_exp, {6});
	std::array<unsigned long, 1> baul; tc::front(baul) = 6;

	auto mutable_range = boost::make_iterator_range(v); TEST_RANGE_LENGTH(mutable_range, 8);
	TEST_RANGE_EQUAL(original, mutable_range);

	boost::iterator_range< boost::range_iterator<std::array<unsigned long,1> const>::type > const baul_r = boost::make_iterator_range(baul);

	STATIC_ASSERT(tc::is_range_with_iterators<decltype(baul)>::value);

	TEST_RANGE_EQUAL(baul_exp, baul_r);

	auto baul_our_r = tc::slice(baul);
	TEST_RANGE_EQUAL(baul_exp, baul_our_r);

}

UNITTESTDEF( boost_range_traits_compat ) {
	TEST_init_hack(tc::vector, unsigned long, original, {1,2,3,4,5,6,7,8});
	TEST_init_hack(tc::vector, unsigned long, exp, {2,4,6,8});

	auto fr = tc::filter(original, [](unsigned long i) noexcept { return i%2==0; });

	STATIC_ASSERT(std::is_same<decltype(std::begin(fr)), decltype(tc::begin(fr))>::value);

	auto bir = boost::make_iterator_range(fr);
	TEST_RANGE_EQUAL(exp, bir);
}

struct inner final {
	inner(int id) noexcept : i(id) {}
	int id() const& noexcept { return i*100; }
private:
	friend struct free_id;
	int i;
};

struct free_id final { int operator()(inner const& in) const& noexcept { return in.id(); } };
struct filter_stub final { template<typename T> bool operator()(T const&) const& noexcept { return true; } };

struct outer final {
	tc::vector<inner> m_in;

	outer() noexcept {
		TEST_init_hack(tc::vector, inner, tmp, {1,2,3,4,5,6,7,8});
		m_in = tmp;
	}

	using TRange = tc::filter_adaptor< filter_stub, tc::transform_adaptor< free_id, tc::vector<inner> const& , true>, true >;
	TRange trans_range() & noexcept {
		return tc::filter( tc::transform(tc::as_const(m_in), free_id()), filter_stub() );
	}
};

UNITTESTDEF( deduce_traits ) {
	TEST_init_hack(tc::vector, int, exp, {100,200,300,400,500,600,700,800});
	outer o;

	auto trange = tc::filter(tc::transform(tc::as_const(o.m_in), free_id()), filter_stub());
	TEST_RANGE_EQUAL( exp, trange );

	tc::vector<int> res;
	tc::for_each( o.trans_range(), [&](int const id) noexcept { tc::cont_emplace_back(res, id); } );
	TEST_RANGE_EQUAL(exp, res);
}

}

max_stars_repo_name: think-cell/range

max_issues_repo_name: think-cell/range

max_forks_repo_name: think-cell/range