ompl/extensions/morse/src/MorseProjection.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: Caleb Voss */
00036 
00037 #include "ompl/extensions/morse/MorseProjection.h"
00038 #include "ompl/base/spaces/RealVectorStateSpace.h"
00039 #include "ompl/util/Exception.h"
00040 
00041 ompl::base::MorseProjection::MorseProjection(const StateSpacePtr &space)
00042 : ProjectionEvaluator(space), space_(dynamic_cast<MorseStateSpace*>(space.get()))
00043 {
00044     if (!space_)
00045         throw Exception("MORSE State Space needed for Morse Projection");
00046 }
00047 
00048 void ompl::base::MorseProjection::setup()
00049 {
00050     ProjectionEvaluator::setup();
00051 }
00052 
00053 unsigned int ompl::base::MorseProjection::getDimension() const
00054 {
00055     // default projection uses 2 coordinates for each rigid body
00056     return 2*space_->getEnvironment()->rigidBodies_;
00057 }
00058 
00059 void ompl::base::MorseProjection::defaultCellSizes()
00060 {
00061     cellSizes_.resize(getDimension());
00062     for (unsigned int i = 0; i < getDimension(); i++)
00063     {
00064         cellSizes_[i] = 1.0;
00065     }
00066 }
00067 
00068 void ompl::base::MorseProjection::project(const State *state, EuclideanProjection &projection) const
00069 {
00070     // this projection uses the x and y coordinates of every rigid body
00071     const MorseStateSpace::StateType *mstate = state->as<MorseStateSpace::StateType>();
00072     projection.resize(getDimension());
00073     for (unsigned int i = 0; i < space_->getEnvironment()->rigidBodies_; i++)
00074     {
00075         // for each rigid body, grab the x and y coords
00076         const double *values = mstate->as<RealVectorStateSpace::StateType>(4*i)->values;
00077         projection[2*i] = values[0];
00078         projection[2*i+1] = values[1];
00079     }
00080 }
00081 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines