KoulesSimulator.h
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2013, Rice University
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the Rice University nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34 
35 /* Author: Beck Chen, Mark Moll */
36 
37 #ifndef DEMOS_KOULES_SIMULATOR_
38 #define DEMOS_KOULES_SIMULATOR_
39 
40 #include "KoulesConfig.h"
41 #include <ompl/control/StatePropagator.h>
42 #include <boost/tuple/tuple.hpp>
43 #include <boost/tuple/tuple_comparison.hpp>
44 #include <queue>
45 
46 // State propagator for KoulesSetup.
48 {
49 public:
51 
52  // A propagate step.
53  void step(const ompl::base::State *start, const ompl::control::Control* control,
54  const double t, ompl::base::State *result);
55 
56 protected:
57  // A tuple containing the time and id's of two objects colliding
58  typedef boost::tuple<double, unsigned int, unsigned int> CollisionEvent;
59  // A priority queue of events, s.t. the top element is the collision
60  // that will happen first.
61  typedef std::priority_queue<CollisionEvent, std::vector<CollisionEvent>,
62  std::greater<CollisionEvent> > CollisionEventQueue;
63 
64  // Compute the collision events based on current positions and velocities.
65  // Push objects apart if they are slightly overlapping.
66  void initCollisionEvents(void);
67  // Return time when i will return with horizontal (dim==0) or vertical
68  // (dim==1) walls.
69  double wallCollideEvent(unsigned int i, int dim);
70  // Compute the collision response velocities when i and j collide.
71  void elasticCollision(unsigned int i, unsigned int j);
72  // Compute time if/when i and j will collide. If it happens before
73  // endTime_, insert a collision event in the queue.
74  void computeCollisionEvent(unsigned int i, unsigned int j);
75  // Advance to the system to time t assuming no collision happen
76  // between time_ and t.
77  void advance(double t);
78  // Mark object i as dead. The koules have id's 1,..,numKoules_, while
79  // the ship has id 0.
80  void markAsDead(unsigned int i);
81  // Analytic solution for ship's motion from time 0 to t.
82  void updateShip(const ompl::control::Control* control, double t);
83 
84  // Pointer to Koules' SpaceInformation.
86  // Number of dimensions in state space.
87  unsigned int numDimensions_;
88  // Number of koules.
89  unsigned int numKoules_;
90  // Scratch space holding the current state.
91  std::vector<double> qcur_;
92  // Scrath space holding the next state after integration.
93  std::vector<double> qnext_;
94  // A vector of flags indicating which objects are dead.
95  std::vector<bool> dead_;
96  // The current time in the simulation.
97  double time_;
98  // The time to stop the simulation.
99  double endTime_;
100  // A queue of collision events.
101  CollisionEventQueue collisionEvents_;
102 };
103 
104 #endif
Definition of an abstract control.
Definition: Control.h:48
Definition of an abstract state.
Definition: State.h:50
Space information containing necessary information for planning with controls. setup() needs to be ca...