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

/* Authors: Alejandro Perez, Sertac Karaman, Ryan Luna, Ioan Sucan */

#include "ompl/geometric/planners/rrt/RRTstar.h"
#include "ompl/base/goals/GoalSampleableRegion.h"
#include "ompl/tools/config/SelfConfig.h"
#include "ompl/tools/config/MagicConstants.h"
#include <algorithm>
#include <limits>
#include <map>
#include <boost/math/constants/constants.hpp>

ompl::geometric::RRTstar::RRTstar(const base::SpaceInformationPtr &si) : base::Planner(si, "RRTstar")
{
    specs_.approximateSolutions = true;
    specs_.optimizingPaths = true;

    goalBias_ = 0.05;
    maxDistance_ = 0.0;
    delayCC_ = true;
    iterations_ = 0;
    lastGoalMotion_ = NULL;

    Planner::declareParam<double>("range", this, &RRTstar::setRange, &RRTstar::getRange, "0.:1.:10000.");
    Planner::declareParam<double>("goal_bias", this, &RRTstar::setGoalBias, &RRTstar::getGoalBias, "0.:.05:1.");
    Planner::declareParam<bool>("delay_collision_checking", this, &RRTstar::setDelayCC, &RRTstar::getDelayCC, "0,1");
}

ompl::geometric::RRTstar::~RRTstar(void)
{
    freeMemory();
}

void ompl::geometric::RRTstar::setup(void)
{
    Planner::setup();
    tools::SelfConfig sc(si_, getName());
    sc.configurePlannerRange(maxDistance_);

    if (!nn_)
        nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion*>(si_->getStateSpace()));
    nn_->setDistanceFunction(boost::bind(&RRTstar::distanceFunction, this, _1, _2));
}

void ompl::geometric::RRTstar::clear(void)
{
    Planner::clear();
    sampler_.reset();
    freeMemory();
    if (nn_)
        nn_->clear();
    iterations_ = 0;
    lastGoalMotion_ = NULL;
    goalMotions_.clear();
}

ompl::base::PlannerStatus ompl::geometric::RRTstar::solve(const base::PlannerTerminationCondition &ptc)
{
    checkValidity();
    base::Goal                  *goal   = pdef_->getGoal().get();
    base::GoalSampleableRegion  *goal_s = dynamic_cast<base::GoalSampleableRegion*>(goal);
    base::OptimizationObjective *opt    = pdef_->getOptimizationObjective().get();

    // when no optimization objective is specified, we create a temporary one (we should not modify the ProblemDefinition)
    boost::scoped_ptr<base::OptimizationObjective> temporaryOptimizationObjective;

    if (opt && !dynamic_cast<base::PathLengthOptimizationObjective*>(opt))
    {
        opt = NULL;
        OMPL_WARN("Optimization objective '%s' specified, but such an objective is not appropriate for %s. Only path length can be optimized.", getName().c_str(), opt->getDescription().c_str());
    }

    if (!opt)
    {
        // by default, optimize path length and run until completion
        opt = new base::PathLengthOptimizationObjective(si_, std::numeric_limits<double>::epsilon());
        temporaryOptimizationObjective.reset(opt);
        OMPL_INFORM("No optimization objective specified. Defaulting to optimization of path length for the allowed planning time.");
    }

    if (!goal)
    {
        OMPL_ERROR("Goal undefined");
        return base::PlannerStatus::INVALID_GOAL;
    }

    while (const base::State *st = pis_.nextStart())
    {
        Motion *motion = new Motion(si_);
        si_->copyState(motion->state, st);
        nn_->add(motion);
    }

    if (nn_->size() == 0)
    {
        OMPL_ERROR("There are no valid initial states!");
        return base::PlannerStatus::INVALID_START;
    }

    if (!sampler_)
        sampler_ = si_->allocStateSampler();

    OMPL_INFORM("Starting with %u states", nn_->size());

    Motion *solution       = lastGoalMotion_;
    Motion *approximation  = NULL;
    double approximatedist = std::numeric_limits<double>::infinity();
    bool sufficientlyShort = false;

    Motion *rmotion        = new Motion(si_);
    base::State *rstate    = rmotion->state;
    base::State *xstate    = si_->allocState();

    // e+e/d.  K-nearest RRT*
    double k_rrg           = boost::math::constants::e<double>() + (boost::math::constants::e<double>()/(double)si_->getStateSpace()->getDimension());

    std::vector<Motion*>       nbh;
    std::vector<double>        dists;
    std::vector<std::size_t>   sortedDistIndices;
    std::vector<int>           valid;
    unsigned int               rewireTest = 0;
    unsigned int               statesGenerated = 0;

    if(solution)
        OMPL_INFORM("Starting with existing solution of cost %.5f", solution->cost);
    OMPL_INFORM("Initial k-nearest value of %u", (unsigned int)std::ceil(k_rrg * log((double)nn_->size()+1)));

    NeighborIndexCompare compareFn(nbh);

    while (ptc == false)
    {
        iterations_++;
        // sample random state (with goal biasing)
        // Goal samples are only sampled until maxSampleCount() goals are in the tree, to prohibit duplicate goal states.
        if (goal_s && goalMotions_.size() < goal_s->maxSampleCount() && rng_.uniform01() < goalBias_ && goal_s->canSample())
            goal_s->sampleGoal(rstate);
        else
            sampler_->sampleUniform(rstate);

        // find closest state in the tree
        Motion *nmotion = nn_->nearest(rmotion);

        base::State *dstate = rstate;

        // find state to add to the tree
        double d = si_->distance(nmotion->state, rstate);
        if (d > maxDistance_)
        {
            si_->getStateSpace()->interpolate(nmotion->state, rstate, maxDistance_ / d, xstate);
            dstate = xstate;
        }

        // Check if the motion between the nearest state and the state to add is valid
        if (si_->checkMotion(nmotion->state, dstate))
        {
            // Duplicate the sampled motion
            double distN = si_->distance(dstate, nmotion->state);
            Motion *motion = new Motion(si_);
            si_->copyState(motion->state, dstate);
            motion->parent = nmotion;

            // Find nearby neighbors of the new motion - k-nearest RRT*
            unsigned int k = std::ceil(k_rrg * log((double)nn_->size()+1));
            nn_->nearestK(motion, k, nbh);
            rewireTest += nbh.size();
            statesGenerated++;

            // cache for distance computations
            dists.resize(nbh.size());
            sortedDistIndices.resize(nbh.size());
            for (std::size_t i = 0; i < sortedDistIndices.size(); ++i)
                sortedDistIndices[i] = i;
            // cache for motion validity
            valid.resize(nbh.size());
            std::fill(valid.begin(), valid.end(), 0);

            // Finding the nearest neighbor to connect to
            // By default, neighborhood states are sorted by distance, and collision checking
            // is performed in increasing order of distance
            if (delayCC_)
            {
                // calculate all costs and distances
                for (unsigned int i = 0; i < nbh.size(); ++i)
                {
                    double d = si_->distance(nbh[i]->state, motion->state);
                    dists[i] = d;
                    nbh[i]->cost += d;
                }

                // sort the nodes
                std::sort(sortedDistIndices.begin(), sortedDistIndices.end(), compareFn);

                for (unsigned int i = 0; i < nbh.size(); ++i)
                    nbh[i]->cost -= dists[i];

                // Collision check until a valid motion is found
                // The first one found is the min, since the neighbors are sorted
                for (std::vector<std::size_t>::const_iterator i = sortedDistIndices.begin();
                     i != sortedDistIndices.end();
                     ++i)
                {
                    if (nbh[*i] == nmotion || si_->checkMotion(nbh[*i]->state, motion->state))
                    {
                        motion->cost = nbh[*i]->cost + dists[*i];
                        motion->parent = nbh[*i];
                        valid[*i] = 1;
                        break;
                    }
                    else
                        valid[*i] = -1;
                }
            }
            else
            {
                motion->cost = distN;
                // find which one we connect the new state to
                for (unsigned int i = 0 ; i < nbh.size() ; ++i)
                {
                    if (nbh[i] != nmotion)
                    {
                        double d = si_->distance(nbh[i]->state, dstate);
                        dists[i] = d;
                        double c = nbh[i]->cost + d;
                        if (c < motion->cost)
                        {
                            if (si_->checkMotion(nbh[i]->state, motion->state))
                            {
                                motion->cost = c;
                                motion->parent = nbh[i];
                                valid[i] = 1;
                            }
                            else
                                valid[i] = -1;
                        }
                    }
                    else
                    {
                        valid[i] = 1;
                        dists[i] = distN;
                    }
                }
            }

            // add motion to the tree
            nn_->add(motion);
            motion->parent->children.push_back(motion);

            // rewire tree if needed
            bool checkForSolution = false;
            for (unsigned int i = 0; i < nbh.size(); ++i)
            {
                if (nbh[i] == motion->parent) continue;

                double newcost = motion->cost + dists[i];
                if (newcost < nbh[i]->cost)
                {
                    // Check if the motion to the neighbor is valid
                    bool v = (valid[i] == 0 ? si_->checkMotion(nbh[i]->state, motion->state) : valid[i] == 1);
                    if (v)
                    {
                        // Need to subtract the difference in cost from all of the node's in the neighbor's subtree
                        double delta = newcost - nbh[i]->cost;
                        // Remove the neighbor node from it's parent's child list
                        removeFromParent(nbh[i]);

                        // Add the neighbor node as a child of motion
                        nbh[i]->parent = motion;
                        nbh[i]->cost = newcost;
                        motion->children.push_back(nbh[i]);

                        updateChildCosts(nbh[i], delta);
                        checkForSolution = true;
                    }
                }
            }

            // Add the new motion to the goalMotion_ list, if it satisfies the goal
            double distanceFromGoal;
            if (goal->isSatisfied(motion->state, &distanceFromGoal))
            {
                goalMotions_.push_back(motion);
                checkForSolution = true;
            }

            // Checking for solution or iterative improvement
            for (size_t i = 0; i < goalMotions_.size() && checkForSolution; ++i)
            {
                sufficientlyShort = opt->isSatisfied(goalMotions_[i]->cost);
                if (sufficientlyShort)
                {
                    solution = goalMotions_[i];
                    break;
                }
                else if (!solution || goalMotions_[i]->cost < solution->cost)
                {
                    solution = goalMotions_[i];
                }
            }

            // Checking for approximate solution (closest state found to the goal)
            if (goalMotions_.size() == 0 && distanceFromGoal < approximatedist)
            {
                approximation = motion;
                approximatedist = distanceFromGoal;
            }
        }

        // terminate if a sufficient solution is found
        if (solution && sufficientlyShort)
            break;
    }

    bool approximate = (solution == NULL);
    bool addedSolution = false;
    if (approximate)
        solution = approximation;
    else
        lastGoalMotion_ = solution;

    if (solution != NULL)
    {
        // construct the solution path
        std::vector<Motion*> mpath;
        while (solution != NULL)
        {
            mpath.push_back(solution);
            solution = solution->parent;
        }

        // set the solution path
        PathGeometric *geopath = new PathGeometric(si_);
        for (int i = mpath.size() - 1 ; i >= 0 ; --i)
            geopath->append(mpath[i]->state);

        base::PathPtr path(geopath);
        // Add the solution path, whether it is approximate (not reaching the goal), and the
        // distance from the end of the path to the goal (-1 if satisfying the goal).
        base::PlannerSolution psol(path, approximate, approximate ? approximatedist : -1.0);
        // Does the solution satisfy the optimization objective?
        psol.optimized_ = sufficientlyShort;

        pdef_->addSolutionPath (psol);

        addedSolution = true;
    }

    si_->freeState(xstate);
    if (rmotion->state)
        si_->freeState(rmotion->state);
    delete rmotion;

    OMPL_INFORM("Created %u new states. Checked %lu rewire options. %u goal states in tree.", statesGenerated, rewireTest, goalMotions_.size());

    return base::PlannerStatus(addedSolution, approximate);
}

