Loading...
Searching...
No Matches
KoulesGoal.cpp
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#include "KoulesConfig.h"
36#include "KoulesStateSpace.h"
37#include "KoulesGoal.h"
38
40{
41 double minX, minY;
42 const double* v = st->as<KoulesStateSpace::StateType>()->values;
43 auto space = si_->getStateSpace()->as<KoulesStateSpace>();
44 std::size_t numKoules = (space->getDimension() - 5) / 4, liveKoules = numKoules;
45 double minDist = sideLength;
46
47 for (std::size_t i = 1, j = 5; i <= numKoules; ++i, j += 4)
48 {
49 if (space->isDead(st, i))
50 liveKoules--;
51 else
52 {
53 minX = std::min(v[j ], sideLength - v[j ]);
54 minY = std::min(v[j + 1], sideLength - v[j + 1]);
55 minDist = std::min(minDist, std::min(minX, minY) - kouleRadius + threshold_);
56 }
57 }
58 if (minDist < 0 || liveKoules == 0)
59 minDist = 0;
60 return .5 * sideLength * (double) liveKoules + minDist;
61}
62
64{
65 double* v = st->as<KoulesStateSpace::StateType>()->values;
66 std::size_t dim = si_->getStateDimension();
67 stateSampler_->sampleUniform(st);
68 for (std::size_t i = 5; i < dim; i += 4)
69 {
70 // randomly pick an edge for each koule to collide
71 if (rng_.uniformBool())
72 {
73 v[i ] = rng_.uniformBool() ? 0. : sideLength;
74 v[i + 1] = rng_.uniformReal(0., sideLength);
75 }
76 else
77 {
78 v[i ] = rng_.uniformReal(0., sideLength);
79 v[i + 1] = rng_.uniformBool() ? 0. : sideLength;
80 }
81 }
82}
virtual void sampleGoal(ompl::base::State *st) const
Sample a state in the goal region.
virtual double distanceGoal(const ompl::base::State *st) const
Compute the distance to the goal (heuristic). This function is the one used in computing the distance...
double threshold_
The maximum distance that is allowed to the goal. By default, this is initialized to the minimum epsi...
Definition GoalRegion.h:91
SpaceInformationPtr si_
The space information for this goal.
Definition Goal.h:149
Definition of an abstract state.
Definition State.h:50
const T * as() const
Cast this instance to a desired type.
Definition State.h:66