All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator
RealVectorStateProjections.cpp
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2010, Rice University
00005 *  All rights reserved.
00006 *
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 *
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of the Rice University nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 *
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 *********************************************************************/
00034 
00035 /* Author: Ioan Sucan */
00036 
00037 #include "ompl/base/spaces/RealVectorStateProjections.h"
00038 #include "ompl/util/Exception.h"
00039 #include "ompl/tools/config/MagicConstants.h"
00040 #include <cstring>
00041 
00043 namespace ompl
00044 {
00045     namespace base
00046     {
00047         static inline void checkSpaceType(const StateSpace *m)
00048         {
00049             if (!dynamic_cast<const RealVectorStateSpace*>(m))
00050                 throw Exception("Expected real vector state space for projection");
00051         }
00052     }
00053 }
00055 
00056 ompl::base::RealVectorLinearProjectionEvaluator::RealVectorLinearProjectionEvaluator(const StateSpace *space, const std::vector<double> &cellSizes,
00057                                                                                      const ProjectionMatrix::Matrix &projection) :
00058     ProjectionEvaluator(space)
00059 {
00060     checkSpaceType(space_);
00061     projection_.mat = projection;
00062     setCellSizes(cellSizes);
00063 }
00064 
00065 ompl::base::RealVectorLinearProjectionEvaluator::RealVectorLinearProjectionEvaluator(const StateSpacePtr &space, const std::vector<double> &cellSizes,
00066                                                                                      const ProjectionMatrix::Matrix &projection) :
00067     ProjectionEvaluator(space)
00068 {
00069     checkSpaceType(space_);
00070     projection_.mat = projection;
00071     setCellSizes(cellSizes);
00072 }
00073 
00074 ompl::base::RealVectorLinearProjectionEvaluator::RealVectorLinearProjectionEvaluator(const StateSpace *space,
00075                                                                                      const ProjectionMatrix::Matrix &projection) :
00076     ProjectionEvaluator(space)
00077 {
00078     checkSpaceType(space_);
00079     projection_.mat = projection;
00080 }
00081 
00082 ompl::base::RealVectorLinearProjectionEvaluator::RealVectorLinearProjectionEvaluator(const StateSpacePtr &space,
00083                                                                                      const ProjectionMatrix::Matrix &projection) :
00084     ProjectionEvaluator(space)
00085 {
00086     checkSpaceType(space_);
00087     projection_.mat = projection;
00088 }
00089 
00090 ompl::base::RealVectorOrthogonalProjectionEvaluator::RealVectorOrthogonalProjectionEvaluator(const StateSpace *space, const std::vector<double> &cellSizes,
00091                                                                                              const std::vector<unsigned int> &components) :
00092     ProjectionEvaluator(space), components_(components)
00093 {
00094     checkSpaceType(space_);
00095     setCellSizes(cellSizes);
00096 }
00097 
00098 ompl::base::RealVectorOrthogonalProjectionEvaluator::RealVectorOrthogonalProjectionEvaluator(const StateSpacePtr &space, const std::vector<double> &cellSizes,
00099                                                                                              const std::vector<unsigned int> &components) :
00100     ProjectionEvaluator(space), components_(components)
00101 {
00102     checkSpaceType(space_);
00103     setCellSizes(cellSizes);
00104 }
00105 
00106 ompl::base::RealVectorOrthogonalProjectionEvaluator::RealVectorOrthogonalProjectionEvaluator(const StateSpace *space, const std::vector<unsigned int> &components) :
00107     ProjectionEvaluator(space), components_(components)
00108 {
00109     checkSpaceType(space_);
00110 }
00111 
00112 ompl::base::RealVectorOrthogonalProjectionEvaluator::RealVectorOrthogonalProjectionEvaluator(const StateSpacePtr &space, const std::vector<unsigned int> &components) :
00113     ProjectionEvaluator(space), components_(components)
00114 {
00115     checkSpaceType(space_);
00116 }
00117 
00118 void ompl::base::RealVectorOrthogonalProjectionEvaluator::defaultCellSizes(void)
00119 {
00120     const RealVectorBounds &bounds = space_->as<RealVectorStateSpace>()->getBounds();
00121     cellSizes_.resize(components_.size());
00122     for (unsigned int i = 0 ; i < cellSizes_.size() ; ++i)
00123         cellSizes_[i] = (bounds.high[components_[i]] - bounds.low[components_[i]]) / magic::PROJECTION_DIMENSION_SPLITS;
00124 }
00125 
00126 unsigned int ompl::base::RealVectorLinearProjectionEvaluator::getDimension(void) const
00127 {
00128     return projection_.mat.size1();
00129 }
00130 
00131 void ompl::base::RealVectorLinearProjectionEvaluator::project(const State *state, EuclideanProjection &projection) const
00132 {
00133     projection_.project(state->as<RealVectorStateSpace::StateType>()->values, projection);
00134 }
00135 
00136 unsigned int ompl::base::RealVectorOrthogonalProjectionEvaluator::getDimension(void) const
00137 {
00138     return components_.size();
00139 }
00140 
00141 void ompl::base::RealVectorOrthogonalProjectionEvaluator::project(const State *state, EuclideanProjection &projection) const
00142 {
00143     for (unsigned int i = 0 ; i < components_.size() ; ++i)
00144         projection(i) = state->as<RealVectorStateSpace::StateType>()->values[components_[i]];
00145 }
00146 
00147 ompl::base::RealVectorIdentityProjectionEvaluator::RealVectorIdentityProjectionEvaluator(const StateSpace *space, const std::vector<double> &cellSizes) :
00148     ProjectionEvaluator(space)
00149 {
00150     checkSpaceType(space_);
00151     setCellSizes(cellSizes);
00152 }
00153 
00154 ompl::base::RealVectorIdentityProjectionEvaluator::RealVectorIdentityProjectionEvaluator(const StateSpace *space) :
00155     ProjectionEvaluator(space)
00156 {
00157     checkSpaceType(space_);
00158 }
00159 
00160 ompl::base::RealVectorIdentityProjectionEvaluator::RealVectorIdentityProjectionEvaluator(const StateSpacePtr &space, const std::vector<double> &cellSizes) :
00161     ProjectionEvaluator(space)
00162 {
00163     checkSpaceType(space_);
00164     setCellSizes(cellSizes);
00165 }
00166 
00167 ompl::base::RealVectorIdentityProjectionEvaluator::RealVectorIdentityProjectionEvaluator(const StateSpacePtr &space) :
00168     ProjectionEvaluator(space)
00169 {
00170     checkSpaceType(space_);
00171 }
00172 
00173 void ompl::base::RealVectorIdentityProjectionEvaluator::defaultCellSizes(void)
00174 {
00175     const RealVectorBounds &bounds = space_->as<RealVectorStateSpace>()->getBounds();
00176     cellSizes_.resize(getDimension());
00177     for (unsigned int i = 0 ; i < cellSizes_.size() ; ++i)
00178         cellSizes_[i] = (bounds.high[i] - bounds.low[i]) / magic::PROJECTION_DIMENSION_SPLITS;
00179 }
00180 
00181 void ompl::base::RealVectorIdentityProjectionEvaluator::setup(void)
00182 {
00183     copySize_ = getDimension() * sizeof(double);
00184     ProjectionEvaluator::setup();
00185 }
00186 
00187 unsigned int ompl::base::RealVectorIdentityProjectionEvaluator::getDimension(void) const
00188 {
00189     return space_->getDimension();
00190 }
00191 
00192 void ompl::base::RealVectorIdentityProjectionEvaluator::project(const State *state, EuclideanProjection &projection) const
00193 {
00194     memcpy(&projection(0), state->as<RealVectorStateSpace::StateType>()->values, copySize_);
00195 }