void ompl::geometric::RRTstar::removeFromParent(Motion *m)
{
    std::vector<Motion*>::iterator it = m->parent->children.begin ();
    while (it != m->parent->children.end ())
    {
        if (*it == m)
        {
            it = m->parent->children.erase(it);
            it = m->parent->children.end ();
        }
        else
            ++it;
    }
}

void ompl::geometric::RRTstar::updateChildCosts(Motion *m, double delta)
{
    for (size_t i = 0; i < m->children.size(); ++i)
    {
        m->children[i]->cost += delta;
        updateChildCosts(m->children[i], delta);
    }
}

void ompl::geometric::RRTstar::freeMemory(void)
{
    if (nn_)
    {
        std::vector<Motion*> motions;
        nn_->list(motions);
        for (unsigned int i = 0 ; i < motions.size() ; ++i)
        {
            if (motions[i]->state)
                si_->freeState(motions[i]->state);
            delete motions[i];
        }
    }
}

void ompl::geometric::RRTstar::getPlannerData(base::PlannerData &data) const
{
    Planner::getPlannerData(data);

    std::vector<Motion*> motions;
    if (nn_)
        nn_->list(motions);

    if (lastGoalMotion_)
        data.addGoalVertex(base::PlannerDataVertex(lastGoalMotion_->state));

    for (unsigned int i = 0 ; i < motions.size() ; ++i)
    {
        if (motions[i]->parent == NULL)
            data.addStartVertex(base::PlannerDataVertex(motions[i]->state));
        else
            data.addEdge(base::PlannerDataVertex(motions[i]->parent->state),
                         base::PlannerDataVertex(motions[i]->state));
    }

    data.properties["iterations INTEGER"] = boost::lexical_cast<std::string>(iterations_);
}
<commit_msg>Fixed bug in RRTstar when it runs with delayCC_ OFF<commit_after>/*********************************************************************
* Software License Agreement (BSD License)
*
*  Copyright (c) 2011, Rice University
*  All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
*
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the Rice University nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/

/* Authors: Alejandro Perez, Sertac Karaman, Ryan Luna, Ioan Sucan */

#include "ompl/geometric/planners/rrt/RRTstar.h"
#include "ompl/base/goals/GoalSampleableRegion.h"
#include "ompl/tools/config/SelfConfig.h"
#include "ompl/tools/config/MagicConstants.h"
#include <algorithm>
#include <limits>
#include <map>
#include <boost/math/constants/constants.hpp>

ompl::geometric::RRTstar::RRTstar(const base::SpaceInformationPtr &si) : base::Planner(si, "RRTstar")
{
    specs_.approximateSolutions = true;
    specs_.optimizingPaths = true;

    goalBias_ = 0.05;
    maxDistance_ = 0.0;
    delayCC_ = true;
    iterations_ = 0;
    lastGoalMotion_ = NULL;

    Planner::declareParam<double>("range", this, &RRTstar::setRange, &RRTstar::getRange, "0.:1.:10000.");
    Planner::declareParam<double>("goal_bias", this, &RRTstar::setGoalBias, &RRTstar::getGoalBias, "0.:.05:1.");
    Planner::declareParam<bool>("delay_collision_checking", this, &RRTstar::setDelayCC, &RRTstar::getDelayCC, "0,1");
}

ompl::geometric::RRTstar::~RRTstar(void)
{
    freeMemory();
}

void ompl::geometric::RRTstar::setup(void)
{
    Planner::setup();
    tools::SelfConfig sc(si_, getName());
    sc.configurePlannerRange(maxDistance_);

    if (!nn_)
        nn_.reset(tools::SelfConfig::getDefaultNearestNeighbors<Motion*>(si_->getStateSpace()));
    nn_->setDistanceFunction(boost::bind(&RRTstar::distanceFunction, this, _1, _2));
}

void ompl::geometric::RRTstar::clear(void)
{
    Planner::clear();
    sampler_.reset();
    freeMemory();
    if (nn_)
        nn_->clear();
    iterations_ = 0;
    lastGoalMotion_ = NULL;
    goalMotions_.clear();
}

ompl::base::PlannerStatus ompl::geometric::RRTstar::solve(const base::PlannerTerminationCondition &ptc)
{
    checkValidity();
    base::Goal                  *goal   = pdef_->getGoal().get();
    base::GoalSampleableRegion  *goal_s = dynamic_cast<base::GoalSampleableRegion*>(goal);
    base::OptimizationObjective *opt    = pdef_->getOptimizationObjective().get();

    // when no optimization objective is specified, we create a temporary one (we should not modify the ProblemDefinition)
    boost::scoped_ptr<base::OptimizationObjective> temporaryOptimizationObjective;

    if (opt && !dynamic_cast<base::PathLengthOptimizationObjective*>(opt))
    {
        opt = NULL;
        OMPL_WARN("Optimization objective '%s' specified, but such an objective is not appropriate for %s. Only path length can be optimized.", getName().c_str(), opt->getDescription().c_str());
    }

    if (!opt)
    {
        // by default, optimize path length and run until completion
        opt = new base::PathLengthOptimizationObjective(si_, std::numeric_limits<double>::epsilon());
        temporaryOptimizationObjective.reset(opt);
        OMPL_INFORM("No optimization objective specified. Defaulting to optimization of path length for the allowed planning time.");
    }

    if (!goal)
    {
        OMPL_ERROR("Goal undefined");
        return base::PlannerStatus::INVALID_GOAL;
    }

    while (const base::State *st = pis_.nextStart())
    {
        Motion *motion = new Motion(si_);
        si_->copyState(motion->state, st);
        nn_->add(motion);
    }

    if (nn_->size() == 0)
    {
        OMPL_ERROR("There are no valid initial states!");
        return base::PlannerStatus::INVALID_START;
    }

    if (!sampler_)
        sampler_ = si_->allocStateSampler();

    OMPL_INFORM("Starting with %u states", nn_->size());

    Motion *solution       = lastGoalMotion_;
    Motion *approximation  = NULL;
    double approximatedist = std::numeric_limits<double>::infinity();
    bool sufficientlyShort = false;

    Motion *rmotion        = new Motion(si_);
    base::State *rstate    = rmotion->state;
    base::State *xstate    = si_->allocState();

    // e+e/d.  K-nearest RRT*
    double k_rrg           = boost::math::constants::e<double>() + (boost::math::constants::e<double>()/(double)si_->getStateSpace()->getDimension());

    std::vector<Motion*>       nbh;
    std::vector<double>        dists;
    std::vector<std::size_t>   sortedDistIndices;
    std::vector<int>           valid;
    unsigned int               rewireTest = 0;
    unsigned int               statesGenerated = 0;

    if(solution)
        OMPL_INFORM("Starting with existing solution of cost %.5f", solution->cost);
    OMPL_INFORM("Initial k-nearest value of %u", (unsigned int)std::ceil(k_rrg * log((double)nn_->size()+1)));

    NeighborIndexCompare compareFn(nbh);

    while (ptc == false)
    {
        iterations_++;
        // sample random state (with goal biasing)
        // Goal samples are only sampled until maxSampleCount() goals are in the tree, to prohibit duplicate goal states.
        if (goal_s && goalMotions_.size() < goal_s->maxSampleCount() && rng_.uniform01() < goalBias_ && goal_s->canSample())
            goal_s->sampleGoal(rstate);
        else
            sampler_->sampleUniform(rstate);

        // find closest state in the tree
        Motion *nmotion = nn_->nearest(rmotion);

        base::State *dstate = rstate;

        // find state to add to the tree
        double d = si_->distance(nmotion->state, rstate);
        if (d > maxDistance_)
        {
            si_->getStateSpace()->interpolate(nmotion->state, rstate, maxDistance_ / d, xstate);
            dstate = xstate;
        }

        // Check if the motion between the nearest state and the state to add is valid
        if (si_->checkMotion(nmotion->state, dstate))
        {
            // Duplicate the sampled motion
            double distN = si_->distance(dstate, nmotion->state);
            Motion *motion = new Motion(si_);
            si_->copyState(motion->state, dstate);
            motion->parent = nmotion;

            // Find nearby neighbors of the new motion - k-nearest RRT*
            unsigned int k = std::ceil(k_rrg * log((double)nn_->size()+1));
            nn_->nearestK(motion, k, nbh);
            rewireTest += nbh.size();
            statesGenerated++;

            // cache for distance computations
            dists.resize(nbh.size());
            sortedDistIndices.resize(nbh.size());
            for (std::size_t i = 0; i < sortedDistIndices.size(); ++i)
                sortedDistIndices[i] = i;
            // cache for motion validity
            valid.resize(nbh.size());
            std::fill(valid.begin(), valid.end(), 0);

            // Finding the nearest neighbor to connect to
            // By default, neighborhood states are sorted by distance, and collision checking
            // is performed in increasing order of distance
            if (delayCC_)
            {
                // calculate all costs and distances
                for (unsigned int i = 0; i < nbh.size(); ++i)
                {
                    double d = si_->distance(nbh[i]->state, motion->state);
                    dists[i] = d;
                    nbh[i]->cost += d;
                }

                // sort the nodes
                std::sort(sortedDistIndices.begin(), sortedDistIndices.end(), compareFn);

                for (unsigned int i = 0; i < nbh.size(); ++i)
                    nbh[i]->cost -= dists[i];

                // Collision check until a valid motion is found
                // The first one found is the min, since the neighbors are sorted
                for (std::vector<std::size_t>::const_iterator i = sortedDistIndices.begin();
                     i != sortedDistIndices.end();
                     ++i)
                {
                    if (nbh[*i] == nmotion || si_->checkMotion(nbh[*i]->state, motion->state))
                    {
                        motion->cost = nbh[*i]->cost + dists[*i];
                        motion->parent = nbh[*i];
                        valid[*i] = 1;
                        break;
                    }
                    else
                        valid[*i] = -1;
                }
            }
            else
            {
                motion->cost = nmotion->cost + distN;
                // find which one we connect the new state to
                for (unsigned int i = 0 ; i < nbh.size() ; ++i)
                {
                    if (nbh[i] != nmotion)
                    {
                        double d = si_->distance(nbh[i]->state, dstate);
                        dists[i] = d;
                        double c = nbh[i]->cost + d;
                        if (c < motion->cost)
                        {
                            if (si_->checkMotion(nbh[i]->state, motion->state))
                            {
                                motion->cost = c;
                                motion->parent = nbh[i];
                                valid[i] = 1;
                            }
                            else
                                valid[i] = -1;
                        }
                    }
                    else
                    {
                        valid[i] = 1;
                        dists[i] = distN;
                    }
                }
            }

            // add motion to the tree
            nn_->add(motion);
            motion->parent->children.push_back(motion);

            // rewire tree if needed
            bool checkForSolution = false;
            for (unsigned int i = 0; i < nbh.size(); ++i)
            {
                if (nbh[i] == motion->parent) continue;

                double newcost = motion->cost + dists[i];
                if (newcost < nbh[i]->cost)
                {
                    // Check if the motion to the neighbor is valid
                    bool v = (valid[i] == 0 ? si_->checkMotion(nbh[i]->state, motion->state) : valid[i] == 1);
                    if (v)
                    {
                        // Need to subtract the difference in cost from all of the node's in the neighbor's subtree
                        double delta = newcost - nbh[i]->cost;
                        // Remove the neighbor node from it's parent's child list
                        removeFromParent(nbh[i]);

                        // Add the neighbor node as a child of motion
                        nbh[i]->parent = motion;
                        nbh[i]->cost = newcost;
                        motion->children.push_back(nbh[i]);

                        updateChildCosts(nbh[i], delta);
                        checkForSolution = true;
                    }
                }
            }

            // Add the new motion to the goalMotion_ list, if it satisfies the goal
            double distanceFromGoal;
            if (goal->isSatisfied(motion->state, &distanceFromGoal))
            {
                goalMotions_.push_back(motion);
                checkForSolution = true;
            }

            // Checking for solution or iterative improvement
            for (size_t i = 0; i < goalMotions_.size() && checkForSolution; ++i)
            {
                sufficientlyShort = opt->isSatisfied(goalMotions_[i]->cost);
                if (sufficientlyShort)
                {
                    solution = goalMotions_[i];
                    break;
                }
                else if (!solution || goalMotions_[i]->cost < solution->cost)
                {
                    solution = goalMotions_[i];
                }
            }

            // Checking for approximate solution (closest state found to the goal)
            if (goalMotions_.size() == 0 && distanceFromGoal < approximatedist)
            {
                approximation = motion;
                approximatedist = distanceFromGoal;
            }
        }

        // terminate if a sufficient solution is found
        if (solution && sufficientlyShort)
            break;
    }

    bool approximate = (solution == NULL);
    bool addedSolution = false;
    if (approximate)
        solution = approximation;
    else
        lastGoalMotion_ = solution;

    if (solution != NULL)
    {
        // construct the solution path
        std::vector<Motion*> mpath;
        while (solution != NULL)
        {
            mpath.push_back(solution);
            solution = solution->parent;
        }

        // set the solution path
        PathGeometric *geopath = new PathGeometric(si_);
        for (int i = mpath.size() - 1 ; i >= 0 ; --i)
            geopath->append(mpath[i]->state);

        base::PathPtr path(geopath);
        // Add the solution path, whether it is approximate (not reaching the goal), and the
        // distance from the end of the path to the goal (-1 if satisfying the goal).
        base::PlannerSolution psol(path, approximate, approximate ? approximatedist : -1.0);
        // Does the solution satisfy the optimization objective?
        psol.optimized_ = sufficientlyShort;

        pdef_->addSolutionPath (psol);

        addedSolution = true;
    }

    si_->freeState(xstate);
    if (rmotion->state)
        si_->freeState(rmotion->state);
    delete rmotion;

    OMPL_INFORM("Created %u new states. Checked %lu rewire options. %u goal states in tree.", statesGenerated, rewireTest, goalMotions_.size());

    return base::PlannerStatus(addedSolution, approximate);
}

