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 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines