text:
//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2013   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//         Copyright 2012 - 2013   MetaScale SAS
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_MEMORY_ALIGNED_INPUT_ITERATOR_HPP_INCLUDED
#define BOOST_SIMD_MEMORY_ALIGNED_INPUT_ITERATOR_HPP_INCLUDED

#include <boost/simd/include/functions/aligned_load.hpp>
#include <boost/simd/sdk/simd/pack.hpp>
#include <boost/simd/sdk/meta/cardinal_of.hpp>
#include <boost/simd/memory/details/uncheck_iterator.hpp>
#include <boost/simd/memory/details/input_iterator_base.hpp>
#include <boost/iterator/iterator_adaptor.hpp>
#include <boost/core/ignore_unused.hpp>
#include <boost/config.hpp>
#include <iterator>

namespace boost { namespace simd
{
  /*!
    @brief Read-only aligned SIMD iterator

    aligned_input_iterator adapt an Iterator to aligned values in memory into
    a standard compliant iterator that traverses this memory block using SIMD
    registers.

    @par Models:

    @model{ http://www.cplusplus.com/reference/iterator/RandomAccessIterator/
          , RandomAccessIterator
          }

    @usage_output{memory/aligned_input_iterator.cpp,memory/aligned_input_iterator.out}

    @tparam Iterator Iterator type to adapt
    @tparam C Width of the SIMD register to use as iteration value. By default
              this value is equal to the optimal register cardinal for current
              architecture and type @c T.
  **/
  template< typename Iterator
          , std::size_t C = meta::cardinal_of
                            < pack< typename std::iterator_traits<Iterator>
                                                ::value_type
                                  >
                            >::value
          >
  struct  aligned_input_iterator
        : details::input_iterator_base
                  < Iterator
                  , C
                  , typename std::iterator_traits<Iterator>::value_type
                  , tag::aligned_load_
                  >
  {
    typedef details::input_iterator_base
                  < Iterator
                  , C
                  , typename std::iterator_traits<Iterator>::value_type
                  , tag::aligned_load_
                  >                                           parent;

    /// Default constructor
    aligned_input_iterator() : parent() {}

    /// INTERNAL ONLY
    aligned_input_iterator( parent const& src ) : parent(src) {}

    /// Constructor from an aligned pointer
    explicit  aligned_input_iterator(Iterator p) : parent(p)
    {
      // MSVC SCL_SECURE mode adds extra-check that make aligned end
      // difficult to check with this.
      typename details::unchecker<Iterator>::type lp = details::unchecker<Iterator>::call(p);
      boost::ignore_unused(lp);

      BOOST_ASSERT_MSG
      ( boost::simd::is_aligned(&(*lp) , C*sizeof(*lp) )
      , "The constructor of iterator<T,C> has been called on a pointer "
        "which alignment is not compatible with the current SIMD extension."
      );
    }
  };

  /*!
    @brief Adapter for aligned SIMD read-only iterator

    Convert an existing iterator referencing the beginning of a aligned memory
    block into a SIMD aware read-only iterator returning SIMD pack of optimal
    cardinal for current architecture.

    @usage_output{memory/aligned_input_iterator.cpp,memory/aligned_input_iterator.out}

    @param p An iterator referencing the beginning of a contiguous memory block.

    @return An instance of aligned_input_iterator
  **/
  template<typename Iterator> inline
  aligned_input_iterator<Iterator> aligned_input_begin(Iterator p)
  {
    return aligned_input_iterator<Iterator>(p);
  }

  /*!
    @brief Adapter for aligned SIMD read-only iterator

    Convert an existing iterator referencing the beginning of a aligned memory
    block into a SIMD aware read-only iterator returning SIMD pack of cardinal
    @c C.

    @usage_output{memory/aligned_input_iterator.cpp,memory/aligned_input_iterator.out}

    @tparam C Width of the SIMD register to use as iteration value.

    @param p An iterator referencing the beginning of a contiguous memory block.

    @return An instance of aligned_input_iterator
  **/
  template<std::size_t C, typename Iterator> inline
  aligned_input_iterator<Iterator, C> aligned_input_begin(Iterator p)
  {
    return aligned_input_iterator<Iterator, C>(p);
  }

  /*!
    @brief Adapter for aligned SIMD read-only iterator

    Convert an existing iterator referencing the end of a aligned memory
    block into a SIMD aware read-only iterator returning SIMD pack of optimal
    cardinal for current architecture.

    @usage_output{memory/aligned_input_iterator.cpp,memory/aligned_input_iterator.out}

    @param p An iterator referencing the end of a contiguous memory block.

    @return An instance of aligned_input_iterator
  **/
  template<typename Iterator> inline
  aligned_input_iterator<Iterator> aligned_input_end(Iterator p)
  {
    return aligned_input_iterator<Iterator>(p);
  }

  /*!
    @brief Adapter for aligned SIMD read-only iterator

    Convert an existing iterator referencing the end of a aligned memory
    block into a SIMD aware read-only iterator returning SIMD pack of cardinal
    @c C.

    @usage_output{memory/aligned_input_iterator.cpp,memory/aligned_input_iterator.out}

    @tparam C Width of the SIMD register to use as iteration value.

    @param p An iterator referencing the end of a contiguous memory block.

    @return An instance of aligned_input_iterator
  **/
  template<std::size_t C, typename Iterator> inline
  aligned_input_iterator<Iterator,C> aligned_input_end(Iterator p)
  {
    return aligned_input_iterator<Iterator, C>(p);
  }
} }

#endif

ext: hpp


text:
#include <Eigen/Core>

#include <ros/ros.h>
#include <std_msgs/Bool.h>
#include <std_msgs/Int16.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2/LinearMath/Vector3.h>

#include <tf2_ros/transform_listener.h>
#include <geometry_msgs/TransformStamped.h>
#include <actionlib/server/simple_action_server.h>
#include <collect_calib_data/rotation_broadcasterAction.h>

#include "./../include/rotation_broadcaster.hpp"

namespace CalibrationVelodyne
{

    RotationBroadcaster::RotationBroadcaster(ros::NodeHandle &nh) :
        success (false),
        server (nh, "rotation_broadcaster", false),
        degree_sub (nh.subscribe("degree", 1, &RotationBroadcaster::degree_cb, this))
    {
        ros::param::get("/rotation_broadcaster/x", x);
        ros::param::get("/rotation_broadcaster/y", y);
        ros::param::get("/rotation_broadcaster/z", z);

        server.registerGoalCallback(boost::bind(&RotationBroadcaster::goal_callback, this));
        server.registerPreemptCallback(boost::bind(&RotationBroadcaster::preempt_callback, this));
        server.start();
    }

    RotationBroadcaster::~RotationBroadcaster()
    {
    }

    // 回転行列Rの初期位置を設定
    void RotationBroadcaster::set_R(
                Eigen::Matrix3f &R,
                const float roll,
                const float pitch,
                const float yaw)
    {
        Eigen::Matrix3f R_tmp;
        Eigen::Matrix3f Rpi;
        Eigen::Matrix3f Rya;
        Eigen::Matrix3f Rro;

        Rro << 1, 0, 0,
               0, cos(roll), -sin(roll),
               0, sin(roll), cos(roll);

        Rya << cos(yaw), 0, sin(yaw),
               0, 1, 0,
               -sin(yaw), 0, cos(yaw);

        Rpi << cos(pitch), -sin(pitch), 0,
               sin(pitch), cos(pitch), 0,
               0, 0, 1;

        R = Rro * Rpi * Rya;
        //R = R_tmp.inverse();

        //std::cout << "R : \n" << R << std::endl;
        feedback.work.data = true;
        feedback.work_method.data = "Done set rotation matrix";
        server.publishFeedback(feedback);
    }

    Eigen::Vector3f RotationBroadcaster::set_diff_xyz(
        const float roll,
        const float pitch,
        const float yaw,
        const float x,
        const float y,
        const float z)
    {
        Eigen::Matrix3f R_3x3;
        Eigen::Vector3f t;
        t << x, y, z;

        set_R(R_3x3, roll, pitch, yaw);
        t = R_3x3*t;

        return t;
    }

    void RotationBroadcaster::set_ts(
        const float roll,
        const float pitch,
        const float yaw,
        geometry_msgs::TransformStamped &ts)
    {
        Eigen::Vector3f t;
        t = set_diff_xyz(x, y, z, roll, pitch, yaw);

        ts.header.stamp = ros::Time::now();
        ts.header.frame_id = "mount";
        ts.child_frame_id = "velodyne";

        tf2::Vector3 translation;
	    translation.setValue(t(0), t(1), t(2));
	    ts.transform.translation.x = translation.x();
	    ts.transform.translation.y = translation.y();
	    ts.transform.translation.z = translation.z();

	    tf2::Quaternion rotation;
	    rotation.setRPY(roll, pitch, yaw);
	    ts.transform.rotation.x = rotation.x();
	    ts.transform.rotation.y = rotation.y();
	    ts.transform.rotation.z = rotation.z();
	    ts.transform.rotation.w = rotation.w();

        feedback.work.data = true;
        feedback.work_method.data = "Done set TransformStamp";
        server.publishFeedback(feedback);
    }

    void RotationBroadcaster::degree_cb(
        const std_msgs::Int16 &potentio_cb)
    {
        if(degree != potentio_cb.data)
        {
            degree = potentio_cb.data;
            cb_success = true;
        }
        else
        {
            cb_success = false;
        }
    }

    void RotationBroadcaster::goal_callback()
    {
        if(server.isNewGoalAvailable())
        {
          goal = server.acceptNewGoal();
        }

        ros::Rate rate(1.0);
        tf2_ros::Buffer tfBuffer;
        tf2_ros::TransformListener tfListener(tfBuffer);
        geometry_msgs::TransformStamped ts;

        while(ros::ok())
        {
            try
            {
                ts = tfBuffer.lookupTransform("map", "velodyne", ros::Time());
                break;
            }
            catch(tf2::TransformException &ex)
            {
                ROS_ERROR("%s", ex.what());
                ros::Duration(1.0).sleep();
                ros::spinOnce();
                continue;
            }

            /*
            if(cb_success)
            {
                pitch = degree * (M_PI/180.0);
                set_ts(0.0, pitch, 0.0, ts);
                cb_success = false;
                break;
            }
            else
            {
                ROS_WARN("Do not movement the mount");
                //ROS_WARN("potentio degree : %d", degree);
                rate.sleep();
                ros::spinOnce();
            }
            */
        }

        result.success_work.data = true;
        result.result_deg.data = degree;
        result.result_rotation = ts;
        server.setSucceeded(result, "ts");
    }

    void RotationBroadcaster::preempt_callback()
    {
        result.success_work.data = false;
        server.setPreempted(result, "preempted");
    }

    void RotationBroadcaster::run()
    {
        ros::spin();
    }

}

int main(int argc, char *argv[])
{
    ros::init(argc, argv, "rotation_broadcaster");
    ros::NodeHandle nh;

    CalibrationVelodyne::RotationBroadcaster *server;
    server = new CalibrationVelodyne::RotationBroadcaster(nh);
    server->run();
    delete server;

    return 0;
}

ext: cpp


text:
#include "Graph.h"

#include <queue>
#include <algorithm>

#include <boost/math/constants/constants.hpp>

namespace {
    double clamp(double val, double min, double max) {
        return std::max(min, std::min(max, val));
    }

    bool pointOnLine(const std::array<cglib::vec3<double>, 2>& line, const cglib::vec3<double>& pos) {
        cglib::vec3<double> v0 = line[1] - line[0];
        cglib::vec3<double> v1 = pos - line[0];
        return (v1 == cglib::vec3<double>(0, 0, 0) || cglib::dot_product(cglib::unit(v0), cglib::unit(v1)) == 1);
    }