void ompl::geometric::RRTstar::removeFromParent(Motion *m)
{
    std::vector<Motion*>::iterator it = m->parent->children.begin ();
    while (it != m->parent->children.end ())
    {
        if (*it == m)
        {
            it = m->parent->children.erase(it);
            it = m->parent->children.end ();
        }
        else
            ++it;
    }
}

void ompl::geometric::RRTstar::updateChildCosts(Motion *m, double delta)
{
    for (size_t i = 0; i < m->children.size(); ++i)
    {
        m->children[i]->cost += delta;
        updateChildCosts(m->children[i], delta);
    }
}

void ompl::geometric::RRTstar::freeMemory(void)
{
    if (nn_)
    {
        std::vector<Motion*> motions;
        nn_->list(motions);
        for (unsigned int i = 0 ; i < motions.size() ; ++i)
        {
            if (motions[i]->state)
                si_->freeState(motions[i]->state);
            delete motions[i];
        }
    }
}

void ompl::geometric::RRTstar::getPlannerData(base::PlannerData &data) const
{
    Planner::getPlannerData(data);

    std::vector<Motion*> motions;
    if (nn_)
        nn_->list(motions);

    if (lastGoalMotion_)
        data.addGoalVertex(base::PlannerDataVertex(lastGoalMotion_->state));

    for (unsigned int i = 0 ; i < motions.size() ; ++i)
    {
        if (motions[i]->parent == NULL)
            data.addStartVertex(base::PlannerDataVertex(motions[i]->state));
        else
            data.addEdge(base::PlannerDataVertex(motions[i]->parent->state),
                         base::PlannerDataVertex(motions[i]->state));
    }

    data.properties["iterations INTEGER"] = boost::lexical_cast<std::string>(iterations_);
}
<|endoftext|>


<commit_before>/******************************************************************************
*       SOFA, Simulation Open-Framework Architecture, development version     *
*                (c) 2006-2017 INRIA, USTL, UJF, CNRS, MGH                    *
*                                                                             *
* This program is free software; you can redistribute it and/or modify it     *
* under the terms of the GNU General Public License as published by the Free  *
* Software Foundation; either version 2 of the License, or (at your option)   *
* any later version.                                                          *
*                                                                             *
* This program is distributed in the hope that it will be useful, but WITHOUT *
* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or       *
* FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for    *
* more details.                                                               *
*                                                                             *
* You should have received a copy of the GNU General Public License along     *
* with this program. If not, see <http://www.gnu.org/licenses/>.              *
*******************************************************************************
* Authors: The SOFA Team and external contributors (see Authors.txt)          *
*                                                                             *
* Contact information: contact@sofa-framework.org                             *
******************************************************************************/
#include "BatchGUI.h"
#include <sofa/simulation/Simulation.h>
#include <sofa/simulation/UpdateContextVisitor.h>
#ifdef SOFA_SMP
#include <athapascan-1>
#endif
#include <sofa/helper/system/thread/CTime.h>
#include <iostream>
#include <sstream>

namespace sofa
{

namespace gui
{

const unsigned int BatchGUI::DEFAULT_NUMBER_OF_ITERATIONS = 1000;
unsigned int BatchGUI::nbIter = BatchGUI::DEFAULT_NUMBER_OF_ITERATIONS;

BatchGUI::BatchGUI()
    : groot(NULL)
{
}

BatchGUI::~BatchGUI()
{
}

int BatchGUI::mainLoop()
{
    if (groot)
    {

        sofa::simulation::getSimulation()->animate(groot.get());
        //As no visualization is done by the Batch GUI, these two lines are not necessary.
        sofa::simulation::getSimulation()->updateVisual(groot.get());
        std::cout << "Computing "<<nbIter<<" iterations." << std::endl;
        sofa::simulation::Visitor::ctime_t rtfreq = sofa::helper::system::thread::CTime::getRefTicksPerSec();
        sofa::simulation::Visitor::ctime_t tfreq = sofa::helper::system::thread::CTime::getTicksPerSec();
        sofa::simulation::Visitor::ctime_t rt = sofa::helper::system::thread::CTime::getRefTime();
        sofa::simulation::Visitor::ctime_t t = sofa::helper::system::thread::CTime::getFastTime();
        for (unsigned int i=0; i<nbIter; i++)
        {
            sofa::simulation::getSimulation()->animate(groot.get());
            //As no visualization is done by the Batch GUI, these two lines are not necessary.
            sofa::simulation::getSimulation()->updateVisual(groot.get());
        }
        t = sofa::helper::system::thread::CTime::getFastTime()-t;
        rt = sofa::helper::system::thread::CTime::getRefTime()-rt;

        std::cout << nbIter << " iterations done in "<< ((double)t)/((double)tfreq) << " s ( " << (((double)tfreq)*nbIter)/((double)t) << " FPS)." << std::endl;
        std::cout << nbIter << " iterations done in "<< ((double)rt)/((double)rtfreq) << " s ( " << (((double)rtfreq)*nbIter)/((double)rt) << " FPS)." << std::endl;
    }
    return 0;
}

void BatchGUI::redraw()
{
}

int BatchGUI::closeGUI()
{
    delete this;
    return 0;
}

void BatchGUI::setScene(sofa::simulation::Node::SPtr groot, const char* filename, bool )
{
    this->groot = groot;
    this->filename = (filename?filename:"");

    resetScene();
}

void BatchGUI::resetScene()
{
    sofa::simulation::Node* root = currentSimulation();

    if ( root )
    {
        root->setTime(0.);
        simulation::getSimulation()->reset ( root );

        sofa::simulation::UpdateSimulationContextVisitor(sofa::core::ExecParams::defaultInstance()).execute(root);
    }
}

void BatchGUI::startDumpVisitor()
{
#ifdef SOFA_DUMP_VISITOR_INFO
    sofa::simulation::Node* root = currentSimulation();
    if (root)
    {
        m_dumpVisitorStream.str("");
        sofa::simulation::Visitor::startDumpVisitor(&m_dumpVisitorStream, root->getTime());
    }
#endif
}

void BatchGUI::stopDumpVisitor()
{
#ifdef SOFA_DUMP_VISITOR_INFO
    sofa::simulation::Visitor::stopDumpVisitor();
    m_dumpVisitorStream.flush();
    m_dumpVisitorStream.str("");
#endif
}

sofa::simulation::Node* BatchGUI::currentSimulation()
{
    return groot.get();
}

int BatchGUI::InitGUI(const char* /*name*/, const std::vector<std::string>& options)
{
    setNumIterations(DEFAULT_NUMBER_OF_ITERATIONS);

    //parse options
    for (unsigned int i=0 ; i<options.size() ; i++)
    {
        size_t cursor = 0;
        std::string opt = options[i];
        //Set number of iterations
        //(option = "nbIterations=N where N is the number of iterations)
        if ( (cursor = opt.find("nbIterations=")) != std::string::npos )
        {
            unsigned int nbIterations;
            std::istringstream iss;
            iss.str(opt.substr(cursor+std::string("nbIterations=").length(), std::string::npos));
            iss >> nbIterations;
            setNumIterations(nbIterations);
        }
    }
    return 0;
}

BaseGUI* BatchGUI::CreateGUI(const char* name, const std::vector<std::string>& /*options*/, sofa::simulation::Node::SPtr groot, const char* filename)
{
    BatchGUI::mGuiName = name;
    BatchGUI* gui = new BatchGUI();
    gui->setScene(groot, filename);
    return gui;
}

} // namespace gui

} // namespace sofa
<commit_msg>[Advanced Timer] in batch mode<commit_after>/******************************************************************************
*       SOFA, Simulation Open-Framework Architecture, development version     *
*                (c) 2006-2017 INRIA, USTL, UJF, CNRS, MGH                    *
*                                                                             *
* This program is free software; you can redistribute it and/or modify it     *
* under the terms of the GNU General Public License as published by the Free  *
* Software Foundation; either version 2 of the License, or (at your option)   *
* any later version.                                                          *
*                                                                             *
* This program is distributed in the hope that it will be useful, but WITHOUT *
* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or       *
* FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for    *
* more details.                                                               *
*                                                                             *
* You should have received a copy of the GNU General Public License along     *
* with this program. If not, see <http://www.gnu.org/licenses/>.              *
*******************************************************************************
* Authors: The SOFA Team and external contributors (see Authors.txt)          *
*                                                                             *
* Contact information: contact@sofa-framework.org                             *
******************************************************************************/
#include "BatchGUI.h"
#include <sofa/simulation/Simulation.h>
#include <sofa/simulation/UpdateContextVisitor.h>
#ifdef SOFA_SMP
#include <athapascan-1>
#endif
#include <sofa/helper/system/thread/CTime.h>
#include <iostream>
#include <sstream>

