demos/Koules/KoulesSimulator.h
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 #ifndef DEMOS_KOULES_SIMULATOR_ 00038 #define DEMOS_KOULES_SIMULATOR_ 00039 00040 #include "KoulesConfig.h" 00041 #include <ompl/control/StatePropagator.h> 00042 #include <boost/tuple/tuple.hpp> 00043 #include <boost/tuple/tuple_comparison.hpp> 00044 #include <queue> 00045 00046 // State propagator for KoulesSetup. 00047 class KoulesSimulator 00048 { 00049 public: 00050 KoulesSimulator(const ompl::control::SpaceInformation* si); 00051 00052 // A propagate step. 00053 void step(const ompl::base::State *start, const ompl::control::Control* control, 00054 const double t, ompl::base::State *result); 00055 00056 protected: 00057 // A tuple containing the time and id's of two objects colliding 00058 typedef boost::tuple<double, unsigned int, unsigned int> CollisionEvent; 00059 // A priority queue of events, s.t. the top element is the collision 00060 // that will happen first. 00061 typedef std::priority_queue<CollisionEvent, std::vector<CollisionEvent>, 00062 std::greater<CollisionEvent> > CollisionEventQueue; 00063 00064 // Compute the collision events based on current positions and velocities. 00065 // Push objects apart if they are slightly overlapping. 00066 void initCollisionEvents(void); 00067 // Return time when i will return with horizontal (dim==0) or vertical 00068 // (dim==1) walls. 00069 double wallCollideEvent(unsigned int i, int dim); 00070 // Compute the collision response velocities when i and j collide. 00071 void elasticCollision(unsigned int i, unsigned int j); 00072 // Compute time if/when i and j will collide. If it happens before 00073 // endTime_, insert a collision event in the queue. 00074 void computeCollisionEvent(unsigned int i, unsigned int j); 00075 // Advance to the system to time t assuming no collision happen 00076 // between time_ and t. 00077 void advance(double t); 00078 // Mark object i as dead. The koules have id's 1,..,numKoules_, while 00079 // the ship has id 0. 00080 void markAsDead(unsigned int i); 00081 // Analytic solution for ship's motion from time 0 to t. 00082 void updateShip(const ompl::control::Control* control, double t); 00083 00084 // Pointer to Koules' SpaceInformation. 00085 const ompl::control::SpaceInformation* si_; 00086 // Number of dimensions in state space. 00087 unsigned int numDimensions_; 00088 // Number of koules. 00089 unsigned int numKoules_; 00090 // Scratch space holding the current state. 00091 std::vector<double> qcur_; 00092 // Scrath space holding the next state after integration. 00093 std::vector<double> qnext_; 00094 // A vector of flags indicating which objects are dead. 00095 std::vector<bool> dead_; 00096 // The current time in the simulation. 00097 double time_; 00098 // The time to stop the simulation. 00099 double endTime_; 00100 // A queue of collision events. 00101 CollisionEventQueue collisionEvents_; 00102 }; 00103 00104 #endif