41 const double dx = other.
x_-
x_;
42 const double dy = other.
y_-
y_;
44 return dx*dx+dy*dy<=R*R;
51 const double dx = other.
x_-
x_;
52 const double dy = other.
y_-
y_;
53 const double R = (std::sqrt)(dx*dx+dy*dy) + other.
rxy_;
73 iv.second = base[i0].second;
74 iv.second.x_ = 0.5 * (base[i0].second.x_+base[i1].second.x_);
75 iv.second.y_ = 0.5 * (base[i0].second.y_+base[i1].second.y_);
76 for(
int i=i0+1;i<=i1;i++)iv.second.bound(base[i].second);
93 const double guard = 1.e10;
96 const double dx = a.
x_-b.
x_;
97 const double dy = a.
y_-b.
y_;
98 return std::sqrt(dx*dx+dy*dy)-a.
rxy_-b.
rxy_;
112 const double dx = a.
x_-b.
x_;
113 const double dy = a.
y_-b.
y_;
114 const double d = std::sqrt(dx*dx+dy*dy)-a.
rxy_-b.
rxy_;
133 const double guard = 1.e10;
135 const double dx = a.
x_-b.
x_;
136 const double dy = a.
y_-b.
y_;
138 if(dx*dx+dy*dy>R*R)
return guard;
156 const double collision_distance = 0.0001;
158 return compute_min(other,collision_distance,result,f);
170 if(rv)result=(
std::max)(result,0.0);
Definition: occupancycylinder.h:84
bool getEarliestCollisionTime(const OccupancyCylinderTree &other, double max_time, double &result) const
Definition: occupancycylinder.h:179
bool collidesWith(const OccupancyCylinderTree &other) const
Definition: occupancycylinder.h:153
bool getPostEncroachmentTime(const OccupancyCylinderTree &other, double max_time, double &result) const
Definition: occupancycylinder.h:166
Definition: vectorbasedvolumetree.h:30
bool compute_min(const VectorBasedVolumeTree< OtherVolumeType, OtherFunctorType > &other, double cutoff, double &result_value, const MetricFunctor &f) const
Definition: vectorbasedvolumetree.h:161
T min(T a, T b, T c, T d)
Definition: adoremath.h:663
bool overlaps(const T &a0, const T &a1, const T &b0, const T &b1)
Definition: adoremath.h:655
adoreMatrix< T, N, M > max(adoreMatrix< T, N, M > a, const adoreMatrix< T, N, M > &b)
Definition: adoremath.h:686
x
Definition: adore_set_goal.py:30
z0
Definition: adore_set_goal.py:27
y
Definition: adore_set_goal.py:31
z1
Definition: adore_set_pose.py:30
Definition: areaofeffectconverter.h:20
Definition: occupancycylinder.h:63
std::vector< IndexedVolumeType > VolumeVector
Definition: occupancycylinder.h:65
std::pair< std::pair< int, int >, OccupancyCylinder > IndexedVolumeType
Definition: occupancycylinder.h:64
IndexedVolumeType operator()(const VolumeVector &base, int i0, int i1) const
Definition: occupancycylinder.h:70
Definition: occupancycylinder.h:105
EarliestOverlapFunctor(double max_time)
Definition: occupancycylinder.h:107
double operator()(const OccupancyCylinder &a, const OccupancyCylinder &b) const
Definition: occupancycylinder.h:108
double max_time_
Definition: occupancycylinder.h:106
Definition: occupancycylinder.h:130
double operator()(const OccupancyCylinder &a, const OccupancyCylinder &b) const
Definition: occupancycylinder.h:131
Definition: occupancycylinder.h:90
double operator()(const OccupancyCylinder &a, const OccupancyCylinder &b) const
Definition: occupancycylinder.h:91
Definition: occupancycylinder.h:25
OccupancyCylinder(double rxy, double x, double y, double t0, double t1, double z0=0.0, double z1=3.0)
Definition: occupancycylinder.h:31
double rxy_
Definition: occupancycylinder.h:26
double t0_
Definition: occupancycylinder.h:28
double y_
Definition: occupancycylinder.h:27
double z1_
Definition: occupancycylinder.h:29
double t1_
Definition: occupancycylinder.h:28
double z0_
Definition: occupancycylinder.h:29
bool intersects(const OccupancyCylinder &other) const
Definition: occupancycylinder.h:37
double x_
Definition: occupancycylinder.h:27
OccupancyCylinder()
Definition: occupancycylinder.h:30
void bound(const OccupancyCylinder &other)
Definition: occupancycylinder.h:49