namespace sofa
{

namespace gui
{

const unsigned int BatchGUI::DEFAULT_NUMBER_OF_ITERATIONS = 1000;
unsigned int BatchGUI::nbIter = BatchGUI::DEFAULT_NUMBER_OF_ITERATIONS;

BatchGUI::BatchGUI()
    : groot(NULL)
{
}

BatchGUI::~BatchGUI()
{
}

int BatchGUI::mainLoop()
{
    if (groot)
    {

        sofa::helper::AdvancedTimer::begin("Animate");
        sofa::simulation::getSimulation()->animate(groot.get());
        sofa::helper::AdvancedTimer::end("Animate");
        //As no visualization is done by the Batch GUI, these two lines are not necessary.
        sofa::simulation::getSimulation()->updateVisual(groot.get());
        std::cout << "Computing "<<nbIter<<" iterations." << std::endl;
        sofa::simulation::Visitor::ctime_t rtfreq = sofa::helper::system::thread::CTime::getRefTicksPerSec();
        sofa::simulation::Visitor::ctime_t tfreq = sofa::helper::system::thread::CTime::getTicksPerSec();
        sofa::simulation::Visitor::ctime_t rt = sofa::helper::system::thread::CTime::getRefTime();
        sofa::simulation::Visitor::ctime_t t = sofa::helper::system::thread::CTime::getFastTime();
        for (unsigned int i=0; i<nbIter; i++)
        {
            sofa::helper::AdvancedTimer::begin("Animate");
            sofa::simulation::getSimulation()->animate(groot.get());
            sofa::helper::AdvancedTimer::end("Animate");
            //As no visualization is done by the Batch GUI, these two lines are not necessary.
            sofa::simulation::getSimulation()->updateVisual(groot.get());
        }
        t = sofa::helper::system::thread::CTime::getFastTime()-t;
        rt = sofa::helper::system::thread::CTime::getRefTime()-rt;

        std::cout << nbIter << " iterations done in "<< ((double)t)/((double)tfreq) << " s ( " << (((double)tfreq)*nbIter)/((double)t) << " FPS)." << std::endl;
        std::cout << nbIter << " iterations done in "<< ((double)rt)/((double)rtfreq) << " s ( " << (((double)rtfreq)*nbIter)/((double)rt) << " FPS)." << std::endl;
    }
    return 0;
}

void BatchGUI::redraw()
{
}

int BatchGUI::closeGUI()
{
    delete this;
    return 0;
}

void BatchGUI::setScene(sofa::simulation::Node::SPtr groot, const char* filename, bool )
{
    this->groot = groot;
    this->filename = (filename?filename:"");

    resetScene();
}

void BatchGUI::resetScene()
{
    sofa::simulation::Node* root = currentSimulation();

    if ( root )
    {
        root->setTime(0.);
        simulation::getSimulation()->reset ( root );

        sofa::simulation::UpdateSimulationContextVisitor(sofa::core::ExecParams::defaultInstance()).execute(root);
    }
}

void BatchGUI::startDumpVisitor()
{
#ifdef SOFA_DUMP_VISITOR_INFO
    sofa::simulation::Node* root = currentSimulation();
    if (root)
    {
        m_dumpVisitorStream.str("");
        sofa::simulation::Visitor::startDumpVisitor(&m_dumpVisitorStream, root->getTime());
    }
#endif
}

void BatchGUI::stopDumpVisitor()
{
#ifdef SOFA_DUMP_VISITOR_INFO
    sofa::simulation::Visitor::stopDumpVisitor();
    m_dumpVisitorStream.flush();
    m_dumpVisitorStream.str("");
#endif
}

sofa::simulation::Node* BatchGUI::currentSimulation()
{
    return groot.get();
}

int BatchGUI::InitGUI(const char* /*name*/, const std::vector<std::string>& options)
{
    setNumIterations(DEFAULT_NUMBER_OF_ITERATIONS);

    //parse options
    for (unsigned int i=0 ; i<options.size() ; i++)
    {
        size_t cursor = 0;
        std::string opt = options[i];
        //Set number of iterations
        //(option = "nbIterations=N where N is the number of iterations)
        if ( (cursor = opt.find("nbIterations=")) != std::string::npos )
        {
            unsigned int nbIterations;
            std::istringstream iss;
            iss.str(opt.substr(cursor+std::string("nbIterations=").length(), std::string::npos));
            iss >> nbIterations;
            setNumIterations(nbIterations);
        }
    }
    return 0;
}

BaseGUI* BatchGUI::CreateGUI(const char* name, const std::vector<std::string>& /*options*/, sofa::simulation::Node::SPtr groot, const char* filename)
{
    BatchGUI::mGuiName = name;
    BatchGUI* gui = new BatchGUI();
    gui->setScene(groot, filename);
    return gui;
}

} // namespace gui

} // namespace sofa
<|endoftext|>


<commit_before>// -----------------------------------------------------------------------------
// Copyright 2016 Marco Biasini
//
// 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.
// -----------------------------------------------------------------------------

#ifndef TYPUS_RESULT_HH
#define TYPUS_RESULT_HH

#include <memory>
#include <cassert>
#include <type_traits>

namespace typus {

template <typename E>
struct error_traits {
    /**
     * Whether the result is OK.
     */
    static bool is_ok(const E& value);

    /**
     * Error value to be used to indicate that result is OK, e.g. contains a
     * constructed value of type T.
     */
    constexpr static E ok_value();

    /**
     * May be provided for an error type as the default error status, so
     * \code result<T,E>::fail() \endcode can be called without arguments. For
     * types where there is no clear default failed value, this can be omitted.
     */
    constexpr static E default_fail_value();
};

/**
 * Default error traits for boolean errors.
 */
template <>
struct error_traits<bool> {
    static bool is_ok(const bool& error) {
        return !error;
    }
    constexpr static bool ok_value() { return false; }
    constexpr static bool default_fail_value() { return true; }
};

namespace detail {

// Tag type for failed result construction.
struct failed_tag_t {};

// holder class for the actual result. This class is required, because we want
// to have separate implementations for trivially destructible value types and
// types that require the destructor to be invoked.
template <typename T, typename E,
          bool =std::is_trivially_destructible<T>::value>
class result_storage {
public:
    using value_type = T;
    using error_type = E;
protected:
    result_storage(): value_(), error_(error_traits<E>::ok_value()) {
    }

    result_storage(const result_storage &rhs): error_(rhs.error_) {
        if (error_traits<E>::is_ok(error_)) {
            // in-place new
            ::new (std::addressof(value_)) value_type(rhs.value_);
        }
    }

    result_storage(const result_storage &&rhs): error_(rhs.error_) {
        if (error_traits<E>::is_ok(error_)) {
            // in-place new
            ::new (std::addressof(value_)) T(std::move(rhs.value_));
        }
    }

    result_storage(const value_type &rhs):
        value_(rhs), error_(error_traits<E>::ok_value()) {
    }

    result_storage(const value_type &&rhs):
        value_(std::move(rhs)), error_(error_traits<E>::ok_value()) {
    }

    result_storage(failed_tag_t, E error): nul_state_('\0'), error_(error) {
    }

    ~result_storage() {
        if (error_traits<E>::is_ok(error_)) {
            value_.~T();
        }
    }
    union {
        char nul_state_;
        T value_;
    };
    E error_;
};

template <typename T, typename E>
class result_storage<T, E, true> {
public:
    using value_type = T;
    using error_type = E;
protected:
    result_storage(): value_(), error_(error_traits<E>::ok_value()) {
    }

    result_storage(const result_storage &rhs): error_(rhs.error_) {
        if (error_traits<E>::is_ok(error_)) {
            // in-place new
            ::new (std::addressof(value_)) value_type(rhs.value_);
        }
    }

    result_storage(const result_storage &&rhs): error_(rhs.error_) {
        if (error_traits<E>::is_ok(error_)) {
            // in-place new
            ::new (std::addressof(value_)) value_type(std::move(rhs.value_));
        }
    }

    result_storage(const value_type &rhs):
        value_(rhs), error_(error_traits<E>::ok_value()) {
    }

    result_storage(const value_type &&rhs):
        value_(std::move(rhs)), error_(error_traits<E>::ok_value()) {
    }

    result_storage(failed_tag_t, E error): nul_state_('\0'), error_(error) {
    }

    // nothing to be done, value_type is trivially destructible
    ~result_storage() { }
    union {
        char nul_state_;
        T value_;
    };
    E error_;
};

} // namespace detail

/**
 * \brief class holding a value of type T or an error of type E.
 *
 * Typical uses of this class include as a return type for a function/method
 * that can fail. The result acts as a replacement for exception-based error
 * handling.
 */
template <typename T, typename E=bool>
class result : public detail::result_storage<T, E> {
public:
    typedef T value_type;
    typedef E error_type;

    /**
     * \brief construct a failed result using the provided error value
     */
    static result<T, E> fail(E error) {
        return result<T,E>(detail::failed_tag_t{}, error);
    }
    /**
     * \brief construct a failed result using the default fail value.
     *
     * This is probably only useful when using a boolean error type as
     * for all others there is not meaningful default error value.
     */
    static result<T, E> fail() {
        return result<T,E>(detail::failed_tag_t{},
                           error_traits<E>::default_fail_value());
    }

    /**
     * \brief whether the result contains a valid value.
     */
    bool ok() const {
        return error_traits<E>::is_ok(this->error_);
    }

    /**
     * \brief whether the result contains a valid value.
     *
     * Identical to calling \ref ok.
     */
    operator bool() const {
        return this->ok();
    }

    /**
     * \brief access the value in-place.
     *
     * Aborts if the result does not hold a valid value.
     */
    const T& value() const {
        assert(this->ok());
        return this->value_;
    }
    /**
     * \brief access the value in-place.
     *
     * Aborts if the result does not hold a valid value.
     */
    T& value() {
        assert(this->ok());
        return this->value_;
    }

    /**
     * \brief extract the value out of the result.
     */
    T && extract() {
        assert(this->ok());
        return std::move(this->value_);
    }
public:
    /**
     * \brief create a result containing a default-constructed value.
     */
    result() = default;
    /**
     * \brief copy-construct a result.
     */
    result(const result& rhs) = default;
    /**
     * \brief construct a new result holding a value by copy-constructor.
     */
    result(const  value_type &rhs): detail::result_storage<T, E>(rhs) {}
    /**
     * \brief construct a new result holding a value through move construction.
     */
    result(value_type &&rhs): detail::result_storage<T, E>(std::move(rhs)) {}

    template <typename T2, typename E2>
    friend class result;

    /**
     * Allow for implicit conversion from one failed result type to another.
     * Only supported if error types are the same. aborts if the value is
     * ok.
     */
    template<typename U>
    result(const result<U, E> &other):
        detail::result_storage<T,E>(detail::failed_tag_t{}, other.error_) {
        assert(!other.ok());
    }

