37 #include "ompl/base/spaces/SO3StateSpace.h"
41 #include "ompl/tools/config/MagicConstants.h"
42 #include <boost/math/constants/constants.hpp>
44 static const double MAX_QUATERNION_NORM_ERROR = 1e-9;
53 double norm = sqrt(ax * ax + ay * ay + az * az);
54 if (norm < MAX_QUATERNION_NORM_ERROR)
58 double s = sin(angle / 2.0);
62 q.w = cos(angle / 2.0);
69 q.x = q0.w*q1.x + q0.x*q1.w + q0.y*q1.z - q0.z*q1.y;
70 q.y = q0.w*q1.y + q0.y*q1.w + q0.z*q1.x - q0.x*q1.z;
71 q.z = q0.w*q1.z + q0.z*q1.w + q0.x*q1.y - q0.y*q1.x;
72 q.w = q0.w*q1.w - q0.x*q1.x - q0.y*q1.y - q0.z*q1.z;
80 computeAxisAngle(*
this, ax, ay, az, angle);
96 if (distance >= .25 * boost::math::constants::pi<double>())
101 double d = rng_.uniform01();
105 computeAxisAngle(q, rng_.gaussian01(), rng_.gaussian01(), rng_.gaussian01(), 2.*pow(d,1./3.)*
distance);
106 quaternionProduct(*qs, *qnear, q);
118 sampleUniform(state);
121 double x = rng_.gaussian(0, stdDev), y = rng_.gaussian(0, stdDev), z = rng_.gaussian(0, stdDev),
122 theta = std::sqrt(x*x + y*y + z*z);
123 if (theta < std::numeric_limits<double>::epsilon())
124 space_->copyState(state, mean);
130 double s = sin(theta / 2) / theta;
131 q.
w = cos(theta / 2);
135 quaternionProduct(*qs, *qmu, q);
146 return .5 * boost::math::constants::pi<double>();
151 double nrmSqr = state->
x * state->
x + state->
y * state->
y + state->
z * state->
z + state->
w * state->
w;
152 return (fabs(nrmSqr - 1.0) > std::numeric_limits<double>::epsilon()) ? sqrt(nrmSqr) : 1.0;
158 double nrm =
norm(qstate);
159 if (fabs(nrm) < MAX_QUATERNION_NORM_ERROR)
161 else if (fabs(nrm - 1.0) > MAX_QUATERNION_NORM_ERROR)
172 return fabs(
norm(static_cast<const StateType*>(state)) - 1.0) < MAX_QUATERNION_NORM_ERROR;
179 qdestination->
x = qsource->
x;
180 qdestination->
y = qsource->
y;
181 qdestination->
z = qsource->
z;
182 qdestination->
w = qsource->
w;
187 return sizeof(double) * 4;
192 memcpy(serialization, &state->
as<
StateType>()->
x,
sizeof(
double) * 4);
197 memcpy(&state->
as<
StateType>()->
x, serialization,
sizeof(
double) * 4);
211 static inline double arcLength(
const State *state1,
const State *state2)
215 double dq = fabs(qs1->
x * qs2->
x + qs1->
y * qs2->
y + qs1->
z * qs2->
z + qs1->
w * qs2->
w);
216 if (dq > 1.0 - MAX_QUATERNION_NORM_ERROR)
227 return arcLength(state1, state2);
232 return arcLength(state1, state2) < std::numeric_limits<double>::epsilon();
242 assert(fabs(
norm(static_cast<const StateType*>(from)) - 1.0) < MAX_QUATERNION_NORM_ERROR);
243 assert(fabs(
norm(static_cast<const StateType*>(to)) - 1.0) < MAX_QUATERNION_NORM_ERROR);
245 double theta = arcLength(from, to);
246 if (theta > std::numeric_limits<double>::epsilon())
248 double d = 1.0 / sin(theta);
249 double s0 = sin((1.0 - t) * theta);
250 double s1 = sin(t * theta);
255 double dq = qs1->
x * qs2->
x + qs1->
y * qs2->
y + qs1->
z * qs2->
z + qs1->
w * qs2->
w;
259 qr->
x = (qs1->
x * s0 + qs2->
x * s1) * d;
260 qr->
y = (qs1->
y * s0 + qs2->
y * s1) * d;
261 qr->
z = (qs1->
z * s0 + qs2->
z * s1) * d;
262 qr->
w = (qs1->
w * s0 + qs2->
w * s1) * d;
301 virtual void defaultCellSizes(
void)
303 cellSizes_.resize(3);
322 return index < 4 ? &(state->
as<
StateType>()->x) + index : NULL;
331 out << qstate->
x <<
" " << qstate->
y <<
" " << qstate->
z <<
" " << qstate->
w;
335 out <<
']' << std::endl;
340 out <<
"SO(3) state space '" <<
getName() <<
"' (represented using quaternions)" << std::endl;