    bool pointInsideTriangle(const std::array<cglib::vec3<double>, 3>& triangle, const cglib::vec3<double>& pos) {
        cglib::vec3<double> v0 = triangle[2] - triangle[0];
        cglib::vec3<double> v1 = triangle[1] - triangle[0];
        cglib::vec3<double> v2 = pos - triangle[0];
        if (cglib::dot_product(v2, cglib::vector_product(v0, v1)) != 0) {
            return false;
        }

        double dot00 = cglib::dot_product(v0, v0);
        double dot01 = cglib::dot_product(v0, v1);
        double dot02 = cglib::dot_product(v0, v2);
        double dot11 = cglib::dot_product(v1, v1);
        double dot12 = cglib::dot_product(v1, v2);

        double invDenom = 1 / (dot00 * dot11 - dot01 * dot01);
        double u = (dot11 * dot02 - dot01 * dot12) * invDenom;
        double v = (dot00 * dot12 - dot01 * dot02) * invDenom;

        return u >= 0 && v >= 0 && u + v <= 1;
    }

    cglib::vec3<double> closestPointOnLine(const std::array<cglib::vec3<double>, 2>& line, const cglib::vec3<double>& pos) {
        cglib::vec3<double> edge = line[1] - line[0];
        if (cglib::norm(edge) == 0) {
            return line[0];
        }
        cglib::vec3<double> v0 = pos - line[0];

        double c = cglib::dot_product(edge, v0);
        double d = cglib::dot_product(edge, edge);

        double s = (v0 != cglib::vec3<double>(0, 0, 0) ? clamp(c / d, 0, 1) : 0);
        return line[0] + edge * s;
    }

    cglib::vec3<double> closestPointOnTriangle(const std::array<cglib::vec3<double>, 3>& triangle, const cglib::vec3<double>& pos) {
        cglib::vec3<double> edge0 = triangle[1] - triangle[0];
        if (cglib::norm(edge0) == 0) {
            return closestPointOnLine(std::array<cglib::vec3<double>, 2> {{ triangle[2], triangle[1] }}, pos);
        }
        cglib::vec3<double> edge1 = triangle[2] - triangle[0];
        if (cglib::norm(edge1) == 0) {
            return closestPointOnLine(std::array<cglib::vec3<double>, 2> {{ triangle[1], triangle[0] }}, pos);
        }
        cglib::vec3<double> edge2 = triangle[1] - triangle[2];
        if (cglib::norm(edge2) == 0) {
            return closestPointOnLine(std::array<cglib::vec3<double>, 2> {{ triangle[2], triangle[0] }}, pos);
        }
        cglib::vec3<double> v0 = triangle[0] - pos;

        double a = cglib::dot_product(edge0, edge0);
        double b = cglib::dot_product(edge0, edge1);
        double c = cglib::dot_product(edge1, edge1);
        double d = cglib::dot_product(edge0, v0);
        double e = cglib::dot_product(edge1, v0);

        double det = a * c - b * b;
        double s = b * e - c * d;
        double t = b * d - a * e;

        if (s + t < det) {
            if (s < 0) {
                if (t < 0) {
                    if (d < 0) {
                        s = clamp(-d / a, 0, 1);
                        t = 0;
                    } else {
                        s = 0;
                        t = clamp(-e / c, 0, 1);
                    }
                } else {
                    s = 0;
                    t = clamp(-e / c, 0, 1);
                }
            } else if (t < 0) {
                s = clamp(-d / a, 0, 1);
                t = 0;
            } else {
                double invDet = 1 / det;
                s *= invDet;
                t *= invDet;
            }
        } else {
            if (s < 0) {
                double tmp0 = b + d;
                double tmp1 = c + e;
                if (tmp1 > tmp0) {
                    double numer = tmp1 - tmp0;
                    double denom = a - 2 * b + c;
                    s = clamp(numer / denom, 0, 1);
                    t = 1 - s;
                } else {
                    t = clamp(-e / c, 0, 1);
                    s = 0;
                }
            } else if (t < 0) {
                if (a + d > b + e) {
                    double numer = c + e - b - d;
                    double denom = a - 2 * b + c;
                    s = clamp(numer / denom, 0, 1);
                    t = 1 - s;
                } else {
                    s = clamp(-e / c, 0, 1);
                    t = 0;
                }
            } else {
                double numer = c + e - b - d;
                double denom = a - 2 * b + c;
                s = clamp(numer / denom, 0, 1);
                t = 1 - s;
            }
        }

        return triangle[0] + edge0 * s + edge1 * t;
    }
}

namespace carto { namespace sgre {
    StaticGraph::StaticGraph(std::vector<Node> nodes, std::vector<Edge> edges, std::vector<FeatureProperties> properties, std::vector<Attributes> attributes) :
        _nodes(std::move(nodes)), _edges(std::move(edges)), _featureProperties(std::move(properties)), _attributes(std::move(attributes))
    {
        // Add edge ids to nodes
        linkNodeEdgeIds(_nodes, _edges);

        // Build RTree for faster spatial queries
        cglib::bbox3<double> bounds = cglib::bbox3<double>::smallest();
        std::vector<EdgeId> edgeIds;
        edgeIds.reserve(_edges.size());
        for (std::size_t i = 0; i < _edges.size(); i++) {
            EdgeId edgeId = static_cast<EdgeId>(i);
            const Edge& edge = getEdge(edgeId);
            const Node& node0 = getNode(edge.nodeIds[0]);
            const Node& node1 = getNode(edge.nodeIds[1]);
            bounds.add(node0.points.begin(), node0.points.end());
            bounds.add(node1.points.begin(), node1.points.end());
            edgeIds.push_back(edgeId);
        }
        _rootNode = buildRTree(bounds, edgeIds);
    }

    std::vector<std::pair<Graph::EdgeId, Point>> StaticGraph::findNearestEdgePoint(const Point& pos, const FeatureFilter& filter, const SearchOptions& options) const {
        struct RTreeNodeRecord {
            std::shared_ptr<const RTreeNode> node;
            double dist;

            bool operator < (const RTreeNodeRecord& rec) const { return rec.dist < dist; }
        };

        static constexpr double DIST_EPSILON = 1.0e-6;
        static constexpr double EARTH_RADIUS = 6378137.0;

        double latScale = EARTH_RADIUS * boost::math::constants::pi<double>() / 180.0;
        double lngScale = latScale * std::max(std::cos(pos(1) * boost::math::constants::pi<double>() / 180.0), 0.01);
        cglib::vec3<double> scale(lngScale, latScale, options.zSensitivity);

        double bestDist = std::numeric_limits<double>::infinity();
        std::vector<std::pair<Graph::EdgeId, Point>> bestResults;

        // Do the matching starting from root RTree node. Use priority queue for sorting and rejecting subnodes.
        std::priority_queue<RTreeNodeRecord> nodeQueue;
        Point nearestRootPos = _rootNode->bounds.nearest_point(pos);
        double rootDist = calculateDistance(nearestRootPos, pos, scale);
        nodeQueue.push({ _rootNode, rootDist });
        while (!nodeQueue.empty()) {
            std::shared_ptr<const RTreeNode> node = nodeQueue.top().node;
            double dist = nodeQueue.top().dist;
            nodeQueue.pop();

            if (dist > bestDist + DIST_EPSILON) {
                break;
            }

            // Store subnodes in the priority queue
            for (const std::shared_ptr<const RTreeNode>& subNode : node->subNodes) {
                if (subNode) {
                    Point nearestPos = subNode->bounds.nearest_point(pos);
                    double dist = calculateDistance(nearestPos, pos, scale);
                    nodeQueue.push({ subNode, dist });
                }
            }

            // Do slow matching for edges store in this RTree node
            for (EdgeId edgeId : node->edgeIds) {
                const Edge& edge = getEdge(edgeId);

                // Apply filter?
                if (!filter.empty()) {
                    if (edge.featureId == FeatureId(-1)) {
                        continue;
                    }
                    const FeatureProperties& properties = getFeatureProperties(edge.featureId);
                    if (!properties.is<picojson::object>()) {
                        continue;
                    }

                    auto it = filter.begin();
                    for (; it != filter.end(); it++) {
                        if (!properties.contains(it->first) || properties.get(it->first) != it->second) {
                            break;
                        }
                    }
                    if (it != filter.end()) {
                        continue;
                    }
                }

                if (auto closestPos = findNearestEdgePoint(edge, pos, scale)) {
                    double dist = calculateDistance(*closestPos, pos, scale);
                    if (dist <= bestDist + DIST_EPSILON) {
                        if (dist + DIST_EPSILON < bestDist) {
                            bestDist = dist;
                            bestResults.clear();
                        }
                        bestResults.emplace_back(edgeId, *closestPos);
                    }
                }
            }
        }

        return bestResults;
    }

    boost::optional<Point> StaticGraph::findNearestEdgePoint(const Edge& edge, const Point& pos, const cglib::vec3<double>& scale) const {
        const Node& node0 = getNode(edge.nodeIds[0]);
        const Node& node1 = getNode(edge.nodeIds[1]);

        // First find i0 and i1 for node points such that P[i0] == P[i1] (find the shared point of triangle edges)
        int i0 = 0, i1 = 0;
        for (int i = 0; i < 2; i++) {
            if (node0.points[i] == node1.points[0]) {
                i0 = i; i1 = 0;
                break;
            } else if (node0.points[i] == node1.points[1]) {
                i0 = i; i1 = 1;
                break;
            }
        }

        // Convert WGS84 coordinates to local orthonormal basis for simpler processing
        Point posScaled = cglib::pointwise_product(pos, scale);
        Point node0PointsScaled[2] = { cglib::pointwise_product(node0.points[0], scale), cglib::pointwise_product(node0.points[1], scale) };
        Point node1PointsScaled[2] = { cglib::pointwise_product(node1.points[0], scale), cglib::pointwise_product(node1.points[1], scale) };

        cglib::vec3<double> invScale(1.0 / scale(0), 1.0 / scale(1), (scale(2) != 0 ? 1.0 / scale(2) : std::numeric_limits<double>::max()));

        // Handle the edge based on its search criteria. Some cases are very tricky and the code depends on the graph builder.
        switch (edge.searchCriteria) {
        case SearchCriteria::NONE:
            return boost::optional<Point>();
        case SearchCriteria::VERTEX:
            if (node0.nodeFlags & NodeFlags::GEOMETRY_VERTEX) {
                if (node1.nodeFlags & NodeFlags::GEOMETRY_VERTEX) {
                    return (cglib::norm(posScaled - node0PointsScaled[i0]) < cglib::norm(posScaled - node1PointsScaled[i1]) ? node0.points[i0] : node1.points[i1]);
                }
                return node0.points[i0];
            } else if (node1.nodeFlags & NodeFlags::GEOMETRY_VERTEX) {
                return node1.points[i1];
            }
            return boost::optional<Point>();
        case SearchCriteria::FIRST_LAST_VERTEX:
            if (node0.nodeFlags & NodeFlags::ENDPOINT_VERTEX) {
                if (node1.nodeFlags & NodeFlags::ENDPOINT_VERTEX) {
                    return (cglib::norm(posScaled - node0PointsScaled[i0]) < cglib::norm(posScaled - node1PointsScaled[i1]) ? node0.points[i0] : node1.points[i1]);
                }
                return node0.points[i0];
            } else if (node1.nodeFlags & NodeFlags::ENDPOINT_VERTEX) {
                return node1.points[i1];
            }
            return boost::optional<Point>();
        case SearchCriteria::EDGE:
            if (edge.edgeFlags & EdgeFlags::GEOMETRY_EDGE) {
                if (pointOnLine(std::array<cglib::vec3<double>, 2> {{ node0.points[1 - i0], node1.points[1 - i1] }}, pos)) {
                    return pos;
                }
                Point closestScaled = closestPointOnLine(std::array<cglib::vec3<double>, 2> {{ node0PointsScaled[1 - i0], node1PointsScaled[1 - i1] }}, posScaled);
                return cglib::pointwise_product(closestScaled, invScale);
            }
            return boost::optional<Point>();
        case SearchCriteria::SURFACE:
            if (node0.points[i0] == node1.points[i1]) {
                if (pointInsideTriangle(std::array<cglib::vec3<double>, 3> {{ node0.points[i0], node0.points[1 - i0], node1.points[1 - i1] }}, pos)) {
                    return pos;
                }
                Point closestScaled = closestPointOnTriangle(std::array<cglib::vec3<double>, 3> {{ node0PointsScaled[i0], node0PointsScaled[1 - i0], node1PointsScaled[1 - i1] }}, posScaled);
                return cglib::pointwise_product(closestScaled, invScale);
            } else {
                if (pointOnLine(std::array<cglib::vec3<double>, 2> {{ node0.points[0], node1.points[0] }}, pos)) {
                    return pos;
                }
                Point closestScaled = closestPointOnLine(std::array<cglib::vec3<double>, 2> {{ node0PointsScaled[0], node1PointsScaled[0] }}, posScaled);
                return cglib::pointwise_product(closestScaled, invScale);
            }
        }

        return boost::optional<Point>();
    }