    /**
     * \brief The error state of the result.
     */
    E error() const { return this->error_; }
private:
    result(detail::failed_tag_t, const error_type &e):
        detail::result_storage<T, E>(detail::failed_tag_t{}, e) {
    }
};

/**
 * macro for Rust-style function return value checking. Note that is is
 * not standard C++ and is only supported in clang and gcc, since it relies
 * on statement expressions. So if you want to be portable, don't use it.
 *
 * Typical usage:
 * \code
 * result<std::string> may_fail() { ... }
 *
 * result<std::string> foo() {
 *    std::string value  = TRY(may_fail());
 *    return do_stuff(value);
 *
 * }
 * \endcode
 */
#define TRY(expr) ({ \
         auto v = expr; \
         if (!v.ok()) { \
             return v; \
         }; \
         v.extract(); \
    })

} // namespace typus

#endif // TYPUS_RESULT_HH

<commit_msg>fix typo<commit_after>// -----------------------------------------------------------------------------
// Copyright 2016 Marco Biasini
//
// 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.
// -----------------------------------------------------------------------------

#ifndef TYPUS_RESULT_HH
#define TYPUS_RESULT_HH

#include <memory>
#include <cassert>
#include <type_traits>

namespace typus {

template <typename E>
struct error_traits {
    /**
     * Whether the result is OK.
     */
    static bool is_ok(const E& value);

    /**
     * Error value to be used to indicate that result is OK, e.g. contains a
     * constructed value of type T.
     */
    constexpr static E ok_value();

    /**
     * May be provided for an error type as the default error status, so
     * \code result<T,E>::fail() \endcode can be called without arguments. For
     * types where there is no clear default failed value, this can be omitted.
     */
    constexpr static E default_fail_value();
};

/**
 * Default error traits for boolean errors.
 */
template <>
struct error_traits<bool> {
    static bool is_ok(const bool& error) {
        return !error;
    }
    constexpr static bool ok_value() { return false; }
    constexpr static bool default_fail_value() { return true; }
};

namespace detail {

// Tag type for failed result construction.
struct failed_tag_t {};

// holder class for the actual result. This class is required, because we want
// to have separate implementations for trivially destructible value types and
// types that require the destructor to be invoked.
template <typename T, typename E,
          bool =std::is_trivially_destructible<T>::value>
class result_storage {
public:
    using value_type = T;
    using error_type = E;
protected:
    result_storage(): value_(), error_(error_traits<E>::ok_value()) {
    }

    result_storage(const result_storage &rhs): error_(rhs.error_) {
        if (error_traits<E>::is_ok(error_)) {
            // in-place new
            ::new (std::addressof(value_)) value_type(rhs.value_);
        }
    }

    result_storage(const result_storage &&rhs): error_(rhs.error_) {
        if (error_traits<E>::is_ok(error_)) {
            // in-place new
            ::new (std::addressof(value_)) T(std::move(rhs.value_));
        }
    }

    result_storage(const value_type &rhs):
        value_(rhs), error_(error_traits<E>::ok_value()) {
    }

    result_storage(const value_type &&rhs):
        value_(std::move(rhs)), error_(error_traits<E>::ok_value()) {
    }

    result_storage(failed_tag_t, E error): nul_state_('\0'), error_(error) {
    }

    ~result_storage() {
        if (error_traits<E>::is_ok(error_)) {
            value_.~T();
        }
    }
    union {
        char nul_state_;
        T value_;
    };
    E error_;
};

template <typename T, typename E>
class result_storage<T, E, true> {
public:
    using value_type = T;
    using error_type = E;
protected:
    result_storage(): value_(), error_(error_traits<E>::ok_value()) {
    }

    result_storage(const result_storage &rhs): error_(rhs.error_) {
        if (error_traits<E>::is_ok(error_)) {
            // in-place new
            ::new (std::addressof(value_)) value_type(rhs.value_);
        }
    }

    result_storage(const result_storage &&rhs): error_(rhs.error_) {
        if (error_traits<E>::is_ok(error_)) {
            // in-place new
            ::new (std::addressof(value_)) value_type(std::move(rhs.value_));
        }
    }

    result_storage(const value_type &rhs):
        value_(rhs), error_(error_traits<E>::ok_value()) {
    }

    result_storage(const value_type &&rhs):
        value_(std::move(rhs)), error_(error_traits<E>::ok_value()) {
    }

    result_storage(failed_tag_t, E error): nul_state_('\0'), error_(error) {
    }

    // nothing to be done, value_type is trivially destructible
    ~result_storage() { }
    union {
        char nul_state_;
        T value_;
    };
    E error_;
};

} // namespace detail

/**
 * \brief class holding a value of type T or an error of type E.
 *
 * Typical uses of this class include as a return type for a function/method
 * that can fail. The result acts as a replacement for exception-based error
 * handling.
 */
template <typename T, typename E=bool>
class result : public detail::result_storage<T, E> {
public:
    typedef T value_type;
    typedef E error_type;

    /**
     * \brief construct a failed result using the provided error value
     */
    static result<T, E> fail(E error) {
        return result<T,E>(detail::failed_tag_t{}, error);
    }
    /**
     * \brief construct a failed result using the default fail value.
     *
     * This is probably only useful when using a boolean error type as
     * for all others there is no meaningful default error value.
     */
    static result<T, E> fail() {
        return result<T,E>(detail::failed_tag_t{},
                           error_traits<E>::default_fail_value());
    }

    /**
     * \brief whether the result contains a valid value.
     */
    bool ok() const {
        return error_traits<E>::is_ok(this->error_);
    }

    /**
     * \brief whether the result contains a valid value.
     *
     * Identical to calling \ref ok.
     */
    operator bool() const {
        return this->ok();
    }

    /**
     * \brief access the value in-place.
     *
     * Aborts if the result does not hold a valid value.
     */
    const T& value() const {
        assert(this->ok());
        return this->value_;
    }
    /**
     * \brief access the value in-place.
     *
     * Aborts if the result does not hold a valid value.
     */
    T& value() {
        assert(this->ok());
        return this->value_;
    }

    /**
     * \brief extract the value out of the result.
     */
    T && extract() {
        assert(this->ok());
        return std::move(this->value_);
    }
public:
    /**
     * \brief create a result containing a default-constructed value.
     */
    result() = default;
    /**
     * \brief copy-construct a result.
     */
    result(const result& rhs) = default;
    /**
     * \brief construct a new result holding a value by copy-constructor.
     */
    result(const  value_type &rhs): detail::result_storage<T, E>(rhs) {}
    /**
     * \brief construct a new result holding a value through move construction.
     */
    result(value_type &&rhs): detail::result_storage<T, E>(std::move(rhs)) {}

    template <typename T2, typename E2>
    friend class result;

    /**
     * Allow for implicit conversion from one failed result type to another.
     * Only supported if error types are the same. aborts if the value is
     * ok.
     */
    template<typename U>
    result(const result<U, E> &other):
        detail::result_storage<T,E>(detail::failed_tag_t{}, other.error_) {
        assert(!other.ok());
    }

    /**
     * \brief The error state of the result.
     */
    E error() const { return this->error_; }
private:
    result(detail::failed_tag_t, const error_type &e):
        detail::result_storage<T, E>(detail::failed_tag_t{}, e) {
    }
};

/**
 * macro for Rust-style function return value checking. Note that is is
 * not standard C++ and is only supported in clang and gcc, since it relies
 * on statement expressions. So if you want to be portable, don't use it.
 *
 * Typical usage:
 * \code
 * result<std::string> may_fail() { ... }
 *
 * result<std::string> foo() {
 *    std::string value  = TRY(may_fail());
 *    return do_stuff(value);
 *
 * }
 * \endcode
 */
#define TRY(expr) ({ \
         auto v = expr; \
         if (!v.ok()) { \
             return v; \
         }; \
         v.extract(); \
    })

} // namespace typus

#endif // TYPUS_RESULT_HH

<|endoftext|>


<commit_before>#ifndef NEWARRAYS_H_INCLUDED
#define NEWARRAYS_H_INCLUDED
///////////////////////////

/// #define ARRAYS_DEBUG

///////////////////////////
#include <wiz/global.h>
#include <wiz/wizardError.h>

namespace wiz{
template <class T>
class Array{
private:
    T* p; // array
    int N; // array
private:
    static T sBegin; /// static -> s, /// to not static?
    static T sEnd;

private:
    void resetP( const T* p )
    {
        if( nullptr != this->p && nullptr != p )
        {
            for( int i=0;i< N;i++ ){
                this->p[i] = p[i];
            }
        }
    }
///for c++11
public:
    class Iter //
    {
    private:
        T* _pt;//
        /// following constant values..
        T* p;
        int N;
        T* pBegin;
        T* pEnd;
    public:
        explicit Iter( T* _pt, T* p, int N, T* pBegin, T* pEnd ) : _pt(_pt),
        p(p), N(N), pBegin(pBegin), pEnd(pEnd)
        {
            if( nullptr == _pt ) { throw wiz::Error( "nullptr == _pt in Iter" ); }
        }
        //
        void left() {
            // p == nullptr -> err.
            if( p == _pt ) { _pt = pBegin; }
            else if( _pt == pBegin ) { throw wiz::Error( " _pt == pBegin in left " ); }
            else if( _pt == pEnd ) { _pt = p+N-1; }
            else {
                _pt--;
            }
        }
        void right() {
            // p == nullptr -> err.
            if( p+N-1 == _pt ) { _pt = pEnd; }
            else if( _pt == pEnd ) { throw wiz::Error( " _pt == pEnd in right " ); }
            else if( _pt == pBegin ) { _pt = p; }
            else {
                _pt++;
            }
        }
        T data()const { return *_pt; }
        bool isnullptr()const { return nullptr == _pt; }
        bool operator!=( const Iter& x ) const
        {
            return x._pt != this->_pt;
        }
        void operator++() { right(); }
        void operator--() { left(); }
        /// TO DO
        /// T* operator->()
        /// const T* operator->()const
        T* operator->() { return _pt; }
        const T* operator->()const { return _pt; }
        T& operator*() { return *_pt; }
        const T& operator*()const { return *_pt; }
    };
    class ConstIter //
    {
    private:
        T* _pt;//
        /// following constant values..
        T* p;
        int N;
        T* pBegin;
        T* pEnd;
    public:
        explicit ConstIter( T* _pt, T* p, int N, T* pBegin, T* pEnd ) : _pt(_pt),
        p(p), N(N), pBegin(pBegin), pEnd(pEnd)
        {
            if( nullptr == _pt ) { throw wiz::Error( "nullptr == _pt in ConstIter" ); }
        }
        //
        void left() {
            // p == nullptr -> err.
            if( p == _pt ) { _pt = pBegin; }
            else if( _pt == pBegin ) { throw wiz::Error( " _pt == pBegin in left " ); }
            else if( _pt == pEnd ) { _pt = p+N-1; }
            else {
                _pt--;
            }
        }
        void right() {
            // p == nullptr -> err.
            if( p+N-1 == _pt ) { _pt = pEnd; }
            else if( _pt == pEnd ) { throw wiz::Error( " _pt == pEnd in right " ); }
            else if( _pt == pBegin ) { _pt = p; }
            else {
                _pt++;
            }
        }
        T data()const { return *_pt; }
        bool isnullptr()const { return nullptr == _pt; }
        bool operator!=( const ConstIter& x ) const
        {
            return x._pt != this->_pt;
        }
        void operator++() { right(); }
        void operator--() { left(); }

        const T* operator->()const { return _pt; }
        const T& operator*()const { return *_pt; }
    };
    Iter begin()  { if( empty() ) { return end(); }
        return Iter( p, p, N, &sBegin, &sEnd );
    }
    Iter end()  { return Iter( &sEnd, p, N, &sBegin, &sEnd ); }
    Iter rbegin()  { if( empty() ){ return rend(); }
        return Iter( p+N-1, p, N, &sBegin, &sEnd );
    }

