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 <tuple>
43 #include <queue>
44 
45 // State propagator for KoulesSetup.
47 {
48 public:
50 
51  // A propagate step.
52  void step(const ompl::base::State *start, const ompl::control::Control* control,
53  const double t, ompl::base::State *result);
54 
55 protected:
56  // A tuple containing the time and id's of two objects colliding
57  using CollisionEvent = std::tuple<double, unsigned int, unsigned int>;
58  // A priority queue of events, s.t. the top element is the collision
59  // that will happen first.
60  using CollisionEventQueue = std::priority_queue<CollisionEvent,
61  std::vector<CollisionEvent>, std::greater<CollisionEvent>>;
62 
63  // Compute the collision events based on current positions and velocities.
64  // Push objects apart if they are slightly overlapping.
65  void initCollisionEvents();
66  // Return time when i will return with horizontal (dim==0) or vertical
67  // (dim==1) walls.
68  double wallCollideEvent(unsigned int i, int dim);
69  // Compute the collision response velocities when i and j collide.
70  void elasticCollision(unsigned int i, unsigned int j);
71  // Compute time if/when i and j will collide. If it happens before
72  // endTime_, insert a collision event in the queue.
73  void computeCollisionEvent(unsigned int i, unsigned int j);
74  // Advance to the system to time t assuming no collision happen
75  // between time_ and t.
76  void advance(double t);
77  // Mark object i as dead. The koules have id's 1,..,numKoules_, while
78  // the ship has id 0.
79  void markAsDead(unsigned int i);
80  // Analytic solution for ship's motion from time 0 to t.
81  void updateShip(const ompl::control::Control* control, double t);
82 
83  // Pointer to Koules' SpaceInformation.
85  // Number of dimensions in state space.
86  unsigned int numDimensions_;
87  // Number of koules.
88  unsigned int numKoules_;
89  // Scratch space holding the current state.
90  std::vector<double> qcur_;
91  // Scrath space holding the next state after integration.
92  std::vector<double> qnext_;
93  // A vector of flags indicating which objects are dead.
94  std::vector<bool> dead_;
95  // The current time in the simulation.
96  double time_;
97  // The time to stop the simulation.
98  double endTime_;
99  // A queue of collision events.
100  CollisionEventQueue collisionEvents_;
101 };
102 
103 #endif
Definition of an abstract control.
Definition: Control.h:111
Definition of an abstract state.
Definition: State.h:113
Space information containing necessary information for planning with controls. setup() needs to be ca...