    std::shared_ptr<StaticGraph::RTreeNode> StaticGraph::buildRTree(const cglib::bbox3<double>& bounds, std::vector<EdgeId> edgeIds) const {
        static const std::size_t SPLIT_THRESHOLD = 4;

        // Create non-leaf nodes?
        if (edgeIds.size() > SPLIT_THRESHOLD) {
            cglib::bbox3<double> splitBounds[3][2];
            std::vector<EdgeId> splitEdgeIds[3][2];
            for (int dim = 0; dim < 3; dim++) {
                splitBounds[dim][0] = splitBounds[dim][1] = cglib::bbox3<double>::smallest();
            }

            // Do simple splitting by comparing the center of each edge to the center of the parent
            cglib::vec3<double> center = bounds.center();
            for (EdgeId edgeId : edgeIds) {
                const Edge& edge = getEdge(edgeId);
                const Node& node0 = getNode(edge.nodeIds[0]);
                const Node& node1 = getNode(edge.nodeIds[1]);
                cglib::bbox3<double> edgeBounds = cglib::bbox3<double>::smallest();
                edgeBounds.add(node0.points.begin(), node0.points.end());
                edgeBounds.add(node1.points.begin(), node1.points.end());
                for (int dim = 0; dim < 3; dim++) {
                    int idx = edgeBounds.center()(dim) < center(dim) ? 0 : 1;
                    splitBounds[dim][idx].add(edgeBounds);
                    splitEdgeIds[dim][idx].push_back(edgeId);
                }
            }

            // Find best splitting dimension (best means 'most balanced' by the number of edges here)
            int bestDim = 0;
            for (int dim = 1; dim < 3; dim++) {
                if (std::min(splitEdgeIds[dim][0].size(), splitEdgeIds[dim][1].size()) > std::min(splitEdgeIds[bestDim][0].size(), splitEdgeIds[bestDim][1].size())) {
                    bestDim = dim;
                }
            }

            // If splitting succeeded (resulting subtrees contain enough elements), recurse
            if (splitEdgeIds[bestDim][0].size() > SPLIT_THRESHOLD / 4 && splitEdgeIds[bestDim][1].size() > SPLIT_THRESHOLD / 4) {
                auto node = std::make_shared<RTreeNode>();
                node->bounds = bounds;
                node->subNodes[0] = buildRTree(splitBounds[bestDim][0], std::move(splitEdgeIds[bestDim][0]));
                node->subNodes[1] = buildRTree(splitBounds[bestDim][1], std::move(splitEdgeIds[bestDim][1]));
                return node;
            }
        }

        // Create final leaf node
        auto node = std::make_shared<RTreeNode>();
        node->bounds = bounds;
        node->edgeIds = std::move(edgeIds);
        return node;
    }

    void StaticGraph::linkNodeEdgeIds(std::vector<Node>& nodes, const std::vector<Edge>& edges) {
        for (std::size_t i = 0; i < nodes.size(); i++) {
            nodes[i].edgeIds.reserve(3); // 3 should be optimal in most cases
        }
        for (std::size_t i = 0; i < edges.size(); i++) {
            const Edge& edge = edges[i];
            Node& node0 = nodes.at(edge.nodeIds[0]);
            node0.edgeIds.push_back(static_cast<EdgeId>(i));
        }
    }

    double StaticGraph::calculateDistance(const Point& pos0, const Point& pos1, const cglib::vec3<double>& scale) {
        Point pos0Scaled = cglib::pointwise_product(pos0, scale);
        Point pos1Scaled = cglib::pointwise_product(pos1, scale);
        return cglib::length(pos0Scaled - pos1Scaled);
    }

    Graph::NodeId DynamicGraph::getNodeIdRangeEnd() const {
        NodeId nodeIdRangeEnd = _staticGraph->getNodeIdRangeEnd();
        for (const std::pair<NodeId, Node>& nodePair : _nodes) {
            if (nodePair.first >= nodeIdRangeEnd) {
                nodeIdRangeEnd = nodePair.first + 1;
            }
        }
        return nodeIdRangeEnd;
    }

    const Graph::Node& DynamicGraph::getNode(NodeId nodeId) const {
        auto it = _nodes.find(nodeId);
        if (it != _nodes.end()) {
            return it->second;
        }
        return _staticGraph->getNode(nodeId);
    }

    const Graph::Edge& DynamicGraph::getEdge(EdgeId edgeId) const {
        if (edgeId < _staticGraph->getEdgeIdRangeEnd()) {
            return _staticGraph->getEdge(edgeId);
        }
        return _edges.at(edgeId - _staticGraph->getEdgeIdRangeEnd());
    }

    const Graph::FeatureProperties& DynamicGraph::getFeatureProperties(FeatureId featureId) const {
        if (featureId < _staticGraph->getFeatureIdRangeEnd()) {
            return _staticGraph->getFeatureProperties(featureId);
        }
        return _featureProperties.at(featureId - _staticGraph->getFeatureIdRangeEnd());
    }

    const Graph::Attributes& DynamicGraph::getAttributes(AttributesId attribsId) const {
        if (attribsId < _staticGraph->getAttributesIdRangeEnd()) {
            return _staticGraph->getAttributes(attribsId);
        }
        return _attributes.at(attribsId - _staticGraph->getAttributesIdRangeEnd());
    }

    void DynamicGraph::reset() {
        _nodes.clear();
        _edges.clear();
        _featureProperties.clear();
        _attributes.clear();
    }

    Graph::NodeId DynamicGraph::addNode(Node node) {
        NodeId nodeId = getNodeIdRangeEnd();
        _nodes[nodeId] = std::move(node);
        return nodeId;
    }

    Graph::EdgeId DynamicGraph::addEdge(Edge edge) {
        EdgeId edgeId = getEdgeIdRangeEnd();
        Node node0 = getNode(edge.nodeIds[0]);
        node0.edgeIds.push_back(edgeId);
        _nodes[edge.nodeIds[0]] = std::move(node0);
        _edges.push_back(std::move(edge));
        return edgeId;
    }

    Graph::FeatureId DynamicGraph::addFeature(FeatureProperties properties) {
        FeatureId featureId = getFeatureIdRangeEnd();
        _featureProperties.push_back(std::move(properties));
        return featureId;
    }

    Graph::AttributesId DynamicGraph::addAttributes(Attributes attribs) {
        AttributesId attribsId = getAttributesIdRangeEnd();
        _attributes.push_back(std::move(attribs));
        return attribsId;
    }
} }

max_stars_repo_name: CartoDB/python-sgre

max_issues_repo_name: CartoDB/python-sgre

max_forks_repo_name: CartoDB/python-sgre


text: #include <boost/mpl/copy_if.hpp>

max_stars_repo_name: miathedev/BoostForArduino

max_issues_repo_name: miathedev/BoostForArduino

max_forks_repo_name: miathedev/BoostForArduino


text:
/*******************************************************************************
 *         Copyright 2003-2012 LASMEA UMR 6602 CNRS/U.B.P
 *         Copyright 2011-2012 LRI    UMR 8623 CNRS/Univ Paris Sud XI
 *
 *          Distributed under the Boost Software License, Version 1.0.
 *                 See accompanying file LICENSE.txt or copy at
 *                     http://www.boost.org/LICENSE_1_0.txt
 ******************************************************************************/
#ifndef NT2_TOOLBOX_LINALG_FUNCTIONS_DETAILS_LU_HPP_INCLUDED
#define NT2_TOOLBOX_LINALG_FUNCTIONS_DETAILS_LU_HPP_INCLUDED

#include <nt2/sdk/error/warning.hpp>
#include <nt2/include/functions/triu.hpp>
#include <nt2/include/functions/tri1l.hpp>
#include <nt2/include/functions/height.hpp>
#include <nt2/include/functions/width.hpp>
#include <nt2/include/functions/numel.hpp>
#include <nt2/include/functions/diag_of.hpp>
#include <nt2/include/functions/eye.hpp>
#include <nt2/include/functions/expand.hpp>
#include <nt2/include/functions/rif.hpp>
#include <nt2/include/functions/max.hpp>
#include <nt2/include/functions/rec.hpp>
#include <nt2/include/functions/norm.hpp>
#include <nt2/include/functions/prod.hpp>
#include <nt2/include/functions/eps.hpp>
#include <nt2/include/functions/frexp.hpp>
#include <nt2/include/functions/abs.hpp>
#include <nt2/include/functions/sb2b.hpp>
#include <nt2/include/functions/is_eqz.hpp>
#include <nt2/include/functions/isempty.hpp>
#include <nt2/include/functions/issquare.hpp>
#include <nt2/include/functions/if_one_else_zero.hpp>
#include <nt2/include/constants/eps.hpp>
#include <nt2/include/constants/zero.hpp>
#include <nt2/include/constants/one.hpp>
#include <nt2/toolbox/linalg/details/utility/options.hpp>
#include <nt2/toolbox/linalg/details/utility/workspace.hpp>
#include <nt2/toolbox/linalg/details/lapack/getrf.hpp>
#include <nt2/toolbox/linalg/details/lapack/gecon.hpp>
#include <nt2/toolbox/linalg/details/lapack/getri.hpp>
#include <nt2/toolbox/linalg/details/lapack/gesvx.hpp>
#include <nt2/toolbox/linalg/details/lapack/lange.hpp>
#include <boost/dispatch/details/ignore_unused.hpp>
#include <nt2/core/container/table/table.hpp>

// TODO:
// these are the kind of syntaxes to be enforced by nt2::chol
//  lu     lu factorization.
//     [l,u] = lu(a) stores an upper triangular matrix in u and a
//     "psychologically lower triangular matrix" (i.e. a product of lower
//     triangular and permutation matrices) in l, so that a = l*u. a can be
//     rectangular.
// -> {pl, u]

//     [l,u,p] = lu(a) returns unit lower triangular matrix l, upper
//     triangular matrix u, and permutation matrix p so that p*a = l*u.
// -> {l, u, p]

//     [l,u,p] = lu(a,'vector') returns the permutation information as a
//     vector instead of a matrix.  that is, p is a row vector such that
//     a(p,:) = l*u.  similarly, [l,u,p] = lu(a,'matrix') returns a
//     permutation matrix p.  this is the default behavior.
// -> {l, u, ip]

////////////////////////////////////////////////////////////////////////////////////////
// The class provides:
// constructor from an expression or matrix a
//
// accesors to l, u, p, pl and ip
//
// l is lower unittriangular part of lu_
// u is upper triangular part of lu_
// p is a permutation matrix
// pl is l permuted by p
// ip is the permutation index vector defining p
// we have l*u =  p*a
//         pl*u =  a
/////////////////////////////////////////////////// TODO  perhaps l and u could return an expresiion rather than a matrix
//
// the class allow to compute
//
// status the plu status from lapack
//
// rank the matrix rank
//
// When a is square
// det and absdet determinant and absolute value of the determinant
// They have two syntaxes
//  1 - d =  f.det()   or d =  f.absdet()
//  2 - m =  f.det(e)  or m =  f.absdet(e)
// In the second case m is a mantissa and e and exponent
// If there is no overflow or undeflow in e the determinant is then equal to ldexp(m, e)
//
// rcond the reciprocal condition number of a
//
// Two solvers
// solve and inplace_solve
//
// a matrix inversion
// inv
// inv() can emit a warning if rcond is bad
// inv(false) never warns
//     please avoid using inv if you need not the inverse coefficients.

