demos/Koules/KoulesSimulator.cpp
00001 /********************************************************************* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2013, 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: Beck Chen, Mark Moll */ 00036 00037 #include <ompl/control/SpaceInformation.h> 00038 #include "KoulesStateSpace.h" 00039 #include "KoulesControlSpace.h" 00040 #include "KoulesSimulator.h" 00041 00042 namespace ob = ompl::base; 00043 namespace oc = ompl::control; 00044 00045 namespace 00046 { 00047 inline double normalizeAngle(double theta) 00048 { 00049 if (theta < -boost::math::constants::pi<double>()) 00050 return theta + 2. * boost::math::constants::pi<double>(); 00051 if (theta > boost::math::constants::pi<double>()) 00052 return theta - 2. * boost::math::constants::pi<double>(); 00053 return theta; 00054 } 00055 00056 inline void normalize2(double* v) 00057 { 00058 double nrm = std::sqrt(v[0] * v[0] + v[1] * v[1]); 00059 if (nrm > std::numeric_limits<double>::epsilon()) 00060 { 00061 v[0] /= nrm; 00062 v[1] /= nrm; 00063 } 00064 else 00065 { 00066 v[0] = 1.; 00067 v[1] = 0.; 00068 } 00069 } 00070 inline double dot2(double* v, double* w) 00071 { 00072 return v[0] * w[0] + v[1] * w[1]; 00073 } 00074 inline void vadd2(double* v, double s, double* w) 00075 { 00076 v[0] += s * w[0]; 00077 v[1] += s * w[1]; 00078 } 00079 inline unsigned int ind(unsigned int i) 00080 { 00081 return i ? 4 * i + 1 : 0; 00082 } 00083 inline void ode(double* y, double* dydx) 00084 { 00085 dydx[0] = y[2]; 00086 dydx[1] = y[3]; 00087 dydx[2] = (.5 * sideLength - y[0]) * lambda_c - y[2] * h; 00088 dydx[3] = (.5 * sideLength - y[1]) * lambda_c - y[3] * h; 00089 } 00090 inline void rungeKutta4(double* y, double h, double* yout) 00091 { 00092 double hh = h * .5, h6 = h / 6.; 00093 static double yt[4], dydx[4], dym[4], dyt[4]; 00094 ode(y, dydx); 00095 for (unsigned int i = 0; i < 4; ++i) 00096 yt[i] = y[i] + hh * dydx[i]; 00097 ode(yt, dyt); 00098 for (unsigned int i = 0; i < 4; ++i) 00099 yt[i] = y[i] + hh * dyt[i]; 00100 ode(yt, dym); 00101 for (unsigned int i = 0; i < 4; ++i) 00102 { 00103 yt[i] = y[i] + h * dym[i]; 00104 dym[i] += dyt[i]; 00105 } 00106 ode(yt, dyt); 00107 for (unsigned int i = 0; i < 4; ++i) 00108 yout[i] = y[i] + h6 * (dydx[i] + dyt[i] + 2. * dym[i]); 00109 } 00110 00111 const float eps = std::numeric_limits<float>::epsilon(); 00112 } 00113 00114 KoulesSimulator::KoulesSimulator(const ompl::control::SpaceInformation* si) : 00115 si_(si), 00116 numDimensions_(si->getStateSpace()->getDimension()), 00117 numKoules_((numDimensions_ - 5) / 4), 00118 qcur_(numDimensions_), 00119 qnext_(numDimensions_), 00120 dead_(numKoules_ + 1) 00121 { 00122 } 00123 00124 void KoulesSimulator::updateShip(const oc::Control* control, double t) 00125 { 00126 const double* cval = control->as<KoulesControlSpace::ControlType>()->values; 00127 double v[2] = { cval[0] - qcur_[2], cval[1] - qcur_[3] }; 00128 double deltaTheta = normalizeAngle(atan2(v[1], v[0]) - qcur_[4]); 00129 double accel = 0., omega = 0.; 00130 00131 if (std::abs(deltaTheta) < shipEps) 00132 { 00133 if (v[0] * v[0] + v[1] * v[1] > shipDelta * shipDelta) 00134 accel = shipAcceleration; 00135 } 00136 else 00137 omega = deltaTheta > 0. ? shipRotVel : -shipRotVel; 00138 00139 if (accel == 0.) 00140 { 00141 qnext_[0] = qcur_[0] + qcur_[2] * t; 00142 qnext_[1] = qcur_[1] + qcur_[3] * t; 00143 qnext_[2] = qcur_[2]; 00144 qnext_[3] = qcur_[3]; 00145 qcur_[4] = normalizeAngle(qcur_[4] + omega * t); 00146 } 00147 else // omega == 0. 00148 { 00149 double ax = accel * cos(qcur_[4]); 00150 double ay = accel * sin(qcur_[4]); 00151 double t2 = .5 * t * t; 00152 qnext_[0] = qcur_[0] + qcur_[2] * t + ax * t2; 00153 qnext_[1] = qcur_[1] + qcur_[3] * t + ay * t2; 00154 qnext_[2] = qcur_[2] + ax * t; 00155 qnext_[3] = qcur_[3] + ay * t; 00156 } 00157 } 00158 00159 void KoulesSimulator::initCollisionEvents() 00160 { 00161 double r[2], d, bad, ri, rij; 00162 unsigned int ii, jj; 00163 collisionEvents_ = CollisionEventQueue(); 00164 for (unsigned int i = 0; i < numKoules_; ++i) 00165 if (!dead_[i]) 00166 { 00167 ii = ind(i); 00168 ri = si_->getStateSpace()->as<KoulesStateSpace>()->getRadius(i); 00169 for (unsigned int j = i + 1; j <= numKoules_; ++j) 00170 if (!dead_[j]) 00171 { 00172 jj = ind(j); 00173 rij = ri + si_->getStateSpace()->as<KoulesStateSpace>()->getRadius(j); 00174 r[0] = qcur_[jj ] - qcur_[ii ]; 00175 r[1] = qcur_[jj + 1] - qcur_[ii + 1]; 00176 d = r[0] * r[0] + r[1] * r[1]; 00177 if (d < rij * rij) 00178 { 00179 d = std::sqrt(d); 00180 bad = rij - d; 00181 r[0] /= d; 00182 r[0] *= bad * (1. + eps) * .5; 00183 r[1] /= d; 00184 r[1] *= bad * (1. + eps) * .5; 00185 qcur_[ii ] -= r[0]; 00186 qcur_[ii + 1] -= r[1]; 00187 qcur_[jj ] += r[0]; 00188 qcur_[jj + 1] += r[1]; 00189 } 00190 } 00191 } 00192 for (unsigned int i = 0; i <= numKoules_; ++i) 00193 for (unsigned int j = i + 1; j <= numKoules_ + 2; ++j) 00194 computeCollisionEvent(i, j); 00195 } 00196 00197 double KoulesSimulator::wallCollideEvent(unsigned int i, int dim) 00198 { 00199 double r = si_->getStateSpace()->as<KoulesStateSpace>()->getRadius(i); 00200 unsigned int ii = ind(i); 00201 if (qcur_[ii + 2 + dim] > 0.) 00202 return std::max(0., (sideLength - r - qcur_[ii + dim]) / qcur_[ii + 2 + dim]); 00203 else if (qcur_[ii + 2 + dim] < 0.) 00204 return std::max(0., (r - qcur_[ii + dim]) / qcur_[ii + 2 + dim]); 00205 else 00206 return -1.; 00207 } 00208 00209 void KoulesSimulator::computeCollisionEvent(unsigned int i, unsigned int j) 00210 { 00211 if (dead_[i] || (j <= numKoules_ && dead_[j])) 00212 return; 00213 00214 double ri = si_->getStateSpace()->as<KoulesStateSpace>()->getRadius(i); 00215 double t; 00216 00217 if (j == numKoules_ + 1) 00218 t = wallCollideEvent(i, 0); 00219 else if (j == numKoules_ + 2) 00220 t = wallCollideEvent(i, 1); 00221 else 00222 { 00223 unsigned int ii = ind(i), jj = ind(j); 00224 double u[2] = { qcur_[ii + 2] - qcur_[jj + 2], qcur_[ii + 3] - qcur_[jj + 3] }; 00225 double v[2] = { qcur_[ii ] - qcur_[jj ], qcur_[ii + 1] - qcur_[jj + 1] }; 00226 double a = u[0] * u[0] + u[1] * u[1]; 00227 if (a == 0.) 00228 t = -1.; 00229 else 00230 { 00231 double r = ri + si_->getStateSpace()->as<KoulesStateSpace>()->getRadius(j); 00232 double b = 2 * u[0] * v[0] + 2 * u[1] * v[1]; 00233 double c = v[0] * v[0] + v[1] * v[1] - r * r; 00234 double disc = b * b - 4. * a * c; 00235 if (std::abs(disc) < std::numeric_limits<float>::epsilon()) 00236 t = -.5 * b / a; 00237 else if (disc < 0.) 00238 t = -1.; 00239 else 00240 { 00241 disc = std::sqrt(disc); 00242 t = (-b - disc) / (2. * a); 00243 if (t < 0.) 00244 t = (-b + disc) / (2. * a); 00245 } 00246 } 00247 } 00248 t += time_; 00249 if (t >= time_ && t <= endTime_) 00250 collisionEvents_.push(boost::make_tuple(t, i, j)); 00251 } 00252 00253 void KoulesSimulator::elasticCollision(unsigned int i, unsigned int j) 00254 { 00255 double *a = &qcur_[ind(i)], *b = &qcur_[ind(j)]; 00256 double d[2] = { b[0] - a[0], b[1] - a[1] }; 00257 normalize2(d); 00258 double va = dot2(a + 2, d), vb = dot2(b + 2, d); 00259 if (va - vb <= 0.) 00260 return; 00261 double ma = si_->getStateSpace()->as<KoulesStateSpace>()->getMass(i); 00262 double mb = si_->getStateSpace()->as<KoulesStateSpace>()->getMass(j); 00263 double vap = (va * (ma - mb) + 2. * mb * vb) / (ma + mb); 00264 double vbp = (vb * (mb - ma) + 2. * ma * va) / (ma + mb); 00265 double amag = vap - va, bmag = vbp - vb; 00266 if (std::abs(amag) < eps) 00267 amag = amag < 0. ? -eps : eps; 00268 if (std::abs(bmag) < eps) 00269 bmag = bmag < 0. ? -eps : eps; 00270 #ifndef NDEBUG 00271 double px = ma * a[2] + mb * b[2], py = ma * a[3] + mb * b[3]; 00272 double k = ma * (a[2] * a[2] + a[3] * a[3]) + mb * (b[2] * b[2] + b[3] * b[3]); 00273 #endif 00274 vadd2(a + 2, amag, d); 00275 vadd2(b + 2, bmag, d); 00276 00277 #ifndef NDEBUG 00278 // preservation of momemtum 00279 assert(std::abs(ma * a[2] + mb * b[2] - px) < 1e-6); 00280 assert(std::abs(ma * a[3] + mb * b[3] - py) < 1e-6); 00281 // preservation of kinetic energy 00282 assert(std::abs(ma * (a[2] * a[2] + a[3] * a[3]) + mb * (b[2] * b[2] + b[3] * b[3]) - k) < 1e-6); 00283 #endif 00284 } 00285 00286 void KoulesSimulator::advance(double t) 00287 { 00288 double dt = t - time_; 00289 qcur_[0] += qcur_[2] * dt; 00290 qcur_[1] += qcur_[3] * dt; 00291 for (unsigned int i = 5; i < numDimensions_; i += 4) 00292 { 00293 qcur_[i ] += qcur_[i + 2] * dt; 00294 qcur_[i + 1] += qcur_[i + 3] * dt; 00295 } 00296 time_ = t; 00297 } 00298 00299 void KoulesSimulator::markAsDead(unsigned int i) 00300 { 00301 unsigned int ii = ind(i); 00302 qcur_[ii] = -2. * kouleRadius; 00303 qcur_[ii + 1] = qcur_[ii + 2] = qcur_[ii + 3] = 0.; 00304 } 00305 00306 void KoulesSimulator::step(const ob::State *start, const oc::Control* control, 00307 const double t, ob::State *result) 00308 { 00309 unsigned int ii; 00310 00311 memcpy(&qcur_[0], start->as<KoulesStateSpace::StateType>()->values, numDimensions_ * sizeof(double)); 00312 time_ = 0.; 00313 endTime_ = t; 00314 std::fill(dead_.begin(), dead_.end(), false); 00315 updateShip(control, t); 00316 for (unsigned int i = 0; i <= numKoules_; ++i) 00317 { 00318 ii = ind(i); 00319 dead_[i] = qcur_[ii] == -2. * kouleRadius; 00320 if (!dead_[i]) 00321 { 00322 if (i) 00323 rungeKutta4(&qcur_[ii], t, &qnext_[ii]); 00324 qcur_[ii + 2] = (qnext_[ii ] - qcur_[ii ]) / t; 00325 qcur_[ii + 3] = (qnext_[ii + 1] - qcur_[ii + 1]) / t; 00326 } 00327 } 00328 initCollisionEvents(); 00329 while (!collisionEvents_.empty()) 00330 { 00331 CollisionEvent event = collisionEvents_.top(); 00332 double ct = event.get<0>(); 00333 unsigned int i = event.get<1>(), j = event.get<2>(); 00334 00335 collisionEvents_.pop(); 00336 advance(ct); 00337 if (j <= numKoules_) 00338 elasticCollision(i, j); 00339 else 00340 { 00341 markAsDead(i); 00342 if (i == 0) 00343 { 00344 memcpy(result->as<KoulesStateSpace::StateType>()->values, &qcur_[0], numDimensions_ * sizeof(double)); 00345 return; 00346 } 00347 } 00348 00349 for (unsigned int k = 0; k <= numKoules_ + 2; ++k) 00350 { 00351 if (k < i) 00352 computeCollisionEvent(k, i); 00353 else if (k > i && k != j) 00354 computeCollisionEvent(i, k); 00355 if (k < j && k != i) 00356 computeCollisionEvent(k, j); 00357 else if (k > j) 00358 computeCollisionEvent(j, k); 00359 } 00360 } 00361 advance(t); 00362 memcpy(result->as<KoulesStateSpace::StateType>()->values, &qcur_[0], numDimensions_ * sizeof(double)); 00363 }