Loading...
Searching...
No Matches
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.
46class KoulesSimulator
47{
48public:
49 KoulesSimulator(const ompl::control::SpaceInformation* si);
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
55protected:
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 state.
Definition State.h:50
Definition of an abstract control.
Definition Control.h:48
Space information containing necessary information for planning with controls. setup() needs to be ca...