namespace nt2 { namespace details
{
  template<class T> struct lu_result
  {
    typedef typename meta::strip<T>::type                   source_t;
    typedef typename source_t::value_type                     type_t;
    typedef typename meta::as_integer<type_t, signed>::type  itype_t;
    typedef typename source_t::index_type                    index_t;
    typedef typename meta::as_real<type_t>::type              base_t;
    typedef T                                                 data_t;
    typedef nt2::table<type_t,nt2::matlab_index_>              tab_t;
    typedef nt2::table<base_t,nt2::matlab_index_>             btab_t;
    typedef nt2::table<itype_t,nt2::matlab_index_>            itab_t;
    typedef nt2::details::workspace<type_t>              workspace_t;
    typedef nt2::table<nt2_la_int,nt2::matlab_index_>         ibuf_t;
    typedef nt2::table<type_t,index_t>                   result_type;

    template<class Input>
    lu_result ( Input& xpr
                , typename boost::
                disable_if_c<boost::is_same<lu_result,Input>::value>::type* = 0
      )
      : a_(xpr)
      , lu_(xpr)
      , m_( nt2::height(xpr) )
      , n_( nt2::width(xpr)  )
      , ldlu_( lu_.leading_size() )
      , ipiv_(nt2::of_size(nt2::min(n_, m_), 1))
      , info_(0)
      , p_(of_size(0, 1))
      , ip_(of_size(0, 1))
      , pl_(of_size(0, 1))
      , invt_(of_size(0, 1))
    {
      nt2::details::getrf(&m_, &n_, lu_.raw(), &ldlu_, ipiv_.raw(), &info_, w_);
    }

    lu_result(lu_result const& src)
      : a_(src.a_) , lu_(src.lu_), m_( src.m_ ), n_( src.n_ )
      , ldlu_( src.ldlu_ ) , ipiv_(src.ipiv_)
      , info_(src.info_) , w_(src.w_)
      , p_(src.p_), ip_(src.ip_), pl_(src.pl_), invt_(src.invt_)
    {}

    lu_result& operator=(lu_result const& src)
    {
      a_      = src.a_;
      lu_     = src.lu_;
      m_      = src.m_;
      n_      = src.n_;
      ldlu_   = src.ldlu_;
      ipiv_   = src.ipiv_;
      info_   = src.info_;
      w_      = src.w_;
      p_      = src.p_;
      ip_     = src.ip_;
      pl_     = src.pl_;
      invt_   = src.invt_;
      return *this;
    }

    //==========================================================================
    // Return raw values
    //==========================================================================
    const tab_t& values() const { return lu_; }

    //==========================================================================
    // Return raw values
    //==========================================================================
    const tab_t& original() const { return a_; }

    //==========================================================================
    // Return u part of the decomposition
    //==========================================================================
    typedef typename meta::call < tag::colon_(ptrdiff_t, ptrdiff_t)>::type                          u_T2;
    typedef typename meta::call < tag::function_(tab_t const&, u_T2, nt2::container::colon_)>::type u_T0;
    typedef typename meta::call < tag::triu_(u_T0)>::type                                           u_result;
    u_result u() const
    {
      ptrdiff_t mm =  std::min(n_, m_);
      return nt2::triu(lu_(_(ptrdiff_t(1), mm),_));
    }
    //==========================================================================
    // Return l part of the decomposition
    //==========================================================================
    typedef typename meta::call < tag::colon_(ptrdiff_t, ptrdiff_t)>::type                          l_T2;
    typedef typename meta::call < tag::function_(tab_t const&, nt2::container::colon_, l_T2)>::type l_T0;
    typedef typename meta::call < tag::tri1l_(l_T0)>::type                                          l_result;
    l_result l() const
    {
      ptrdiff_t mm =  std::min(n_, m_);
      return nt2::tri1l(lu_(_,_(ptrdiff_t(1), mm)));
    }

    //==========================================================================
    // Return p part of the decomposition as a matrix such that p*a = l*u
    //==========================================================================
    const tab_t& p()
    {
      if (isempty(p_))
      {
        std::size_t mm = nt2::numel(ipiv_);
        p_ = nt2::eye(mm, mm, meta::as_<type_t>());
        for(size_t i=1; i <= mm; ++i)
          // p_({i, ipiv_(i)}, _) =  p_({ipiv_(i),i}, _)
        {
          tab_t c = p_(i, _);
          p_(i,_) = p_(ipiv_(i),_);
          p_(ipiv_(i),_) = c;
        }
      }
      return p_;
    }
    //==========================================================================
    // Return p part of the decomposition as a vector
    //==========================================================================
    const itab_t& ip()
    {
      if (isempty(ip_))
      {
        //      itab_t ip = itab_t(ipiv_.raw(), ipiv_.raw()+numel(ipiv_));
        ip_.resize(of_size(1, numel(ipiv_)));
        for(size_t i=1; i <= numel(ipiv_); ++i) ip_(i) = ipiv_(i);
      }
      return ip_;
    }

    //==========================================================================
    // Return tpl part of the decomposition  a =  tpl*u (pl = tp *l)
    //==========================================================================
    const tab_t& pl()
    {
      if (isempty(pl_))
      {
        //    return trans(p())*l;
        std::size_t mm = nt2::numel(ipiv_); //incorrect
        pl_ = l();
        for(size_t i=1; i <= mm; ++i)
          // pp({i, ipiv_(i)}, _) =  pp({ipiv_(i),i}, _)
        {
          tab_t c = pl_(ipiv_(i), _);
          pl_(ipiv_(i),_) = pl_(i,_);
          pl_(i,_) = c;
        }
      }
      return pl_;
    }

    //==========================================================================
    // Return post-computation status
    //==========================================================================
    nt2_la_int  status() const { return info_; }

    //==========================================================================
    // Reverse conditioning evaluation
    //==========================================================================
    base_t rcond(char c = '1')
    {
      /* this method which is presumably faster provides results that depend on
      // the lapack used version
      // it seems that gecon is buggy (J.T.L. 28/2/2013)
      //
      //   base_t rc = 0;
      //   tab_t aa = a_;
      //   char norm = (c == 1) ? '1' : ((c == 0) ? 'o' : c);
      //   base_t anorm = nt2::details::lange(&norm,  &n_,  &n_, aa.raw(), &ldlu_);
      //   nt2::details::gecon(&norm, &n_,  lu_.raw(), &ldlu_, &anorm, &rc, &info_);
      //   return rc;
      //
        So we switch to a direct computation
      */
      return nt2::rec(nt2::norm(a_, c)*nt2::norm(this->inv(false), c));
    }

    //==========================================================================
    // system rank
    //==========================================================================
    size_t rank(base_t epsi = nt2::Eps<base_t>()) //provisouare
    {
      //int32_t r = 0;
      base_t thresh = nt2::max(n_, m_)*epsi*nt2::max(nt2::abs(nt2::diag_of(lu_)(_)));
      return  size_t(sum(if_one_else_zero(gt(nt2::diag_of(lu_), thresh))(_)));

      //       for(int i=1; i <= nt2::min(n_, m_); ++i)
      //         {
      //           if(nt2::abs(lu_(i, i)) > thresh) ++r;
      //         }
      //       return r;
      //      nt2::inbtrue(nt2::abs(diag_of(lu_)) > nt2::max(n_, m_)*epsi*nt2::max(abs(diag_of(lu_()))) );
    }

    base_t absdet()
    {
      BOOST_ASSERT_MSG(m_ == n_, "non square matrix in determinant computation");
      return nt2::prod(nt2::abs(nt2::diag_of(lu_)(_)));
    }

    type_t signdet(bool check = true){
      BOOST_ASSERT_MSG(m_ == n_, "non square matrix in determinant computation");
      boost::dispatch::ignore_unused(check);
      //if (check)     BOOST_ASSERT_MSG(is_real<type_t>::value, "determinant sign is not avalaible for complex matrices");
      //count modulo 2 the number of ipiv_ elements such that ipiv_(i) !=  i
      //return nt2::sum(nt2::sb2b(ipiv_ != cif(numel(ipiv_), 1, meta::as_<itype_t>())))&1 ? Mone<type_t>() : One<type_t>();
      type_t s = One<type_t>();
      const nt2_la_int num = numel(ipiv_);
      for(nt2_la_int i=1; i < num ; ++i)
      {
        if (ipiv_(i) !=  i) s = -s;
      }
      return s;
    }

    type_t det(){
      BOOST_ASSERT_MSG(m_ == n_, "non square matrix in determinant computation");
      //     BOOST_ASSERT_MSG(is_real<type_t>::value, "determinant sign is not avalaible for complex matrices");
      //count modulo 2 the number of ipiv_ elements such that ipiv_(i) !=  i
      return  nt2::prod(nt2::diag_of(lu_)(_))*signdet(false);
    }

    type_t absdet(itype_t & exponent)
    {
      BOOST_ASSERT_MSG(m_ ==  n_, "non square matrix in determinant computation");
      // compute e and return m for matrix determinant such that |det| = ldexp(m, e)
      // if no overflow or underflow can occur,  with 0.5 < abs(m) < 1
      // the exponent result is enough to know the order of magnitude of the determinant
      // (between 0.5*2^e and 2^e if the mantissa is non zero)
      // This routine is inspired from linpack http://www.netlib.org/linpack/dgedi.f
      // that use ten power factor instead
      type_t   m1 = One<type_t>();
      exponent = Zero<itype_t>();
      for(size_t i = 1;  i <= size_t(n_); ++i)
      {
        itype_t e;
        m1 *=  nt2::abs(nt2::frexp(nt2::abs(lu_(i, i)), e));
        exponent+= e;
      }
      if (is_eqz(m1)){
        exponent = Zero<itype_t>();
      }
      return m1;
    }

    type_t det(itype_t & exponent)
    {
      BOOST_ASSERT_MSG(m_ ==  n_, "non square matrix in determinant computation");
      // compute e and m for matrix determinant such that det = ldexp(m, e)
      // if no overflow or underflow can occur,  with 0.5 < abs(m) < 1
      // the exponent result is enough to know the order of magnitude of the determinant
      // (between 0.5*2^e and 2^e if the mantissa is non zero)
      // This routine is inspired from linpack http://www.netlib.org/linpack/dgedi.f
      // that use ten power factor instead
      type_t   m1 = One<type_t>();
      exponent = Zero<itype_t>();
      for(size_t i = 1;  i <= size_t(n_); ++i)
      {
        itype_t e;
        m1 *=  nt2::frexp(nt2::abs(lu_(i, i)), e);
        exponent+= e;
      }
      if (is_eqz(m1)){
        exponent = Zero<itype_t>();
      }
      return m1*signdet(false);
    }

    //==========================================================================
    // Solver interface
    //==========================================================================
    template<class XPR> result_type solve(const XPR& b )
    {
      result_type bb = b;
      inplace_solve(bb);
      return bb;
    }

    //==========================================================================
    // inverse matrix: DO NOT USE THAT TO SOLVE A SYSTEM
    //==========================================================================
    const tab_t& inv(bool warn = true)
    {
      if(isempty(invt_))
      {
        if (warn)
        {
          base_t rc = rcond();
          NT2_WARNING ( (rc >= nt2::Eps<base_t>())
                        , "Matrix is close to singular or badly scaled."
                        " Results may be inaccurate."
            );
          return invt_;  /* it has been calculated by rcond */
        }
        invt_ = lu_;
        nt2::details::getri(&n_, invt_.raw(), &ldlu_, ipiv_.raw(), &info_, w_);
      }
      return invt_;
    }

    template<class Xpr> void inplace_solve(Xpr& b )
    {
      BOOST_ASSERT_MSG(issquare(a_), "matrix must be square to use the lu solver");
      nt2_la_int nrhs = nt2::size(b, 2);
      nt2_la_int ldb  = b.leading_size();
      tab_t x(b);
      nt2_la_int ldx  = x.leading_size();
      btab_t ferr(of_size(nrhs, 1)), berr(of_size(nrhs, 1));
      btab_t r_(of_size(n_, 1)), c_(of_size(n_, 1));
      char equed = 'N';
      base_t rc = rcond();
      nt2::details::gesvx(nt2::details::lapack_option('F'),
                          nt2::details::lapack_option('N'),
                          &n_, &nrhs,
                          a_.raw(), &ldlu_,
                          lu_.raw(), &ldlu_,
                          ipiv_.raw(),
                          &equed,
                          r_.raw(),
                          c_.raw(),
                          b.raw(), &ldb,
                          x.raw(), &ldx,
                          &rc,
                          ferr.raw(),
                          berr.raw(),
                          &info_,
                          w_);
      b = x;
    }

  private:
    data_t                            a_;
    tab_t                            lu_;
    nt2_la_int                     m_,n_;
    nt2_la_int                     ldlu_;
    ibuf_t                         ipiv_;
    nt2_la_int                     info_;
    workspace_t                       w_;
    tab_t                             p_;
    itab_t                           ip_;
    tab_t                            pl_;
    tab_t                          invt_;
  };
} }