    Iter rend()  { return Iter( &sBegin, p, N, &sBegin, &sEnd ); }
    /// Check....
    ConstIter crend() const { return ConstIter( &sBegin, p, N, &sBegin, &sEnd ); }
    ConstIter cbegin() const { if( empty() ) { return cend(); }
        return ConstIter( p, p, N, &sBegin, &sEnd );
    }
    ConstIter cend() const { return ConstIter( &sEnd, p, N, &sBegin, &sEnd ); }
    ConstIter crbegin() const { if( empty() ){ return crend(); }
        return ConstIter( p+N-1, p, N, &sBegin, &sEnd );
    }

    explicit Array( initializer_list<T> args )
    {
        N = args.size();

        #ifdef ARRAYS_DEBUG
        wiz::checkUnderError( 1, N, 0 );
        #endif

        if( 0 == N ) { p = nullptr; return; }
        p = new T[N];

        int count = 0;
        for( auto iter = args.begin(); iter != args.end(); ++iter )
        {
            p[count] = *iter;
            count++;
        }

    }

    Array<T>& operator=( Array<T>&& arr )
    {
        if( p == arr.p ) { return *this; }

        if( p ) { delete[] p; }
        p = arr.p; arr.p = nullptr;
        N = arr.N; arr.N = 0;

        return *this;
    }
    Array( Array<T>&& arr )
    {
        p = arr.p; arr.p = nullptr;
        N = arr.N; arr.N = 0;
    }

//
public:
	explicit Array(const int _N, const T reset_val = T()) : N(_N)
	{
#ifdef ARRAYS_DEBUG
		wiz::checkUnderError(2, N, 1);
#endif
		p = new T[_N];

		//reset( reset_val );
		if (!std::is_class<T>::value)
		{
			for (int i = 0; i < N; i++) {
				p[i] = reset_val;
			}
		}
	}

    Array( const Array<T>& _arr )
    : p(nullptr), N(0)
    {
        *this = _arr;
    }
    explicit Array() : p(nullptr), N(0)
    { }

    virtual ~Array()
    {
        if( p )
            delete[] p;
    }

    void reset( const T& resetValue )
    {
        for( int i=0;i< N;i++ )//
            p[i] = resetValue;
    }
    bool operator==( const Array<T>& arr ) const ///  ..
    {
        return this->p == arr.p;
    }
    bool operator!=( const Array<T>& arr ) const
    {
        return this->p != arr.p;
    }
    /// cf) isSameValues?
    template<class IS_SAME_VALUE=EQ<T>> /// need more thinking..
    bool isSameValue( const Array<T>& arr ) const
    {
        if( arr.size() != size() ) { return false; }
        for( int i=0; i < arr.size(); i++ )
        {
            if( !( IS_SAME_VALUE()( arr[i], p[i] ) ) ) { return false; }
        }
        return true;
    }
    // ũ
    int size()const{ return N; }
    int length()const{ return N; }
    //
    T& operator[]( const int index )
    {
        #ifdef ARRAYS_DEBUG
        wiz::checkIndexError( index, N );
        #endif
		return p[index];
    }
    const T& operator[]( const int index )const
    {
        #ifdef ARRAYS_DEBUG
        wiz::checkIndexError( index, N );
        #endif
		return p[index];
    }
    //
    Array<T>& operator=( const Array<T>& _p )
    {
        if( _p.p == p ) { return *this; }

        if( nullptr != p && size() != _p.size() )
        {
            delete[] p;
            p = nullptr;
        }

		if( nullptr == p && nullptr != _p.p )
        {
            p = new T[_p.N];
        }

        //reset
        N = _p.N;
        resetP( _p.p );

        return *this;
    }

    bool empty()const
    {
    	return nullptr == p;
    }
    bool isValid()const
    {
        return !empty();
    }
    void DoEmpty()
    {
        if( p ) delete[] p;
        p = nullptr;
        N = 0;
    }
public:
	void expand()
	{
		if( nullptr != p )
		{
		    // 2012.5.8
			const int newN = 2 * N;
            T* temp = new T[2*N]; // new Array!

            // data copy!
		    for( int i=0; i < N; i++ )
			{
				temp[i] = std::move( p[i] ); // cf)  Ѱ p[i]  ???
			}

			if (!std::is_class<T>::value)
			{
				for (int i = N; i < newN; i++)
				{
					temp[i] = T();
				}
			}

            ///
            delete[] p;
            p = temp; N = newN;
		}
	}

public: /// Done-swap..
    void swap( Array<T>& arr )
    {
        wiz::Swap( this->p, arr.p );
        wiz::Swap( this->N, arr.N );
    }
};// end class array
template <class T>
T Array<T>::sBegin = T();
template <class T>
T Array<T>::sEnd = T();
// not use debug() and use below!
template <class T>
inline ostream& operator<<( ostream& stream, const Array<T>& arr )
{
    if( arr.empty() ) { return stream; }

    for( int index=0; index < arr.length()-1; index++ )
        stream << arr[index] << " ";
    stream << arr[arr.length()-1]; // chk 2012.7.30

    return stream;
}
template <class T>
inline istream& operator>>( istream& stream, Array<T>& arr )
{
    for( int i=0; i < arr.size(); ++i )
    {
        stream >> arr[i];
    }

    return stream;
}
template <class T>
class SecondArray{ /// To Do Add bool isSameValue?
private:
    Array<T>** p;
    int row_N; // array
    int column_N;

private:
    void Delete() /// to public??
    {
		if( nullptr != p )
        {
			for( int i=0;i< row_N;i++ )
			{
				if( p[i] ){ delete p[i]; p[i] = nullptr; }
			}
			delete[] p;//
            p = nullptr;
        }
        row_N = 0; column_N = 0;
    }

    void reset( const int row_N, const int column_N )
    {
        #ifdef ARRAYS_DEBUG
        checkUnderError( 4, row_N, 0 );
        checkUnderError( 5, column_N, 0 );
        #endif

        this->row_N = row_N;
        this->column_N = column_N;
    }

public:
    explicit SecondArray()
	:  p( nullptr ), row_N( 0 ), column_N( 0 )
	{
        //
	}
    explicit SecondArray( const int _row_N, const int _column_N,  const T _resetValue=T() )
    : row_N(_row_N), column_N(_column_N)
    {
        #ifdef ARRAYS_DEBUG
        wiz::checkUnderError( 6, row_N, 1 );
        wiz::checkUnderError( 7, column_N, 1 );
        #endif

        p = new Array<T>*[row_N];
        for( int i=0;i< row_N;i++ )
        {
            p[i] = new Array<T>( column_N, _resetValue );
        }
    }

    virtual ~SecondArray()
    {
        Delete();
    }
    // ִ ũ⸦ ޴´.
    SecondArray( const SecondArray<T>& dap )
    : p( nullptr ), row_N( 0 ), column_N( 0 )
    {
        *this = dap;
    }

    SecondArray<T>& operator=( const SecondArray<T>& dap ) /// need more thinking...
    {
        if( dap.p == p ) { return *this; }

        /// nullptr != p && nullptr == dap.p -> ( nullptr!=p && other size ) is true.
        if( nullptr != p && ( row_N != dap.row_N || column_N != dap.column_N ) )
        /// nullptr != p && nullptr == dap.p
        /// nullptr != p && nullptr != dap.p && ( different row_size or different col_size )
        {
            for( int i=0; i < this->row_N; ++i )
            {
                delete p[i];
            }
            delete[] p;
            p = nullptr;
        }

    	if( nullptr == p && nullptr != dap.p )
        {
            p = new Array<T>*[dap.row_N];
            for( int i=0;i< dap.row_N;i++ )
            {
                p[i] = new Array<T>( *(dap.p[i]) );
            }
        }
        else /// else if( nullptr != p && nullptr != dap.p && same row_size && same col_size )
             /// else if( nullptr == p && nullptr == dap.p ) -> dap.row_N == 0.
        {
            for( int i=0; i < dap.row_N; i++ )
            {
                (*p[i]) = (*dap.p[i]);
            }
        }

        reset( dap.row_N, dap.column_N );
        //
        return *this;
    }

    /// DONE 2014.3.7 - ̵, ̵Կ ߰.
    SecondArray<T>& operator=( SecondArray<T>&& arr )
    {
        if( p == arr.p ) { return *this; }
        if( p ) { Delete(); }
        p = arr.p;
        row_N = arr.row_N;
        column_N = arr.column_N;

        arr.p = nullptr;
        arr.row_N = 0;
        arr.column_N = 0;
        return *this;
    }
    SecondArray( SecondArray<T>&& arr )
    {
        p = arr.p; arr.p = nullptr;
        row_N = arr.row_N; arr.row_N = 0;
        column_N = arr.column_N; arr.column_N = 0;
    }

    /// chk [i].size() == column_N ??
    T getValue( const int i, const int j )const
    {
        #ifdef ARRAYS_DEBUG
        wiz::checkIndexError( i, row_N );
        wiz::checkIndexError( j, column_N );
        #endif
        return (*p[i])[j]; //
    }

    void setValue( const int i, const int j, const T& set )
    {
        #ifdef ARRAYS_DEBUG
        wiz::checkIndexError( i, row_N );
        wiz::checkIndexError( j, column_N );
        #endif
        (*p[i])[j] = set;
    }

    // get size
    int getRowN()const{ return row_N; }
    int getColumnN()const{ return column_N; }

    void reset( const T& val )
    {
        for( int i=0;i< row_N;i++ ){
            for( int j=0;j< column_N;j++ ){
                (*p[i])[j] = val;
            }
        }
    }

    /// chk [i].size() == column_N ??
    Array<T>& operator[]( const int index )
    {
        #ifdef ARRAYS_DEBUG
        wiz::checkIndexError( index, row_N );
        wiz::assertEquals( p[index]->size(), column_N );
        #endif
        return *(p[index]);
    }

    const Array<T>& operator[]( const int index )const
    {
        #ifdef ARRAYS_DEBUG
        wiz::checkIndexError( index, row_N );
        #endif
        return *(p[index]);
    }

    bool empty() const
    {
        return nullptr == p;
    }
	bool isValid()const
    {
    	return nullptr != p;
    }
   //Array<T> getColumnArray( const int columnNo );
   //Array<T> getRowArray( const int rowNo );
public:
    virtual bool isSquareSecondArray(){ return false; }
    /// ToDo swap
    void swap( SecondArray<T>& sarr )
    {
        wiz::Swap( this->p, sarr.p );
        wiz::Swap( this->row_N, sarr.row_N );
        wiz::Swap( this->column_N, sarr.column_N );
    }

    /// ToDO row_expand, col_expand, row_col_expand. - *2, *2, *4.
    void row_expand()
    {
        if( isSquareSecondArray() ) { return; } /// throw err.?
        if( nullptr != p ) {
            const int new_row_size = 2 * row_N;
            Array<T>** temp = new Array<T>*[new_row_size];

            for( int i=0; i < row_N; i++ ) {
                temp[i] = p[i];
            }
            for( int i=row_N; i < new_row_size; i++ ) {
                temp[i] = new Array<T>( column_N, T() );
            }
            delete[] p;
            p = temp;
            row_N = new_row_size;
        }
    }
};// end double array
template <typename T>
class SquareSecondArray : public SecondArray<T> /// row_expand -> err. ???
{
public:
    virtual bool isSquareSecondArray(){ return true; }
public:
    SquareSecondArray() : SecondArray<T>() { }
    virtual ~SquareSecondArray() { }
    SquareSecondArray( const SquareSecondArray& sda ) : SecondArray<T>( sda ) { }
    SquareSecondArray( const int N, const T init=T() ):SecondArray<T>( N, N, init ){
        //
    }

