All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator
SO3StateSpace.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/SO3StateSpace.h"
00038 #include <algorithm>
00039 #include <limits>
00040 #include <cmath>
00041 #include "ompl/tools/config/MagicConstants.h"
00042 #include <boost/math/constants/constants.hpp>
00043 
00044 static const double MAX_QUATERNION_NORM_ERROR = 1e-9;
00045 
00046 void ompl::base::SO3StateSpace::StateType::setAxisAngle(double ax, double ay, double az, double angle)
00047 {
00048     double norm = sqrt(ax * ax + ay * ay + az * az);
00049     if (norm < MAX_QUATERNION_NORM_ERROR)
00050         setIdentity();
00051     else
00052     {
00053         double s = sin(angle / 2.0);
00054         x = s * ax / norm;
00055         y = s * ay / norm;
00056         z = s * az / norm;
00057         w = cos(angle / 2.0);
00058     }
00059 }
00060 
00061 void ompl::base::SO3StateSpace::StateType::setIdentity(void)
00062 {
00063     x = y = z = 0.0;
00064     w = 1.0;
00065 }
00066 
00067 void ompl::base::SO3StateSampler::sampleUniform(State *state)
00068 {
00069     rng_.quaternion(&state->as<SO3StateSpace::StateType>()->x);
00070 }
00071 
00072 void ompl::base::SO3StateSampler::sampleUniformNear(State *state, const State * /* near */, const double /* distance */)
00073 {
00075     sampleUniform(state);
00076 }
00077 
00078 void ompl::base::SO3StateSampler::sampleGaussian(State *state, const State * /* mean */, const double /* stdDev */)
00079 {
00081     sampleUniform(state);
00082 }
00083 
00084 
00085 unsigned int ompl::base::SO3StateSpace::getDimension(void) const
00086 {
00087     return 3;
00088 }
00089 
00090 double ompl::base::SO3StateSpace::getMaximumExtent(void) const
00091 {
00092     return boost::math::constants::pi<double>();
00093 }
00094 
00095 double ompl::base::SO3StateSpace::norm(const StateType *state) const
00096 {
00097     double nrmSqr = state->x * state->x + state->y * state->y + state->z * state->z + state->w * state->w;
00098     return (fabs(nrmSqr - 1.0) > std::numeric_limits<double>::epsilon()) ? sqrt(nrmSqr) : 1.0;
00099 }
00100 
00101 void ompl::base::SO3StateSpace::enforceBounds(State *state) const
00102 {
00103     StateType *qstate = static_cast<StateType*>(state);
00104     double nrm = norm(qstate);
00105     if (fabs(nrm - 1.0) > MAX_QUATERNION_NORM_ERROR)
00106     {
00107         qstate->x /= nrm;
00108         qstate->y /= nrm;
00109         qstate->z /= nrm;
00110         qstate->w /= nrm;
00111     }
00112     else
00113         qstate->setIdentity();
00114 }
00115 
00116 bool ompl::base::SO3StateSpace::satisfiesBounds(const State *state) const
00117 {
00118     return fabs(norm(static_cast<const StateType*>(state)) - 1.0) < MAX_QUATERNION_NORM_ERROR;
00119 }
00120 
00121 void ompl::base::SO3StateSpace::copyState(State *destination, const State *source) const
00122 {
00123     const StateType *qsource = static_cast<const StateType*>(source);
00124     StateType *qdestination = static_cast<StateType*>(destination);
00125     qdestination->x = qsource->x;
00126     qdestination->y = qsource->y;
00127     qdestination->z = qsource->z;
00128     qdestination->w = qsource->w;
00129 }
00130 
00131 
00132 /*
00133 Based on code from :
00134 
00135 Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans  http://continuousphysics.com/Bullet/
00136 */
00137 double ompl::base::SO3StateSpace::distance(const State *state1, const State *state2) const
00138 {
00139     const StateType *qs1 = static_cast<const StateType*>(state1);
00140     const StateType *qs2 = static_cast<const StateType*>(state2);
00141     double dq = fabs(qs1->x * qs2->x + qs1->y * qs2->y + qs1->z * qs2->z + qs1->w * qs2->w);
00142     if (dq > 1.0 - MAX_QUATERNION_NORM_ERROR)
00143         return 0.0;
00144     else
00145         return acos(dq) * 2.0;
00146 }
00147 
00148 bool ompl::base::SO3StateSpace::equalStates(const State *state1, const State *state2) const
00149 {
00150     return distance(state1, state2) < std::numeric_limits<double>::epsilon() * 2.0;
00151 }
00152 
00153 /*
00154 Based on code from :
00155 
00156 Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans  http://continuousphysics.com/Bullet/
00157 */
00158 void ompl::base::SO3StateSpace::interpolate(const State *from, const State *to, const double t, State *state) const
00159 {
00160     assert(norm(static_cast<const StateType*>(from)) - 1.0 < MAX_QUATERNION_NORM_ERROR);
00161     assert(norm(static_cast<const StateType*>(to)) - 1.0 < MAX_QUATERNION_NORM_ERROR);
00162 
00163     double theta = distance(from, to) / 2.0;
00164     if (theta > std::numeric_limits<double>::epsilon())
00165     {
00166         double d = 1.0 / sin(theta);
00167         double s0 = sin((1.0 - t) * theta);
00168         double s1 = sin(t * theta);
00169 
00170         const StateType *qs1 = static_cast<const StateType*>(from);
00171         const StateType *qs2 = static_cast<const StateType*>(to);
00172         StateType       *qr  = static_cast<StateType*>(state);
00173         double dq = qs1->x * qs2->x + qs1->y * qs2->y + qs1->z * qs2->z + qs1->w * qs2->w;
00174         if (dq < 0)  // Take care of long angle case see http://en.wikipedia.org/wiki/Slerp
00175             s1 = -s1;
00176 
00177         qr->x = (qs1->x * s0 + qs2->x * s1) * d;
00178         qr->y = (qs1->y * s0 + qs2->y * s1) * d;
00179         qr->z = (qs1->z * s0 + qs2->z * s1) * d;
00180         qr->w = (qs1->w * s0 + qs2->w * s1) * d;
00181     }
00182     else
00183     {
00184         if (state != from)
00185             copyState(state, from);
00186     }
00187 }
00188 
00189 ompl::base::StateSamplerPtr ompl::base::SO3StateSpace::allocStateSampler(void) const
00190 {
00191     return StateSamplerPtr(new SO3StateSampler(this));
00192 }
00193 
00194 ompl::base::State* ompl::base::SO3StateSpace::allocState(void) const
00195 {
00196     return new StateType();
00197 }
00198 
00199 void ompl::base::SO3StateSpace::freeState(State *state) const
00200 {
00201     delete static_cast<StateType*>(state);
00202 }
00203 
00204 void ompl::base::SO3StateSpace::registerProjections(void)
00205 {
00206     class SO3DefaultProjection : public ProjectionEvaluator
00207     {
00208     public:
00209 
00210         SO3DefaultProjection(const StateSpace *space) : ProjectionEvaluator(space)
00211         {
00212         }
00213 
00214         virtual unsigned int getDimension(void) const
00215         {
00216             return 3;
00217         }
00218 
00219         virtual void defaultCellSizes(void)
00220         {
00221             cellSizes_.resize(3);
00222             cellSizes_[0] = boost::math::constants::pi<double>() / magic::PROJECTION_DIMENSION_SPLITS;
00223             cellSizes_[1] = boost::math::constants::pi<double>() / magic::PROJECTION_DIMENSION_SPLITS;
00224             cellSizes_[2] = boost::math::constants::pi<double>() / magic::PROJECTION_DIMENSION_SPLITS;
00225         }
00226 
00227         virtual void project(const State *state, EuclideanProjection &projection) const
00228         {
00229             projection(0) = state->as<SO3StateSpace::StateType>()->x;
00230             projection(1) = state->as<SO3StateSpace::StateType>()->y;
00231             projection(2) = state->as<SO3StateSpace::StateType>()->z;
00232         }
00233     };
00234 
00235     registerDefaultProjection(ProjectionEvaluatorPtr(dynamic_cast<ProjectionEvaluator*>(new SO3DefaultProjection(this))));
00236 }
00237 
00238 double* ompl::base::SO3StateSpace::getValueAddressAtIndex(State *state, const unsigned int index) const
00239 {
00240     return index < 4 ? &(state->as<StateType>()->x) + index : NULL;
00241 }
00242 
00243 void ompl::base::SO3StateSpace::printState(const State *state, std::ostream &out) const
00244 {
00245     out << "SO3State [";
00246     if (state)
00247     {
00248         const StateType *qstate = static_cast<const StateType*>(state);
00249         out << qstate->x << " " << qstate->y << " " << qstate->z << " " << qstate->w;
00250     }
00251     else
00252         out << "NULL";
00253     out << ']' << std::endl;
00254 }
00255 
00256 void ompl::base::SO3StateSpace::printSettings(std::ostream &out) const
00257 {
00258     out << "SO(3) state space '" << getName() << "' (represented using quaternions)" << std::endl;
00259 }