#endif


text:
// __BEGIN_LICENSE__
//  Copyright (c) 2009-2013, United States Government as represented by the
//  Administrator of the National Aeronautics and Space Administration. All
//  rights reserved.
//
//  The NGT platform is 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.
// __END_LICENSE__

#include <vw/Core/Exception.h>          // for ArgumentErr, vw_throw, etc
#include <vw/Math/Quaternion.h>         // for Quat, Quaternion
#include <vw/Math/Vector.h>             // for Vector, Vector3, Vector4, etc
#include <vw/Cartography/Datum.h>       // for Datum

#include <asp/Sessions/DG/XML.h>
#include <asp/Sessions/RPC/RPCModel.h>

#include <xercesc/parsers/XercesDOMParser.hpp>
#include <xercesc/sax/HandlerBase.hpp>
#include <xercesc/util/PlatformUtils.hpp>

#include <boost/algorithm/string/predicate.hpp>
#include <boost/filesystem.hpp>
#include <boost/lexical_cast.hpp>

using namespace vw;
using namespace xercesc;

namespace fs=boost::filesystem;

void asp::ImageXML::parse_meta( xercesc::DOMElement* node ) {
  // Note: dg_mosaic wipes most image tags. If it is desired to parse
  // more tags here, ensure they are not wiped by dg_mosaic and use
  // sensible values when creating them for the combined mosaics.
  cast_xmlch( get_node<DOMElement>( node, "SATID" )->getTextContent(),
              sat_id );
  cast_xmlch( get_node<DOMElement>( node, "SCANDIRECTION" )->getTextContent(),
              scan_direction );
  cast_xmlch( get_node<DOMElement>( node, "TLCTIME" )->getTextContent(),
              tlc_start_time );
  cast_xmlch( get_node<DOMElement>( node, "FIRSTLINETIME" )->getTextContent(),
              first_line_start_time );
  cast_xmlch( get_node<DOMElement>( node, "AVGLINERATE" )->getTextContent(),
              avg_line_rate );
  size_t num_tlc;
  cast_xmlch( get_node<DOMElement>( node, "NUMTLC" )->getTextContent(),
              num_tlc );
  tlc_vec.resize( num_tlc );
}

void asp::ImageXML::parse_band_p( xercesc::DOMElement* node ) {
  cast_xmlch( get_node<DOMElement>( node, "TDILEVEL" )->getTextContent(),
              tdi );
}

void asp::ImageXML::parse_tlc_list( xercesc::DOMElement* node ) {
  DOMNodeList* children = node->getChildNodes();
  size_t count = 0;

  for ( XMLSize_t i = 0; i < children->getLength(); i++ ) {
    if ( children->item(i)->getNodeType() == DOMNode::ELEMENT_NODE ) {
      DOMElement* element = dynamic_cast<DOMElement*>( children->item(i) );
      std::string buffer;
      cast_xmlch( element->getTextContent(), buffer );

      std::istringstream istr( buffer );
      istr >> tlc_vec[count].first >> tlc_vec[count].second;

      count++;
    }
  }

  VW_ASSERT( count == tlc_vec.size(),
             IOErr() << "Read incorrect number of TLC." );
}

void asp::ImageXML::parse_image_size( xercesc::DOMElement* node ) {
  cast_xmlch( get_node<DOMElement>( node, "NUMROWS" )->getTextContent(),
              image_size[1] );
  cast_xmlch( get_node<DOMElement>( node, "NUMCOLUMNS" )->getTextContent(),
              image_size[0] );
}

asp::ImageXML::ImageXML() : XMLBase(4) {}

void asp::ImageXML::parse( xercesc::DOMElement* node ) {
  DOMElement* image = get_node<DOMElement>( node, "IMAGE" );
  parse_meta( image );
  check_argument(0);

  tdi = 0;
  try{
    DOMElement* band_p = get_node<DOMElement>( node, "BAND_P" );
    parse_band_p( band_p );
  }catch(...){
    // An XML file after being processed by dg_mosaic may not have the tdi field.
  }
  check_argument(1);

  parse_tlc_list( get_node<DOMElement>( image, "TLCLISTList" ) );
  check_argument(2);

  parse_image_size( node );
  check_argument(3);
}

void asp::GeometricXML::parse_principal_distance( xercesc::DOMElement* node ) {
  cast_xmlch( get_node<DOMElement>( node, "PD" )->getTextContent(),
              principal_distance );
}

void asp::GeometricXML::parse_optical_distortion( xercesc::DOMElement* node ) {
  cast_xmlch( get_node<DOMElement>( node, "POLYORDER" )->getTextContent(),
              optical_polyorder );

  // If the optical polyorder is >= 1 and some of the polynomial
  // coefficients are non-zero, then throw an error as we did not
  // implement optical distortion.

  if ( optical_polyorder <= 0 ) return; // it actually can be negative

  std::string list_list_types[] = {"ALISTList", "BLISTList"};
  for (unsigned ls = 0; ls < sizeof(list_list_types)/sizeof(std::string); ls++){
    std::string list_list_type = list_list_types[ls];

    DOMElement* list_list = get_node<DOMElement>( node, list_list_type );
    if (list_list == NULL) return;

    DOMNodeList* children = list_list->getChildNodes();
    if (children == NULL) return;

    for ( XMLSize_t i = 0; i < children->getLength(); ++i ) {

      DOMNode* current = children->item(i);
      if ( current->getNodeType() != DOMNode::ELEMENT_NODE ) continue;

      DOMElement* element = dynamic_cast< xercesc::DOMElement* >( current );
      std::string buffer;
      cast_xmlch( element->getTextContent(), buffer );

      std::istringstream istr( buffer );
      double val;
      while (istr >> val){
        if (val != 0)
          vw_throw( NoImplErr() << "Optical distortion is not implemented.\n" );
      }
    }
  }

}

void asp::GeometricXML::parse_perspective_center( xercesc::DOMElement* node ) {
  cast_xmlch( get_node<DOMElement>( node, "CX" )->getTextContent(),
              perspective_center[0] );
  cast_xmlch( get_node<DOMElement>( node, "CY" )->getTextContent(),
              perspective_center[1] );
  cast_xmlch( get_node<DOMElement>( node, "CZ" )->getTextContent(),
              perspective_center[2] );
}

void asp::GeometricXML::parse_camera_attitude( xercesc::DOMElement* node ) {
  Vector4 buffer; // We buffer since Q will want to constantly
  // renormalize itself.
  cast_xmlch( get_node<DOMElement>( node, "QCS4" )->getTextContent(),
              buffer[0] );
  cast_xmlch( get_node<DOMElement>( node, "QCS1" )->getTextContent(),
              buffer[1] );
  cast_xmlch( get_node<DOMElement>( node, "QCS2" )->getTextContent(),
              buffer[2] );
  cast_xmlch( get_node<DOMElement>( node, "QCS3" )->getTextContent(),
              buffer[3] );
  camera_attitude = Quat(buffer[0],buffer[1],buffer[2],buffer[3]);
}

void asp::GeometricXML::parse_detector_mounting( xercesc::DOMElement* node ) {

  DOMNodeList* children = node->getChildNodes();

  // Read through all the bands, storing their data
  std::vector<Vector4> bandVec;
  for ( XMLSize_t i = 0; i < children->getLength(); i++ ) {
    DOMNode* current = children->item(i);
    if ( current->getNodeType() == DOMNode::ELEMENT_NODE ) {
      DOMElement* element =
        dynamic_cast< DOMElement* > (current);

      std::string tag( XMLString::transcode(element->getTagName()) );
      if (boost::starts_with( tag, "BAND_" )) {
        Vector4 data;

        DOMElement* detector = get_node<DOMElement>( element, "DETECTOR_ARRAY" );
        cast_xmlch( get_node<DOMElement>(detector,"DETORIGINX")->getTextContent(),
                    data[0] );
        cast_xmlch( get_node<DOMElement>(detector,"DETORIGINY")->getTextContent(),
                    data[1] );
        cast_xmlch( get_node<DOMElement>(detector,"DETROTANGLE")->getTextContent(),
                    data[2] );
        cast_xmlch( get_node<DOMElement>(detector,"DETPITCH")->getTextContent(),
                    data[3] );
        bandVec.push_back(data);
      }
    }
  }

  if (bandVec.empty()){
    vw_throw(ArgumentErr() << "Could not find any bands in the "
             << "DETECTOR_MOUNTING section of the XML file.\n");
  }

  // If there are multiple bands, they must have the same values
  for (int i = 1; i < (int)bandVec.size(); i++){
    if (bandVec[0] != bandVec[i]){
      vw_throw(ArgumentErr() << "XML files with multiple and "
               << "distinct detector arrays in the DETECTOR_MOUNTING "
               << "section of the XML file are not supported.\n");
    }
  }

  Vector4 data = bandVec[0];
  detector_origin[0]   = data[0];
  detector_origin[1]   = data[1];
  detector_rotation    = data[2];
  detector_pixel_pitch = data[3];

}

asp::GeometricXML::GeometricXML() : XMLBase(5) {}

void asp::GeometricXML::parse( xercesc::DOMElement* node ) {
  DOMNodeList* children = node->getChildNodes();
  const XMLSize_t nodeCount = children->getLength();

  for ( XMLSize_t i = 0; i < nodeCount; ++i ) {
    DOMNode* current = children->item(i);
    if ( current->getNodeType() == DOMNode::ELEMENT_NODE ) {
      DOMElement* element =
        dynamic_cast< xercesc::DOMElement* >( current );

      std::string tag( XMLString::transcode(element->getTagName()) );
      if ( tag == "PRINCIPAL_DISTANCE" ) {
        parse_principal_distance( element );
        check_argument(0);
      } else if ( tag == "OPTICAL_DISTORTION" ) {
        parse_optical_distortion( element );
        check_argument(1);
      } else if ( tag == "PERSPECTIVE_CENTER" ) {
        parse_perspective_center( element );
        check_argument(2);
      } else if ( tag == "CAMERA_ATTITUDE" ) {
        parse_camera_attitude( element );
        check_argument(3);
      } else if ( tag == "DETECTOR_MOUNTING" ) {
        parse_detector_mounting( element );
        check_argument(4);
      }
    }
  }
}

void asp::EphemerisXML::parse_meta( xercesc::DOMElement* node ) {
  cast_xmlch( get_node<DOMElement>( node, "STARTTIME" )->getTextContent(),
              start_time );
  cast_xmlch( get_node<DOMElement>( node, "TIMEINTERVAL" )->getTextContent(),
              time_interval );
  size_t num_points;
  cast_xmlch( get_node<DOMElement>( node, "NUMPOINTS" )->getTextContent(),
              num_points );
  position_vec.resize( num_points );
  velocity_vec.resize( num_points );
  covariance_vec.resize( num_points );
}

