ompl/base/spaces/src/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     copyBounds();
00097 }
00098 
00099 ompl::base::RealVectorOrthogonalProjectionEvaluator::RealVectorOrthogonalProjectionEvaluator(const StateSpacePtr &space, const std::vector<double> &cellSizes,
00100                                                                                              const std::vector<unsigned int> &components) :
00101     ProjectionEvaluator(space), components_(components)
00102 {
00103     checkSpaceType(space_);
00104     setCellSizes(cellSizes);
00105     copyBounds();
00106 }
00107 
00108 ompl::base::RealVectorOrthogonalProjectionEvaluator::RealVectorOrthogonalProjectionEvaluator(const StateSpace *space, const std::vector<unsigned int> &components) :
00109     ProjectionEvaluator(space), components_(components)
00110 {
00111     checkSpaceType(space_);
00112 }
00113 
00114 ompl::base::RealVectorOrthogonalProjectionEvaluator::RealVectorOrthogonalProjectionEvaluator(const StateSpacePtr &space, const std::vector<unsigned int> &components) :
00115     ProjectionEvaluator(space), components_(components)
00116 {
00117     checkSpaceType(space_);
00118 }
00119 
00120 void ompl::base::RealVectorOrthogonalProjectionEvaluator::copyBounds()
00121 {
00122     bounds_.resize(components_.size());
00123     const RealVectorBounds &bounds = space_->as<RealVectorStateSpace>()->getBounds();
00124     for (unsigned int i = 0 ; i < components_.size() ; ++i)
00125     {
00126         bounds_.low[i] = bounds.low[components_[i]];
00127         bounds_.high[i] = bounds.high[components_[i]];
00128     }
00129 }
00130 
00131 void ompl::base::RealVectorOrthogonalProjectionEvaluator::defaultCellSizes()
00132 {
00133     const RealVectorBounds &bounds = space_->as<RealVectorStateSpace>()->getBounds();
00134     bounds_.resize(components_.size());
00135     cellSizes_.resize(components_.size());
00136     for (unsigned int i = 0 ; i < cellSizes_.size() ; ++i)
00137     {
00138         bounds_.low[i] = bounds.low[components_[i]];
00139         bounds_.high[i] = bounds.high[components_[i]];
00140         cellSizes_[i] = (bounds_.high[i] - bounds_.low[i]) / magic::PROJECTION_DIMENSION_SPLITS;
00141     }
00142 }
00143 
00144 unsigned int ompl::base::RealVectorLinearProjectionEvaluator::getDimension() const
00145 {
00146     return projection_.mat.size1();
00147 }
00148 
00149 void ompl::base::RealVectorLinearProjectionEvaluator::project(const State *state, EuclideanProjection &projection) const
00150 {
00151     projection_.project(state->as<RealVectorStateSpace::StateType>()->values, projection);
00152 }
00153 
00154 unsigned int ompl::base::RealVectorOrthogonalProjectionEvaluator::getDimension() const
00155 {
00156     return components_.size();
00157 }
00158 
00159 void ompl::base::RealVectorOrthogonalProjectionEvaluator::project(const State *state, EuclideanProjection &projection) const
00160 {
00161     for (unsigned int i = 0 ; i < components_.size() ; ++i)
00162         projection(i) = state->as<RealVectorStateSpace::StateType>()->values[components_[i]];
00163 }
00164 
00165 ompl::base::RealVectorIdentityProjectionEvaluator::RealVectorIdentityProjectionEvaluator(const StateSpace *space, const std::vector<double> &cellSizes) :
00166     ProjectionEvaluator(space)
00167 {
00168     checkSpaceType(space_);
00169     setCellSizes(cellSizes);
00170     copyBounds();
00171 }
00172 
00173 ompl::base::RealVectorIdentityProjectionEvaluator::RealVectorIdentityProjectionEvaluator(const StateSpace *space) :
00174     ProjectionEvaluator(space)
00175 {
00176     checkSpaceType(space_);
00177 }
00178 
00179 ompl::base::RealVectorIdentityProjectionEvaluator::RealVectorIdentityProjectionEvaluator(const StateSpacePtr &space, const std::vector<double> &cellSizes) :
00180     ProjectionEvaluator(space)
00181 {
00182     checkSpaceType(space_);
00183     setCellSizes(cellSizes);
00184     copyBounds();
00185 }
00186 
00187 ompl::base::RealVectorIdentityProjectionEvaluator::RealVectorIdentityProjectionEvaluator(const StateSpacePtr &space) :
00188     ProjectionEvaluator(space)
00189 {
00190     checkSpaceType(space_);
00191 }
00192 
00193 void ompl::base::RealVectorIdentityProjectionEvaluator::copyBounds()
00194 {
00195     bounds_ = space_->as<RealVectorStateSpace>()->getBounds();
00196 }
00197 
00198 void ompl::base::RealVectorIdentityProjectionEvaluator::defaultCellSizes()
00199 {
00200     bounds_ = space_->as<RealVectorStateSpace>()->getBounds();
00201     cellSizes_.resize(getDimension());
00202     for (unsigned int i = 0 ; i < cellSizes_.size() ; ++i)
00203         cellSizes_[i] = (bounds_.high[i] - bounds_.low[i]) / magic::PROJECTION_DIMENSION_SPLITS;
00204 }
00205 
00206 void ompl::base::RealVectorIdentityProjectionEvaluator::setup()
00207 {
00208     copySize_ = getDimension() * sizeof(double);
00209     ProjectionEvaluator::setup();
00210 }
00211 
00212 unsigned int ompl::base::RealVectorIdentityProjectionEvaluator::getDimension() const
00213 {
00214     return space_->getDimension();
00215 }
00216 
00217 void ompl::base::RealVectorIdentityProjectionEvaluator::project(const State *state, EuclideanProjection &projection) const
00218 {
00219     memcpy(&projection(0), state->as<RealVectorStateSpace::StateType>()->values, copySize_);
00220 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines