59 if (p > maxv) maxv = p;
60 if (p < minv) minv = p;
96 d[6] = p[0] + p[1] - p[2];
97 d[7] = p[0] + p[2] - p[1];
98 d[8] = p[1] + p[2] - p[0];
103 FCL_REAL real_max = (std::numeric_limits<FCL_REAL>::max)();
104 dist_.template head<N / 2>().setConstant(real_max);
105 dist_.template tail<N / 2>().setConstant(-real_max);
110 for (
short i = 0; i < 3; ++i) {
111 dist_[i] = dist_[N / 2 + i] = v[i];
116 for (
short i = 0; i < (N - 6) / 2; ++i) {
117 dist_[3 + i] = dist_[3 + i + N / 2] = d[i];
123 for (
short i = 0; i < 3; ++i) {
124 minmax(a[i], b[i], dist_[i], dist_[i + N / 2]);
127 FCL_REAL ad[(N - 6) / 2], bd[(N - 6) / 2];
130 for (
short i = 0; i < (N - 6) / 2; ++i) {
131 minmax(ad[i], bd[i], dist_[3 + i], dist_[3 + i + N / 2]);
137 if ((dist_.template head<N / 2>() > other.
dist_.template tail<N / 2>()).any())
139 if ((dist_.template tail<N / 2>() < other.
dist_.template head<N / 2>()).any())
146 FCL_REAL& sqrDistLowerBound)
const {
151 (dist_.template head<N / 2>() - other.
dist_.template tail<N / 2>())
153 if (a > breakDistance) {
154 sqrDistLowerBound = a * a;
159 (other.
dist_.template head<N / 2>() - dist_.template tail<N / 2>())
161 if (b > breakDistance) {
162 sqrDistLowerBound = b *
b;
166 sqrDistLowerBound = std::min(a, b);
172 if ((p.array() < dist_.template head<3>()).any())
return false;
173 if ((p.array() > dist_.template segment<3>(N / 2)).any())
return false;
175 enum {
P = ((N - 6) / 2) };
176 Eigen::Array<FCL_REAL, P, 1> d;
177 getDistances<P>(p, d.data());
179 if ((d < dist_.template segment<P>(3)).any())
return false;
180 if ((d > dist_.template segment<P>(3 + N / 2)).any())
return false;
187 for (
short i = 0; i < 3; ++i) {
188 minmax(p[i], dist_[i], dist_[N / 2 + i]);
193 for (
short i = 0; i < (N - 6) / 2; ++i) {
194 minmax(pd[i], dist_[3 + i], dist_[3 + N / 2 + i]);
202 for (
short i = 0; i < N / 2; ++i) {
203 dist_[i] = std::min(other.
dist_[i], dist_[i]);
204 dist_[i + N / 2] = std::max(other.
dist_[i + N / 2], dist_[i + N / 2]);
218 std::cerr <<
"KDOP distance not implemented!" << std::endl;
225 for (
short i = 0; i < 3; ++i) {
227 res.
dist(
short(N / 2 + i)) += t[i];
232 for (
short i = 0; i < (N - 6) / 2; ++i) {
233 res.
dist(
short(3 + i)) += d[i];
234 res.
dist(
short(3 + i + N / 2)) += d[i];
template KDOP< 18 > translate< 18 >(const KDOP< 18 > &, const Vec3f &)
bool inside(const Vec3f &p) const
KDOP class describes the KDOP collision structures. K is set as the template parameter, which should be 16, 18, or 24 The KDOP structure is defined by some pairs of parallel planes defined by some axes. For K = 16, the planes are 6 AABB planes and 10 diagonal planes that cut off some space of the edges: (-1,0,0) and (1,0,0) -> indices 0 and 8 (0,-1,0) and (0,1,0) -> indices 1 and 9 (0,0,-1) and (0,0,1) -> indices 2 and 10 (-1,-1,0) and (1,1,0) -> indices 3 and 11 (-1,0,-1) and (1,0,1) -> indices 4 and 12 (0,-1,-1) and (0,1,1) -> indices 5 and 13 (-1,1,0) and (1,-1,0) -> indices 6 and 14 (-1,0,1) and (1,0,-1) -> indices 7 and 15 For K = 18, the planes are 6 AABB planes and 12 diagonal planes that cut off some space of the edges: (-1,0,0) and (1,0,0) -> indices 0 and 9 (0,-1,0) and (0,1,0) -> indices 1 and 10 (0,0,-1) and (0,0,1) -> indices 2 and 11 (-1,-1,0) and (1,1,0) -> indices 3 and 12 (-1,0,-1) and (1,0,1) -> indices 4 and 13 (0,-1,-1) and (0,1,1) -> indices 5 and 14 (-1,1,0) and (1,-1,0) -> indices 6 and 15 (-1,0,1) and (1,0,-1) -> indices 7 and 16 (0,-1,1) and (0,1,-1) -> indices 8 and 17 For K = 18, the planes are 6 AABB planes and 18 diagonal planes that cut off some space of the edges: (-1,0,0) and (1,0,0) -> indices 0 and 12 (0,-1,0) and (0,1,0) -> indices 1 and 13 (0,0,-1) and (0,0,1) -> indices 2 and 14 (-1,-1,0) and (1,1,0) -> indices 3 and 15 (-1,0,-1) and (1,0,1) -> indices 4 and 16 (0,-1,-1) and (0,1,1) -> indices 5 and 17 (-1,1,0) and (1,-1,0) -> indices 6 and 18 (-1,0,1) and (1,0,-1) -> indices 7 and 19 (0,-1,1) and (0,1,-1) -> indices 8 and 20 (-1, -1, 1) and (1, 1, -1) –> indices 9 and 21 (-1, 1, -1) and (1, -1, 1) –> indices 10 and 22 (1, -1, -1) and (-1, 1, 1) –> indices 11 and 23.
void getDistances< 6 >(const Vec3f &p, FCL_REAL *d)
template KDOP< 24 > translate< 24 >(const KDOP< 24 > &, const Vec3f &)
template KDOP< 16 > translate< 16 >(const KDOP< 16 > &, const Vec3f &)
KDOP< N > & operator+=(const Vec3f &p)
Merge the point and the KDOP.
void getDistances(const Vec3f &, FCL_REAL *)
Compute the distances to planes with normals from KDOP vectors except those of AABB face planes...
request to the collision algorithm
void getDistances< 5 >(const Vec3f &p, FCL_REAL *d)
Specification of getDistances.
void minmax(FCL_REAL a, FCL_REAL b, FCL_REAL &minv, FCL_REAL &maxv)
Find the smaller and larger one of two values.
KDOP()
Creating kDOP containing nothing.
KDOP< N > operator+(const KDOP< N > &other) const
Create a KDOP by mergin two KDOPs.
Eigen::Array< FCL_REAL, N, 1 > dist_
Origin's distances to N KDOP planes.
FCL_REAL break_distance
Distance below which bounding volumes are broken down. See Collision.
static AABB translate(const AABB &aabb, const Vec3f &t)
translate the center of AABB by t
FCL_REAL dist(short i) const
void getDistances< 9 >(const Vec3f &p, FCL_REAL *d)
bool overlap(const KDOP< N > &other) const
Check whether two KDOPs overlap.
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
FCL_REAL security_margin
Distance below which objects are considered in collision. See Collision.
FCL_REAL distance(const KDOP< N > &other, Vec3f *P=NULL, Vec3f *Q=NULL) const
The distance between two KDOP<N>. Not implemented.