void asp::EphemerisXML::parse_eph_list( xercesc::DOMElement* node ) {
  DOMNodeList* children = node->getChildNodes();
  size_t count = 0;

  for ( XMLSize_t i = 0; i < children->getLength(); i++ ) {
    if ( children->item(i)->getNodeType() == DOMNode::ELEMENT_NODE ) {
      DOMElement* element = dynamic_cast<DOMElement*>( children->item(i) );
      std::string buffer;
      cast_xmlch( element->getTextContent(), buffer );

      std::istringstream istr( buffer );
      std::string index_b;
      istr >> index_b;
      size_t index;
      try{
        index = size_t(boost::lexical_cast<float>(index_b) + 0.5) - 1;
      } catch (boost::bad_lexical_cast const& e) {
        vw_throw(ArgumentErr() << "Failed to parse string: " << index_b << "\n");
      }

      istr >> position_vec[index][0] >> position_vec[index][1]
           >> position_vec[index][2] >> velocity_vec[index][0]
           >> velocity_vec[index][1] >> velocity_vec[index][2];
      istr >> covariance_vec[index][0] >> covariance_vec[index][1]
           >> covariance_vec[index][2] >> covariance_vec[index][3]
           >> covariance_vec[index][4] >> covariance_vec[index][5];

      count++;
    }
  }

  VW_ASSERT( count == position_vec.size(),
             IOErr() << "Read incorrect number of points." );
}

asp::EphemerisXML::EphemerisXML() : XMLBase(2) {}

void asp::EphemerisXML::parse( xercesc::DOMElement* node ) {
  parse_meta( node );
  check_argument(0);

  parse_eph_list( get_node<DOMElement>( node, "EPHEMLISTList" ) );
  check_argument(1);
}

void asp::AttitudeXML::parse_meta( xercesc::DOMElement* node ) {
  cast_xmlch( get_node<DOMElement>( node, "STARTTIME" )->getTextContent(),
              start_time );
  cast_xmlch( get_node<DOMElement>( node, "TIMEINTERVAL" )->getTextContent(),
              time_interval );
  size_t num_points;
  cast_xmlch( get_node<DOMElement>( node, "NUMPOINTS" )->getTextContent(),
              num_points );
  quat_vec.resize( num_points );
  covariance_vec.resize( num_points );
}

void asp::AttitudeXML::parse_att_list( xercesc::DOMElement* node ) {
  DOMNodeList* children = node->getChildNodes();
  size_t count = 0;
  Vector4 qbuf;

  for ( XMLSize_t i = 0; i < children->getLength(); i++ ) {
    if ( children->item(i)->getNodeType() == DOMNode::ELEMENT_NODE ) {
      DOMElement* element = dynamic_cast<DOMElement*>( children->item(i) );
      std::string buffer;
      cast_xmlch( element->getTextContent(), buffer );

      std::istringstream istr( buffer );
      std::string index_b;
      istr >> index_b;
      size_t index;
      try {
        index = size_t(boost::lexical_cast<float>(index_b) + 0.5) - 1;
      } catch (boost::bad_lexical_cast const& e) {
        vw_throw(ArgumentErr() << "Failed to parse string: " << index_b << "\n");
      }

      istr >> qbuf[0] >> qbuf[1] >> qbuf[2] >> qbuf[3];
      quat_vec[index] = Quat(qbuf[3], qbuf[0], qbuf[1], qbuf[2] );
      istr >> covariance_vec[index][0] >> covariance_vec[index][1]
           >> covariance_vec[index][2] >> covariance_vec[index][3]
           >> covariance_vec[index][4] >> covariance_vec[index][5]
           >> covariance_vec[index][6] >> covariance_vec[index][7]
           >> covariance_vec[index][8] >> covariance_vec[index][9];

      count++;
    }
  }

  VW_ASSERT( count == quat_vec.size(),
             IOErr() << "Read incorrect number of points." );
}

asp::AttitudeXML::AttitudeXML() : XMLBase(2) {}

void asp::AttitudeXML::parse( xercesc::DOMElement* node ) {
  parse_meta( node );
  check_argument(0);

  parse_att_list( get_node<DOMElement>( node, "ATTLISTList" ) );
  check_argument(1);
}

void asp::RPCXML::parse_vector( xercesc::DOMElement* node,
                                Vector<double,20>& vec ) {
  std::string buffer;
  cast_xmlch( node->getTextContent(), buffer );

  std::istringstream istr( buffer );
  istr >> vec[0] >> vec[1] >> vec[2] >> vec[3] >> vec[4] >> vec[5]
       >> vec[6] >> vec[7] >> vec[8] >> vec[9] >> vec[10] >> vec[11]
       >> vec[12] >> vec[13] >> vec[14] >> vec[15] >> vec[16] >> vec[17]
       >> vec[18] >> vec[19];
}

asp::RPCXML::RPCXML() : XMLBase(2) {}

void asp::RPCXML::read_from_file( std::string const& name ) {
  boost::scoped_ptr<XercesDOMParser> parser( new XercesDOMParser() );
  parser->setValidationScheme(XercesDOMParser::Val_Always);
  parser->setDoNamespaces(true);
  boost::scoped_ptr<ErrorHandler> errHandler( new HandlerBase() );
  parser->setErrorHandler(errHandler.get());

  DOMDocument* xmlDoc;
  DOMElement* elementRoot;

  try{
    parser->parse( name.c_str() );
    xmlDoc = parser->getDocument();
    elementRoot = xmlDoc->getDocumentElement();
  }catch(...){
    vw_throw( ArgumentErr() << "XML file \"" << name << "\" is invalid.\n" );
  }

  try {
    parse_rpb( get_node<DOMElement>( elementRoot, "RPB" ) );
    return;
  } catch ( vw::IOErr const& e ) {
    // Possibly RPB doesn't exist
  }
  try {
    parse_rational_function_model( get_node<DOMElement>( elementRoot, "Rational_Function_Model" ) );
    return;
  } catch ( vw::IOErr const& e ) {
    // Possibly Rational_Function_Model doesn't work
  }
  vw_throw( vw::NotFoundErr() << "Couldn't find RPB or Rational_Function_Model tag inside XML file." );
}

void asp::RPCXML::parse_rpb( xercesc::DOMElement* node ) {
  DOMElement* image = get_node<DOMElement>( node, "IMAGE" );

  // Pieces that will go into the RPC Model
  Vector<double,20> line_num_coeff, line_den_coeff, samp_num_coeff, samp_den_coeff;
  Vector2 xy_offset, xy_scale;
  Vector3 geodetic_offset, geodetic_scale;

  // Painfully extract from the XML
  cast_xmlch( get_node<DOMElement>( image, "SAMPOFFSET" )->getTextContent(),
              xy_offset.x() );
  cast_xmlch( get_node<DOMElement>( image, "LINEOFFSET" )->getTextContent(),
              xy_offset.y() );
  cast_xmlch( get_node<DOMElement>( image, "SAMPSCALE" )->getTextContent(),
              xy_scale.x() );
  cast_xmlch( get_node<DOMElement>( image, "LINESCALE" )->getTextContent(),
              xy_scale.y() );
  cast_xmlch( get_node<DOMElement>( image, "LONGOFFSET" )->getTextContent(),
              geodetic_offset.x() );
  cast_xmlch( get_node<DOMElement>( image, "LATOFFSET" )->getTextContent(),
              geodetic_offset.y() );
  cast_xmlch( get_node<DOMElement>( image, "HEIGHTOFFSET" )->getTextContent(),
              geodetic_offset.z() );
  cast_xmlch( get_node<DOMElement>( image, "LONGSCALE" )->getTextContent(),
              geodetic_scale.x() );
  cast_xmlch( get_node<DOMElement>( image, "LATSCALE" )->getTextContent(),
              geodetic_scale.y() );
  cast_xmlch( get_node<DOMElement>( image, "HEIGHTSCALE" )->getTextContent(),
              geodetic_scale.z() );
  check_argument(0);
  parse_vector( get_node<DOMElement>(get_node<DOMElement>( image, "LINENUMCOEFList" ), "LINENUMCOEF" ),
                line_num_coeff );
  parse_vector( get_node<DOMElement>(get_node<DOMElement>( image, "LINEDENCOEFList" ), "LINEDENCOEF" ),
                line_den_coeff );
  parse_vector( get_node<DOMElement>(get_node<DOMElement>( image, "SAMPNUMCOEFList" ), "SAMPNUMCOEF" ),
                samp_num_coeff );
  parse_vector( get_node<DOMElement>(get_node<DOMElement>( image, "SAMPDENCOEFList" ), "SAMPDENCOEF" ),
                samp_den_coeff );
  check_argument(1);

  // Push into the RPC Model so that it is easier to work with
  //
  // The choice of using the WGS84 datum comes from Digital Globe's
  // QuickBird Imagery Products : Products Guide. Section 11.1 makes a
  // blanket statement that all heights are meters against the WGS 84
  // ellipsoid.
  m_rpc.reset( new RPCModel( cartography::Datum("WGS84"),
                             line_num_coeff, line_den_coeff,
                             samp_num_coeff, samp_den_coeff,
                             xy_offset, xy_scale,
                             geodetic_offset, geodetic_scale ) );
}