    int getN()const{ return SecondArray<T>::getColumnN(); }

    void Transpose() // only n*n!// cf) return transposed Square Double Array?
    { //
        const int N = SecondArray<T>::getRowN();

        for( int i=0;i < N; i++ ){
            for( int j=i+1;j < N; j++ ){
                // swap [i][j] and [j][i];
                wiz::Swap( (*this)[i][j], (*this)[j][i] );
            }
        }
    }
};
// not use debug() and use below!
template <class T>
inline ostream& operator<<( ostream& stream, const SecondArray<T>& arr )
{
    for( int i=0; i < arr.getRowN(); i++ )
    {
        stream << arr[i];
        stream << "\n";
    }

    return stream;
}

template <class T>
inline istream& operator>>( istream& stream, SecondArray<T>& arr )
{
    for( int i=0; i < arr.getRowN(); ++i )
    {
        stream >> arr[i];
    }

    return stream;
}
}
#endif // NEWARRAYS_H_INCLUDED
<commit_msg>Delete NEWARRAYS.H<commit_after><|endoftext|>


<commit_before>/* Siconos-sample version 3.1.0, Copyright INRIA 2005-2009.
 * Siconos is a program dedicated to modeling, simulation and control
 * of non smooth dynamical systems.
 * Siconos is a free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 * Siconos is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with Siconos; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 *
 * Contact: Vincent ACARY vincent.acary@inrialpes.fr
 *
 */

#ifndef BodiesViewer_hpp
#define BodiesViewer_hpp

/* Siconos */
#include <SiconosKernel.hpp>

#include <Disk.hpp>
#include <DiskDiskR.hpp>
#include <DiskPlanR.hpp>
#include <DiskMovingPlanR.hpp>
#include <Circle.hpp>
#include <CircleCircleR.hpp>
#include <SphereLDS.hpp>
#include <SphereLDSPlanR.hpp>
#include <SphereLDSSphereLDSR.hpp>
#include <SphereNEDS.hpp>
#include <SphereNEDSPlanR.hpp>
#include <SphereNEDSSphereNEDSR.hpp>

#if WITH_BULLET
#include <BulletDS.hpp>
#include <BulletR.hpp>
#define IFBULLET(X) X
#else
#define IFBULLET(X)
#endif

#include "SiconosBodies.hpp"

/* QGLViewer */
#include <qglviewer.h>
#include <qapplication.h>

#include <qevent.h>
#include <qmessagebox.h>

#ifdef QT_INTERFACE
#include "ui_viewerInterface.Qt4.h"
class ViewerInterface : public QDialog, public Ui::Dialog
{
public:
  ViewerInterface()
  {
    setupUi(this);
  }
};
#endif

/* convenient macros */

#define GETALLDS(M) M->model()->nonSmoothDynamicalSystem()->topology()->dSG(0)
#define GETNDS(M) GETALLDS(M)->size()

struct ForNdof : public Question<unsigned int>
{
  ANSWER_V(Disk, 3);
  ANSWER_V(Circle, 3);
  ANSWER_V(SphereLDS, 6);
  ANSWER_V(SphereNEDS, 6);
  IFBULLET(ANSWER_V(BulletDS, 6));
};

struct ForFExt : public Question<SP::SiconosVector>
{

  ANSWER(Disk, fExt());
  ANSWER(Circle, fExt());
  ANSWER(SphereLDS, fExt());
  ANSWER(SphereNEDS, fExt());
  IFBULLET(ANSWER(BulletDS, fExt()));
};

struct ForPosition : public Question<SP::SiconosVector>
{

  ANSWER(Disk, q());
  ANSWER(Circle, q());
  ANSWER(SphereLDS, q());
  ANSWER(SphereNEDS, q());
  IFBULLET(ANSWER(BulletDS, q()));
};

struct ForRadius : public Question<double>
{

  ANSWER(Disk, getRadius());
  ANSWER(Circle, getRadius());
  ANSWER(SphereLDS, getRadius());
  ANSWER(SphereNEDS, getRadius());
  IFBULLET(ANSWER_V(BulletDS, 0.)); // fix
};

struct ForMassValue : public Question<double>
{
  ANSWER(Disk, mass()->getValue(0, 0));
  ANSWER(Circle, mass()->getValue(0, 0));
  ANSWER(SphereLDS, mass()->getValue(0, 0));
  ANSWER(SphereNEDS, massValue());
  IFBULLET(ANSWER(BulletDS, massValue()));
};

struct ForJachq : public Question<SP::SiconosMatrix>
{
  ANSWER(LagrangianR, jachq());
  ANSWER(NewtonEulerR, jachq());
  ANSWER(DiskDiskR, jachq());
  ANSWER(DiskPlanR, jachq());
  ANSWER(DiskMovingPlanR, jachq());
  ANSWER(SphereLDSPlanR, jachq());
  ANSWER(SphereNEDSPlanR, jachq());
  ANSWER(SphereLDSSphereLDSR, jachq());
  ANSWER(SphereNEDSSphereNEDSR, jachq());
  IFBULLET(ANSWER(BulletR, jachq()));
};

struct ForContactForce : public Question<SP::SiconosVector>
{
  IFBULLET(ANSWER(BulletR, contactForce()));
};

#define GETX(C) ask<ForPosition>(*C)->getValue(0)
#define GETY(C) ask<ForPosition>(*C)->getValue(1)
#define GETZ(C) ask<ForPosition>(*C)->getValue(2)
#define GETA1(C) ask<ForPosition>(*C)->getValue(3)
#define GETA2(C) ask<ForPosition>(*C)->getValue(4)
#define GETA3(C) ask<ForPosition>(*C)->getValue(5)
#define GETA4(C) ask<ForPosition>(*C)->getValue(6)
#define GETXFE(C) ask<ForFExt>(*C)->getValue(0)
#define GETYFE(C) ask<ForFExt>(*C)->getValue(1)
#define GETZFE(C) ask<ForFExt>(*C)->getValue(2)

#define GETVX(C) ask<ForVelocity>(*C)->getValue(0)
#define GETVY(C) ask<ForVelocity>(*C)->getValue(1)
#define GETVZ(C) ask<ForVelocity>(*C)->getValue(2)

#define GETVA1(C) ask<ForVelocity>(*C)->getValue(3)
#define GETVA2(C) ask<ForVelocity>(*C)->getValue(4)
#define GETVA3(C) ask<ForVelocity>(*C)->getValue(5)

#define GETRADIUS(C) ask<ForRadius>(*C)

enum SHAPE
{
  DISK,
  CIRCLE,
  SPHERE,
  IFBULLET(BULLET)
};

struct ForShape : public Question<SHAPE>
{
  ANSWER_V(Disk, DISK);
  ANSWER_V(Circle, CIRCLE);
  ANSWER_V(SphereLDS, SPHERE);
  ANSWER_V(SphereNEDS, SPHERE);
  IFBULLET(ANSWER_V(BulletDS, BULLET));
};

/* dynamical system / figure association */
class QGLShape
{

public:

  /* construction from a LagrangianDS */
  QGLShape(SHAPE f, SP::DynamicalSystem D, const qglviewer::Frame* ref)
  {
    assert(D);

    figure_ = f;
    DS_ = D;
    frame_.reset(new qglviewer::ManipulatedFrame());
    frame_->setReferenceFrame(ref);
    savedFExt_ = ask<ForFExt>(*DS());
    selected_ = false;
    saved_ = true;
    type_ = Type::value(*DS());

  };

  ~QGLShape() {};

  /* pointer to DS */
  SP::DynamicalSystem DS() const
  {
    return DS_;
  };

  /* selection with mouse */
  bool selected()
  {
    return selected_ ;
  };
  void setSelection(bool v)
  {
    selected_ = v ;
  };

  /* identifiant */
  int getD()
  {
    return id_ ;
  };
  void setID(int i)
  {
    id_ = i;
  };

  /* External force set from mouse and restore */
  void saveFExt()
  {
    savedFExt_ = ask<ForFExt>(*DS());
  };
  void restoreFExt()
  {
    switch (Type::value(*DS()))
    {
    case Type::NewtonEulerDS :
    {
      std11::static_pointer_cast<NewtonEulerDS>(DS())
      ->setFExtPtr(std11::static_pointer_cast<SiconosVector>(savedFExt_));
      break;
    }
    case Type::LagrangianDS :
    {
      std11::static_pointer_cast<LagrangianDS>(DS())
      ->setFExtPtr(std11::static_pointer_cast<SiconosVector>(savedFExt_));
      break;
    };
    default:
    {};
    }
  };

  /* DS frame */
  qglviewer::ManipulatedFrame * frame()
  {
    return frame_.get();
  };

  SHAPE shape() const
  {
    return figure_;
  }

  Type::Siconos type() const
  {
    return type_;
  }

protected:
  int id_;
  bool selected_;
  bool saved_;
  SP::SiconosVector savedFExt_;
  boost::shared_ptr<qglviewer::ManipulatedFrame> frame_;

  SHAPE figure_;
  SP::DynamicalSystem DS_;
  int positionSize_;

  Type::Siconos type_;

};

TYPEDEF_SPTR(QGLShape);

/* QGLViewer main object */
class BodiesViewer : public QGLViewer
{

public:

#ifdef QT_INTERFACE
  Viewer(QWidget *parent);
#endif

public:
  virtual void draw() = 0;
  void drawWithNames();
  void initUCircle();
  void drawUCircle();
  void drawUDisk();
  void drawUCircleTicks(float a);
  void drawUDiskTicks(float a) ;
  void drawUTriangle(float depth = 0.) ;
  void drawVec(float x1, float y1, float x2, float y2) ;
  void drawRec(float x1, float y1, float x2, float y2, float w, float z = 0.);
  void drawArrow(float x1, float y1, float x2, float y2, float w);
  void drawPar(float x1, float y1, float z1, float x2, float y2, float z2, float w);
  void drawArrow(float x1, float y1, float z1, float x2, float y2, float z2, float w);
  void drawCircleTicks(float x, float y, float a, float r);
  void drawDiskTicks(float x, float y, float a, float r, float *c);
  void drawSimpleCircle(float x, float y, float r);
  void drawCircle(float x, float y, float a, float r, float *c);
  void drawDisk(float x, float y, float a, float r, float *c);
  void rotate(const float R[12]);
  void drawSphere(float x, float y, float z, float theta, float phi, float psi, float r, float *c);
  void drawSphere(float x, float y, float z, float a, float b, float c, float d, float r, float *color);
  void drawPolyg(unsigned int n, double* coor, float *c);

  virtual void drawQGLShape(const QGLShape&);
  virtual void drawSelectedQGLShape(const QGLShape&);
  void insertQGLShape(SHAPE, SP::DynamicalSystem);

protected :

  void postSelection(const QPoint& point);
  virtual void init();
  virtual void animate();
  virtual void mousePressEvent(QMouseEvent *);
  virtual void mouseMoveEvent(QMouseEvent *);
  virtual void mouseReleaseEvent(QMouseEvent *);
  virtual void keyPressEvent(QKeyEvent *);