void asp::RPCXML::parse_rational_function_model( xercesc::DOMElement* node ) {
  DOMElement* inverse_model =
    get_node<DOMElement>( node, "Inverse_Model" ); // Inverse model
                                                   // takes ground to
                                                   // image.
  DOMElement* rfmvalidity  = get_node<DOMElement>( node, "RFM_Validity" );

  // Pieces that will go into the RPC Model
  Vector<double,20> line_num_coeff, line_den_coeff, samp_num_coeff, samp_den_coeff;
  Vector2 xy_offset, xy_scale;
  Vector3 geodetic_offset, geodetic_scale;

  // Painfully extract the single values from the XML data
  cast_xmlch( get_node<DOMElement>( inverse_model, "SAMP_NUM_COEFF_1")->getTextContent(), samp_num_coeff[0] );
  cast_xmlch( get_node<DOMElement>( inverse_model, "SAMP_NUM_COEFF_2")->getTextContent(), samp_num_coeff[1] );
  cast_xmlch( get_node<DOMElement>( inverse_model, "SAMP_NUM_COEFF_3")->getTextContent(), samp_num_coeff[2] );
  cast_xmlch( get_node<DOMElement>( inverse_model, "SAMP_NUM_COEFF_4")->getTextContent(), samp_num_coeff[3] );
  cast_xmlch( get_node<DOMElement>( inverse_model, "SAMP_NUM_COEFF_5")->getTextContent(), samp_num_coeff[4] );
  cast_xmlch( get_node<DOMElement>( inverse_model, "SAMP_NUM_COEFF_6")->getTextContent(), samp_num_coeff[5] );
  cast_xmlch( get_node<DOMElement>( inverse_model, "SAMP_NUM_COEFF_7")->getTextContent(), samp_num_coeff[6] );
  cast_xmlch( get_node<DOMElement>( inverse_model, "SAMP_NUM_COEFF_8")->getTextContent(), samp_num_coeff[7] );
  cast_xmlch( get_node<DOMElement>( inverse_model, "SAMP_NUM_COEFF_9")->getTextContent(), samp_num_coeff[8] );
  cast_xmlch( get_node<DOMElement>( inverse_model, "SAMP_NUM_COEFF_10")->getTextContent(), samp_num_coeff[9] );
  cast_xmlch( get_node<DOMElement>( inverse_model, "SAMP_NUM_COEFF_11")->getTextContent(), samp_num_coeff[10] );
  cast_xmlch( get_node<DOMElement>( inverse_model, "SAMP_NUM_COEFF_12")->getTextContent(), samp_num_coeff[11] );
  cast_xmlch( get_node<DOMElement>( inverse_model, "SAMP_NUM_COEFF_13")->getTextContent(), samp_num_coeff[12] );
  cast_xmlch( get_node<DOMElement>( inverse_model, "SAMP_NUM_COEFF_14")->getTextContent(), samp_num_coeff[13] );
  cast_xmlch( get_node<DOMElement>( inverse_model, "SAMP_NUM_COEFF_15")->getTextContent(), samp_num_coeff[14] );
  cast_xmlch( get_node<DOMElement>( inverse_model, "SAMP_NUM_COEFF_16")->getTextContent(), samp_num_coeff[15] );
  cast_xmlch( get_node<DOMElement>( inverse_model, "SAMP_NUM_COEFF_17")->getTextContent(), samp_num_coeff[16] );
  cast_xmlch( get_node<DOMElement>( inverse_model, "SAMP_NUM_COEFF_18")->getTextContent(), samp_num_coeff[17] );
  cast_xmlch( get_node<DOMElement>( inverse_model, "SAMP_NUM_COEFF_19")->getTextContent(), samp_num_coeff[18] );
  cast_xmlch( get_node<DOMElement>( inverse_model, "SAMP_NUM_COEFF_20")->getTextContent(), samp_num_coeff[19] );

  cast_xmlch( get_node<DOMElement>( inverse_model, "SAMP_DEN_COEFF_1")->getTextContent(), samp_den_coeff[0] );
  cast_xmlch( get_node<DOMElement>( inverse_model, "SAMP_DEN_COEFF_2")->getTextContent(), samp_den_coeff[1] );
  cast_xmlch( get_node<DOMElement>( inverse_model, "SAMP_DEN_COEFF_3")->getTextContent(), samp_den_coeff[2] );
  cast_xmlch( get_node<DOMElement>( inverse_model, "SAMP_DEN_COEFF_4")->getTextContent(), samp_den_coeff[3] );
  cast_xmlch( get_node<DOMElement>( inverse_model, "SAMP_DEN_COEFF_5")->getTextContent(), samp_den_coeff[4] );
  cast_xmlch( get_node<DOMElement>( inverse_model, "SAMP_DEN_COEFF_6")->getTextContent(), samp_den_coeff[5] );
  cast_xmlch( get_node<DOMElement>( inverse_model, "SAMP_DEN_COEFF_7")->getTextContent(), samp_den_coeff[6] );
  cast_xmlch( get_node<DOMElement>( inverse_model, "SAMP_DEN_COEFF_8")->getTextContent(), samp_den_coeff[7] );
  cast_xmlch( get_node<DOMElement>( inverse_model, "SAMP_DEN_COEFF_9")->getTextContent(), samp_den_coeff[8] );
  cast_xmlch( get_node<DOMElement>( inverse_model, "SAMP_DEN_COEFF_10")->getTextContent(), samp_den_coeff[9] );
  cast_xmlch( get_node<DOMElement>( inverse_model, "SAMP_DEN_COEFF_11")->getTextContent(), samp_den_coeff[10] );
  cast_xmlch( get_node<DOMElement>( inverse_model, "SAMP_DEN_COEFF_12")->getTextContent(), samp_den_coeff[11] );
  cast_xmlch( get_node<DOMElement>( inverse_model, "SAMP_DEN_COEFF_13")->getTextContent(), samp_den_coeff[12] );
  cast_xmlch( get_node<DOMElement>( inverse_model, "SAMP_DEN_COEFF_14")->getTextContent(), samp_den_coeff[13] );
  cast_xmlch( get_node<DOMElement>( inverse_model, "SAMP_DEN_COEFF_15")->getTextContent(), samp_den_coeff[14] );
  cast_xmlch( get_node<DOMElement>( inverse_model, "SAMP_DEN_COEFF_16")->getTextContent(), samp_den_coeff[15] );
  cast_xmlch( get_node<DOMElement>( inverse_model, "SAMP_DEN_COEFF_17")->getTextContent(), samp_den_coeff[16] );
  cast_xmlch( get_node<DOMElement>( inverse_model, "SAMP_DEN_COEFF_18")->getTextContent(), samp_den_coeff[17] );
  cast_xmlch( get_node<DOMElement>( inverse_model, "SAMP_DEN_COEFF_19")->getTextContent(), samp_den_coeff[18] );
  cast_xmlch( get_node<DOMElement>( inverse_model, "SAMP_DEN_COEFF_20")->getTextContent(), samp_den_coeff[19] );

  cast_xmlch( get_node<DOMElement>( inverse_model, "LINE_NUM_COEFF_1")->getTextContent(), line_num_coeff[0] );
  cast_xmlch( get_node<DOMElement>( inverse_model, "LINE_NUM_COEFF_2")->getTextContent(), line_num_coeff[1] );
  cast_xmlch( get_node<DOMElement>( inverse_model, "LINE_NUM_COEFF_3")->getTextContent(), line_num_coeff[2] );
  cast_xmlch( get_node<DOMElement>( inverse_model, "LINE_NUM_COEFF_4")->getTextContent(), line_num_coeff[3] );
  cast_xmlch( get_node<DOMElement>( inverse_model, "LINE_NUM_COEFF_5")->getTextContent(), line_num_coeff[4] );
  cast_xmlch( get_node<DOMElement>( inverse_model, "LINE_NUM_COEFF_6")->getTextContent(), line_num_coeff[5] );
  cast_xmlch( get_node<DOMElement>( inverse_model, "LINE_NUM_COEFF_7")->getTextContent(), line_num_coeff[6] );
  cast_xmlch( get_node<DOMElement>( inverse_model, "LINE_NUM_COEFF_8")->getTextContent(), line_num_coeff[7] );
  cast_xmlch( get_node<DOMElement>( inverse_model, "LINE_NUM_COEFF_9")->getTextContent(), line_num_coeff[8] );
  cast_xmlch( get_node<DOMElement>( inverse_model, "LINE_NUM_COEFF_10")->getTextContent(), line_num_coeff[9] );
  cast_xmlch( get_node<DOMElement>( inverse_model, "LINE_NUM_COEFF_11")->getTextContent(), line_num_coeff[10] );
  cast_xmlch( get_node<DOMElement>( inverse_model, "LINE_NUM_COEFF_12")->getTextContent(), line_num_coeff[11] );
  cast_xmlch( get_node<DOMElement>( inverse_model, "LINE_NUM_COEFF_13")->getTextContent(), line_num_coeff[12] );
  cast_xmlch( get_node<DOMElement>( inverse_model, "LINE_NUM_COEFF_14")->getTextContent(), line_num_coeff[13] );
  cast_xmlch( get_node<DOMElement>( inverse_model, "LINE_NUM_COEFF_15")->getTextContent(), line_num_coeff[14] );
  cast_xmlch( get_node<DOMElement>( inverse_model, "LINE_NUM_COEFF_16")->getTextContent(), line_num_coeff[15] );
  cast_xmlch( get_node<DOMElement>( inverse_model, "LINE_NUM_COEFF_17")->getTextContent(), line_num_coeff[16] );
  cast_xmlch( get_node<DOMElement>( inverse_model, "LINE_NUM_COEFF_18")->getTextContent(), line_num_coeff[17] );
  cast_xmlch( get_node<DOMElement>( inverse_model, "LINE_NUM_COEFF_19")->getTextContent(), line_num_coeff[18] );
  cast_xmlch( get_node<DOMElement>( inverse_model, "LINE_NUM_COEFF_20")->getTextContent(), line_num_coeff[19] );

  cast_xmlch( get_node<DOMElement>( inverse_model, "LINE_DEN_COEFF_1")->getTextContent(), line_den_coeff[0] );
  cast_xmlch( get_node<DOMElement>( inverse_model, "LINE_DEN_COEFF_2")->getTextContent(), line_den_coeff[1] );
  cast_xmlch( get_node<DOMElement>( inverse_model, "LINE_DEN_COEFF_3")->getTextContent(), line_den_coeff[2] );
  cast_xmlch( get_node<DOMElement>( inverse_model, "LINE_DEN_COEFF_4")->getTextContent(), line_den_coeff[3] );
  cast_xmlch( get_node<DOMElement>( inverse_model, "LINE_DEN_COEFF_5")->getTextContent(), line_den_coeff[4] );
  cast_xmlch( get_node<DOMElement>( inverse_model, "LINE_DEN_COEFF_6")->getTextContent(), line_den_coeff[5] );
  cast_xmlch( get_node<DOMElement>( inverse_model, "LINE_DEN_COEFF_7")->getTextContent(), line_den_coeff[6] );
  cast_xmlch( get_node<DOMElement>( inverse_model, "LINE_DEN_COEFF_8")->getTextContent(), line_den_coeff[7] );
  cast_xmlch( get_node<DOMElement>( inverse_model, "LINE_DEN_COEFF_9")->getTextContent(), line_den_coeff[8] );
  cast_xmlch( get_node<DOMElement>( inverse_model, "LINE_DEN_COEFF_10")->getTextContent(), line_den_coeff[9] );
  cast_xmlch( get_node<DOMElement>( inverse_model, "LINE_DEN_COEFF_11")->getTextContent(), line_den_coeff[10] );
  cast_xmlch( get_node<DOMElement>( inverse_model, "LINE_DEN_COEFF_12")->getTextContent(), line_den_coeff[11] );
  cast_xmlch( get_node<DOMElement>( inverse_model, "LINE_DEN_COEFF_13")->getTextContent(), line_den_coeff[12] );
  cast_xmlch( get_node<DOMElement>( inverse_model, "LINE_DEN_COEFF_14")->getTextContent(), line_den_coeff[13] );
  cast_xmlch( get_node<DOMElement>( inverse_model, "LINE_DEN_COEFF_15")->getTextContent(), line_den_coeff[14] );
  cast_xmlch( get_node<DOMElement>( inverse_model, "LINE_DEN_COEFF_16")->getTextContent(), line_den_coeff[15] );
  cast_xmlch( get_node<DOMElement>( inverse_model, "LINE_DEN_COEFF_17")->getTextContent(), line_den_coeff[16] );
  cast_xmlch( get_node<DOMElement>( inverse_model, "LINE_DEN_COEFF_18")->getTextContent(), line_den_coeff[17] );
  cast_xmlch( get_node<DOMElement>( inverse_model, "LINE_DEN_COEFF_19")->getTextContent(), line_den_coeff[18] );
  cast_xmlch( get_node<DOMElement>( inverse_model, "LINE_DEN_COEFF_20")->getTextContent(), line_den_coeff[19] );

  cast_xmlch( get_node<DOMElement>( rfmvalidity, "LONG_SCALE")->getTextContent(), geodetic_scale.x() );
  cast_xmlch( get_node<DOMElement>( rfmvalidity, "LAT_SCALE")->getTextContent(), geodetic_scale.y() );
  cast_xmlch( get_node<DOMElement>( rfmvalidity, "HEIGHT_SCALE")->getTextContent(), geodetic_scale.z() );
  cast_xmlch( get_node<DOMElement>( rfmvalidity, "LONG_OFF")->getTextContent(), geodetic_offset.x() );
  cast_xmlch( get_node<DOMElement>( rfmvalidity, "LAT_OFF")->getTextContent(), geodetic_offset.y() );
  cast_xmlch( get_node<DOMElement>( rfmvalidity, "HEIGHT_OFF")->getTextContent(), geodetic_offset.z() );

  cast_xmlch( get_node<DOMElement>( rfmvalidity, "SAMP_SCALE")->getTextContent(), xy_scale.x() );
  cast_xmlch( get_node<DOMElement>( rfmvalidity, "LINE_SCALE")->getTextContent(), xy_scale.y() );
  cast_xmlch( get_node<DOMElement>( rfmvalidity, "SAMP_OFF")->getTextContent(), xy_offset.x() );
  cast_xmlch( get_node<DOMElement>( rfmvalidity, "LINE_OFF")->getTextContent(), xy_offset.y() );

  xy_offset -= Vector2i(1,1);

  // Push into the RPC Model so that it is easier to work with
  m_rpc.reset( new RPCModel( cartography::Datum("WGS84"),
                             line_num_coeff, line_den_coeff,
                             samp_num_coeff, samp_den_coeff,
                             xy_offset, xy_scale,
                             geodetic_offset, geodetic_scale ) );

}