  virtual QString helpString() const;
  boost::shared_ptr<qglviewer::WorldConstraint> constraint_;

  SP::SiconosBodies Siconos_;

  qglviewer::Frame* referenceFrame_;

  std::vector<SP::QGLShape>  shapes_;

  void print(float x, float y, const char *s, int size);

  int NDS_;

  qglviewer::Vec selectedPoint_;

  int lastSelected_;

  bool myMouseBehavior_;

  bool stepSimulation_;
  bool stepNow_;

  long timeSiconos_;

  long timeGlob_;

  float _transparency;

};

#endif
<commit_msg>using SiconosVisitor::visit<commit_after>/* Siconos-sample version 3.1.0, Copyright INRIA 2005-2009.
 * Siconos is a program dedicated to modeling, simulation and control
 * of non smooth dynamical systems.
 * Siconos is a free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 * Siconos is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with Siconos; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 *
 * Contact: Vincent ACARY vincent.acary@inrialpes.fr
 *
 */

#ifndef BodiesViewer_hpp
#define BodiesViewer_hpp

/* Siconos */
#include <SiconosKernel.hpp>

#include <Disk.hpp>
#include <DiskDiskR.hpp>
#include <DiskPlanR.hpp>
#include <DiskMovingPlanR.hpp>
#include <Circle.hpp>
#include <CircleCircleR.hpp>
#include <SphereLDS.hpp>
#include <SphereLDSPlanR.hpp>
#include <SphereLDSSphereLDSR.hpp>
#include <SphereNEDS.hpp>
#include <SphereNEDSPlanR.hpp>
#include <SphereNEDSSphereNEDSR.hpp>

#if WITH_BULLET
#include <BulletDS.hpp>
#include <BulletR.hpp>
#define IFBULLET(X) X
#else
#define IFBULLET(X)
#endif

#include "SiconosBodies.hpp"

/* QGLViewer */
#include <qglviewer.h>
#include <qapplication.h>

#include <qevent.h>
#include <qmessagebox.h>

#ifdef QT_INTERFACE
#include "ui_viewerInterface.Qt4.h"
class ViewerInterface : public QDialog, public Ui::Dialog
{
public:
  ViewerInterface()
  {
    setupUi(this);
  }
};
#endif

/* convenient macros */

#define GETALLDS(M) M->model()->nonSmoothDynamicalSystem()->topology()->dSG(0)
#define GETNDS(M) GETALLDS(M)->size()

struct ForNdof : public Question<unsigned int>
{
  using SiconosVisitor::visit;

  ANSWER_V(Disk, 3);
  ANSWER_V(Circle, 3);
  ANSWER_V(SphereLDS, 6);
  ANSWER_V(SphereNEDS, 6);
  IFBULLET(ANSWER_V(BulletDS, 6));
};

struct ForFExt : public Question<SP::SiconosVector>
{
  using SiconosVisitor::visit;

  ANSWER(Disk, fExt());
  ANSWER(Circle, fExt());
  ANSWER(SphereLDS, fExt());
  ANSWER(SphereNEDS, fExt());
  IFBULLET(ANSWER(BulletDS, fExt()));
};

struct ForPosition : public Question<SP::SiconosVector>
{
  using SiconosVisitor::visit;

  ANSWER(Disk, q());
  ANSWER(Circle, q());
  ANSWER(SphereLDS, q());
  ANSWER(SphereNEDS, q());
  IFBULLET(ANSWER(BulletDS, q()));
};

struct ForRadius : public Question<double>
{
  using SiconosVisitor::visit;

  ANSWER(Disk, getRadius());
  ANSWER(Circle, getRadius());
  ANSWER(SphereLDS, getRadius());
  ANSWER(SphereNEDS, getRadius());
  IFBULLET(ANSWER_V(BulletDS, 0.)); // fix
};

struct ForMassValue : public Question<double>
{
  using SiconosVisitor::visit;

  ANSWER(Disk, mass()->getValue(0, 0));
  ANSWER(Circle, mass()->getValue(0, 0));
  ANSWER(SphereLDS, mass()->getValue(0, 0));
  ANSWER(SphereNEDS, massValue());
  IFBULLET(ANSWER(BulletDS, massValue()));
};

struct ForJachq : public Question<SP::SiconosMatrix>
{
  using SiconosVisitor::visit;

  ANSWER(LagrangianR, jachq());
  ANSWER(NewtonEulerR, jachq());
  ANSWER(DiskDiskR, jachq());
  ANSWER(DiskPlanR, jachq());
  ANSWER(DiskMovingPlanR, jachq());
  ANSWER(SphereLDSPlanR, jachq());
  ANSWER(SphereNEDSPlanR, jachq());
  ANSWER(SphereLDSSphereLDSR, jachq());
  ANSWER(SphereNEDSSphereNEDSR, jachq());
  IFBULLET(ANSWER(BulletR, jachq()));
};

struct ForContactForce : public Question<SP::SiconosVector>
{
  using SiconosVisitor::visit;

  IFBULLET(ANSWER(BulletR, contactForce()));
};

#define GETX(C) ask<ForPosition>(*C)->getValue(0)
#define GETY(C) ask<ForPosition>(*C)->getValue(1)
#define GETZ(C) ask<ForPosition>(*C)->getValue(2)
#define GETA1(C) ask<ForPosition>(*C)->getValue(3)
#define GETA2(C) ask<ForPosition>(*C)->getValue(4)
#define GETA3(C) ask<ForPosition>(*C)->getValue(5)
#define GETA4(C) ask<ForPosition>(*C)->getValue(6)
#define GETXFE(C) ask<ForFExt>(*C)->getValue(0)
#define GETYFE(C) ask<ForFExt>(*C)->getValue(1)
#define GETZFE(C) ask<ForFExt>(*C)->getValue(2)

#define GETVX(C) ask<ForVelocity>(*C)->getValue(0)
#define GETVY(C) ask<ForVelocity>(*C)->getValue(1)
#define GETVZ(C) ask<ForVelocity>(*C)->getValue(2)

#define GETVA1(C) ask<ForVelocity>(*C)->getValue(3)
#define GETVA2(C) ask<ForVelocity>(*C)->getValue(4)
#define GETVA3(C) ask<ForVelocity>(*C)->getValue(5)

#define GETRADIUS(C) ask<ForRadius>(*C)

enum SHAPE
{
  DISK,
  CIRCLE,
  SPHERE,
  IFBULLET(BULLET)
};

struct ForShape : public Question<SHAPE>
{
  using SiconosVisitor::visit;

  ANSWER_V(Disk, DISK);
  ANSWER_V(Circle, CIRCLE);
  ANSWER_V(SphereLDS, SPHERE);
  ANSWER_V(SphereNEDS, SPHERE);
  IFBULLET(ANSWER_V(BulletDS, BULLET));
};

/* dynamical system / figure association */
class QGLShape
{

public:

  /* construction from a LagrangianDS */
  QGLShape(SHAPE f, SP::DynamicalSystem D, const qglviewer::Frame* ref)
  {
    assert(D);

    figure_ = f;
    DS_ = D;
    frame_.reset(new qglviewer::ManipulatedFrame());
    frame_->setReferenceFrame(ref);
    savedFExt_ = ask<ForFExt>(*DS());
    selected_ = false;
    saved_ = true;
    type_ = Type::value(*DS());

  };

  ~QGLShape() {};

  /* pointer to DS */
  SP::DynamicalSystem DS() const
  {
    return DS_;
  };

  /* selection with mouse */
  bool selected()
  {
    return selected_ ;
  };
  void setSelection(bool v)
  {
    selected_ = v ;
  };

  /* identifiant */
  int getD()
  {
    return id_ ;
  };
  void setID(int i)
  {
    id_ = i;
  };

  /* External force set from mouse and restore */
  void saveFExt()
  {
    savedFExt_ = ask<ForFExt>(*DS());
  };
  void restoreFExt()
  {
    switch (Type::value(*DS()))
    {
    case Type::NewtonEulerDS :
    {
      std11::static_pointer_cast<NewtonEulerDS>(DS())
      ->setFExtPtr(std11::static_pointer_cast<SiconosVector>(savedFExt_));
      break;
    }
    case Type::LagrangianDS :
    {
      std11::static_pointer_cast<LagrangianDS>(DS())
      ->setFExtPtr(std11::static_pointer_cast<SiconosVector>(savedFExt_));
      break;
    };
    default:
    {};
    }
  };

  /* DS frame */
  qglviewer::ManipulatedFrame * frame()
  {
    return frame_.get();
  };

  SHAPE shape() const
  {
    return figure_;
  }

  Type::Siconos type() const
  {
    return type_;
  }

protected:
  int id_;
  bool selected_;
  bool saved_;
  SP::SiconosVector savedFExt_;
  boost::shared_ptr<qglviewer::ManipulatedFrame> frame_;

  SHAPE figure_;
  SP::DynamicalSystem DS_;
  int positionSize_;

  Type::Siconos type_;

};

TYPEDEF_SPTR(QGLShape);

/* QGLViewer main object */
class BodiesViewer : public QGLViewer
{

public:

#ifdef QT_INTERFACE
  Viewer(QWidget *parent);
#endif

public:
  virtual void draw() = 0;
  void drawWithNames();
  void initUCircle();
  void drawUCircle();
  void drawUDisk();
  void drawUCircleTicks(float a);
  void drawUDiskTicks(float a) ;
  void drawUTriangle(float depth = 0.) ;
  void drawVec(float x1, float y1, float x2, float y2) ;
  void drawRec(float x1, float y1, float x2, float y2, float w, float z = 0.);
  void drawArrow(float x1, float y1, float x2, float y2, float w);
  void drawPar(float x1, float y1, float z1, float x2, float y2, float z2, float w);
  void drawArrow(float x1, float y1, float z1, float x2, float y2, float z2, float w);
  void drawCircleTicks(float x, float y, float a, float r);
  void drawDiskTicks(float x, float y, float a, float r, float *c);
  void drawSimpleCircle(float x, float y, float r);
  void drawCircle(float x, float y, float a, float r, float *c);
  void drawDisk(float x, float y, float a, float r, float *c);
  void rotate(const float R[12]);
  void drawSphere(float x, float y, float z, float theta, float phi, float psi, float r, float *c);
  void drawSphere(float x, float y, float z, float a, float b, float c, float d, float r, float *color);
  void drawPolyg(unsigned int n, double* coor, float *c);

  virtual void drawQGLShape(const QGLShape&);
  virtual void drawSelectedQGLShape(const QGLShape&);
  void insertQGLShape(SHAPE, SP::DynamicalSystem);

protected :

  void postSelection(const QPoint& point);
  virtual void init();
  virtual void animate();
  virtual void mousePressEvent(QMouseEvent *);
  virtual void mouseMoveEvent(QMouseEvent *);
  virtual void mouseReleaseEvent(QMouseEvent *);
  virtual void keyPressEvent(QKeyEvent *);

  virtual QString helpString() const;
  boost::shared_ptr<qglviewer::WorldConstraint> constraint_;

  SP::SiconosBodies Siconos_;

  qglviewer::Frame* referenceFrame_;

  std::vector<SP::QGLShape>  shapes_;

  void print(float x, float y, const char *s, int size);

  int NDS_;

  qglviewer::Vec selectedPoint_;

  int lastSelected_;

  bool myMouseBehavior_;

  bool stepSimulation_;
  bool stepNow_;

  long timeSiconos_;

  long timeGlob_;

  float _transparency;

};

#endif
<|endoftext|>