// Helper functions to allow us to fill the objects
void asp::read_xml( std::string const& filename,
                    GeometricXML& geo,
                    AttitudeXML& att,
                    EphemerisXML& eph,
                    ImageXML& img,
                    RPCXML& rpc
                    ) {

  // Check if the file actually exists and throw a user helpful file.
  if ( !fs::exists( filename ) )
    vw_throw( ArgumentErr() << "XML file \"" << filename << "\" does not exist." );

  try{
    boost::scoped_ptr<XercesDOMParser> parser( new XercesDOMParser() );
    parser->setValidationScheme(XercesDOMParser::Val_Always);
    parser->setDoNamespaces(true);
    boost::scoped_ptr<ErrorHandler> errHandler( new HandlerBase() );
    parser->setErrorHandler(errHandler.get());

    parser->parse( filename.c_str() );
    DOMDocument* xmlDoc = parser->getDocument();
    DOMElement* elementRoot = xmlDoc->getDocumentElement();

    DOMNodeList* children = elementRoot->getChildNodes();
    for ( XMLSize_t i = 0; i < children->getLength(); ++i ) {
      DOMNode* curr_node = children->item(i);
      if ( curr_node->getNodeType() == DOMNode::ELEMENT_NODE ) {
        DOMElement* curr_element =
          dynamic_cast<DOMElement*>( curr_node );

        std::string tag( XMLString::transcode(curr_element->getTagName()) );

        if ( tag == "GEO" )
          geo.parse( curr_element );
        else if ( tag == "EPH" )
          eph.parse( curr_element );
        else if ( tag == "ATT" )
          att.parse( curr_element );
        else if ( tag == "IMD" )
          img.parse( curr_element );
        else if ( tag == "RPB" )
          rpc.parse( curr_element );
      }
    }
  } catch ( const std::exception& e ) {
    vw_throw( ArgumentErr() << e.what() << "XML file \"" << filename << "\" is invalid.\n" );
  }

}

vw::Vector2i asp::xml_image_size( std::string const& filename ){
  GeometricXML geo;
  AttitudeXML att;
  EphemerisXML eph;
  ImageXML img;
  RPCXML rpc;

  read_xml( filename, geo, att, eph, img, rpc);
  return img.image_size;
}

asp::RPCModel* asp::RPCXML::rpc_ptr() const {
  VW_ASSERT( m_rpc.get(),
             LogicErr() << "read_from_file or parse needs to be called first before an RPCModel is ready" );
  return m_rpc.get();
}

ext: cc


text:
// students_t_example2.cpp

// Copyright Paul A. Bristow 2006.
// Use, modification and distribution are 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)

// Example 2 of using Student's t

// A general guide to Student's t is at
// http://en.wikipedia.org/wiki/Student's_t-test
// (and many other elementary and advanced statistics texts).
// It says:
// The t statistic was invented by William Sealy Gosset
// for cheaply monitoring the quality of beer brews.
// "Student" was his pen name.
// Gosset was statistician for Guinness brewery in Dublin, Ireland,
// hired due to Claude Guinness's innovative policy of recruiting the
// best graduates from Oxford and Cambridge for applying biochemistry
// and statistics to Guinness's industrial processes.
// Gosset published the t test in Biometrika in 1908,
// but was forced to use a pen name by his employer who regarded the fact
// that they were using statistics as a trade secret.
// In fact, Gosset's identity was unknown not only to fellow statisticians
// but to his employer - the company insisted on the pseudonym
// so that it could turn a blind eye to the breach of its rules.

// The Students't distribution function is described at
// http://en.wikipedia.org/wiki/Student%27s_t_distribution

#include <boost/math/distributions/students_t.hpp>
   using boost::math::students_t;  // Probability of students_t(df, t).

#include <iostream>
   using std::cout;
   using std::endl;
#include <iomanip>
   using std::setprecision;
   using std::setw;
#include <cmath>
   using std::sqrt;

// This example of a one-sided test is from:
//
// from Statistics for Analytical Chemistry, 3rd ed. (1994), pp 59-60
// J. C. Miller and J. N. Miller, Ellis Horwood ISBN 0 13 0309907.

// An acid-base titrimetric method has a significant indicator error and
// thus tends to give results with a positive systematic error (+bias).
// To test this an exactly 0.1 M solution of acid is used to titrate
// 25.00 ml of exactly 0.1 M solution of alkali,
// with the following results (ml):

double reference = 25.00; // 'True' result.
const int values = 6; // titrations.
double data [values] = {25.06, 25.18, 24.87, 25.51, 25.34, 25.41};

int main()
{
   cout << "Example2 using Student's t function. ";
#if defined(__FILE__) && defined(__TIMESTAMP__) && defined(_MSC_FULL_VER)
   cout << "  " << __FILE__ << ' ' << __TIMESTAMP__ << ' '<< _MSC_FULL_VER;
#endif
   cout << endl;

   double sum = 0.;
   for (int value = 0; value < values; value++)
   { // Echo data and calculate mean.
      sum += data[value];
      cout << setw(4) << value << ' ' << setw(14) << data[value] << endl;
   }
   double mean = sum /static_cast<double>(values);
   cout << "Mean = " << mean << endl; // 25.2283

   double sd = 0.;
   for (int value = 0; value < values; value++)
   { // Calculate standard deviation.
      sd +=(data[value] - mean) * (data[value] - mean);
   }
   int degrees_of_freedom = values - 1; // Use the n-1 formula.
   sd /= degrees_of_freedom; // == variance.
   sd= sqrt(sd);
   cout << "Standard deviation = " << sd<< endl; // = 0.238279

   double t = (mean - reference) * sqrt(static_cast<double>(values))/ sd; //
   cout << "Student's t = " << t << ", with " << degrees_of_freedom << " degrees of freedom." << endl; // = 2.34725

   cout << "Probability of positive bias is " << cdf(students_t(degrees_of_freedom), t) << "."<< endl; // =  0.967108.
   // A 1-sided test because only testing for a positive bias.
   // If > 0.95 then greater than 1 in 20 conventional (arbitrary) requirement.

   return 0;
}  // int main()

/*

Output is:

------ Build started: Project: students_t_example2, Configuration: Debug Win32 ------
Compiling...
students_t_example2.cpp
Linking...
Autorun "i:\boost-06-05-03-1300\libs\math\test\Math_test\debug\students_t_example2.exe"
Example2 using Student's t function.   ..\..\..\..\..\..\boost-sandbox\libs\math_functions\example\students_t_example2.cpp Sat Aug 12 16:55:59 2006 140050727
   0          25.06
   1          25.18
   2          24.87
   3          25.51
   4          25.34
   5          25.41
Mean = 25.2283
Standard deviation = 0.238279
Student's t = 2.34725, with 5 degrees of freedom.
Probability of positive bias is 0.967108.
Build Time 0:03
Build log was saved at "file://i:\boost-06-05-03-1300\libs\math\test\Math_test\students_t_example2\Debug\BuildLog.htm"
students_t_example2 - 0 error(s), 0 warning(s)
========== Build: 1 succeeded, 0 failed, 0 up-to-date, 0 skipped ==========

*/


text:
//  (C) Copyright Edward Diener 2020
//  Use, modification and distribution are 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).

#include <boost/tti/detail/dmacro_sunfix.hpp>

  struct AType
    {
    template<class X,class Y,class Z> double SomeFuncTemplateName(X,Y *,Z &) { return 0.0; }
    template<class X,class Y,short AA> static int StatFuncTemplate(X *,Y) { int ret(AA); return ret; }
    };

  template
    <
    class C,
    class T
    >
  struct TestFunctionTemplate
    {
    typedef char Bad;
    struct Good { char x[2]; };
    template<T> struct helper BOOST_TTI_DETAIL_MACRO_SUNFIX ;
    template<class U> static Good check(helper<&U::template SomeFuncTemplateName<int,long,double> > *);
    template<class U> static Bad check(...);
    static const bool value=sizeof(check<C>(0))==sizeof(Good);
    };

  template
    <
    class C,
    class T
    >
  struct TestStaticFunctionTemplate
    {
    typedef char Bad;
    struct Good { char x[2]; };
    template<T *> struct helper BOOST_TTI_DETAIL_MACRO_SUNFIX ;
    template<class U> static Good check(helper<&U::template StatFuncTemplate<long,int,1234> > *);
    template<class U> static Bad check(...);
    static const bool value=sizeof(check<C>(0))==sizeof(Good);
    };

  int main()
    {

    bool bv =

    TestFunctionTemplate
    <
    AType,
    double (AType::*) (int,long *,double &)
    >::value

    &&

    TestStaticFunctionTemplate
    <
    AType,
    int (long *,int)
    >::value
    ;

    return (bv ? 0 : 1);
    }

max_issues_repo_name: qingkouwei/mediaones

max_forks_repo_name: qingkouwei/mediaones


text:
// vim:ts=2:sw=2:expandtab:autoindent:filetype=cpp:

// Copyright (c) 2007 Aristid Breitkreuz, Ruediger Sonderfeld, Christian Uhrhan
//
// Distributed under the Boost Software License, Version 1.0 and under the MIT
// License.
//
// See accompanying LICENSES.txt or
// <http://fragments.sourceforge.net/LICENSES.txt> or, for the Boost Software
// License, <http://www.boost.org/LICENSE_1_0.txt>.
//
// 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, TITLE AND NON-INFRINGEMENT. IN NO EVENT
// SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE
// FOR ANY DAMAGES OR OTHER LIABILITY, WHETHER IN CONTRACT, TORT OR OTHERWISE,
// ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
// DEALINGS IN THE SOFTWARE.

#ifndef FRAGMENTS_DETAIL_VISIBLE_FRAGMENTS_HPP
#define FRAGMENTS_DETAIL_VISIBLE_FRAGMENTS_HPP

#include <fragments/detail/create_vector.hpp>
#include <boost/mpl/has_xxx.hpp>
#include <boost/mpl/bool.hpp>
#include <boost/mpl/deref.hpp>
#include <boost/mpl/eval_if.hpp>
#include <boost/mpl/begin.hpp>
#include <boost/mpl/end.hpp>
#include <boost/mpl/next_prior.hpp>
#include <boost/mpl/iterator_range.hpp>

namespace fragments { namespace detail {

BOOST_MPL_HAS_XXX_TRAIT_DEF(shadow)

template<typename T, bool = has_shadow<T>::value>
struct test_shadow
  : T::shadow
{};

template<typename T>
struct test_shadow<T, false>
  : boost::mpl::false_
{};

template<
  typename Seq,
  typename First = typename boost::mpl::begin<Seq>::type,
  typename Last = typename boost::mpl::end<Seq>::type
>
struct visible_fragments {
  typedef typename boost::mpl::deref<First>::type first;

  typedef typename boost::mpl::next<First>::type next;

  typedef typename boost::mpl::eval_if<
      test_shadow<first>,
      visible_fragments<
        boost::mpl::iterator_range<First, Last>,
        next,
        Last
      >,
      visible_fragments<
        Seq,
        next,
        Last
      >
    >::type type;
};

template<typename Seq, typename End>
struct visible_fragments<Seq, End, End> {
  typedef typename create_vector<Seq>::type type;
};

}}

#endif

max_stars_repo_name: aristidb/fragments

max_issues_repo_name: aristidb/fragments

max_forks_repo_name: aristidb/fragments


text:
// Copyright Louis Dionne 2013-2016
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)

#include <boost/hana/assert.hpp>
namespace hana = boost::hana;

template <bool value>
bool runtime_bool() { return value; }

int main() {
    // Make sure it works at function scope
    BOOST_HANA_RUNTIME_ASSERT(runtime_bool<true>());
    BOOST_HANA_RUNTIME_ASSERT_MSG(runtime_bool<true>(), "message");

    // Make sure we can reference a local variable
    auto yes = runtime_bool<true>();
    BOOST_HANA_RUNTIME_ASSERT(yes);
    BOOST_HANA_RUNTIME_ASSERT_MSG(yes, "message");
}

max_issues_repo_name: lipper/arangodb

max_forks_repo_name: lipper/arangodb