Skip to content

Commit

Permalink
Add DART_ prefix to macros to avoid potential conflicts (#1586)
Browse files Browse the repository at this point in the history
  • Loading branch information
@jslee02
jslee02 authored Jul 3, 2021
1 parent e2bd9d1 commit 0799362
Show file tree
Hide file tree
Showing 2 changed files with 80 additions and 70 deletions.
16 changes: 8 additions & 8 deletions dart/collision/fcl/FCLCollisionDetector.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1588,17 +1588,17 @@ int FFtest(
fcl::Vector3* res2)
{
float U0[3], U1[3], U2[3], V0[3], V1[3], V2[3], RES1[3], RES2[3];
SET(U0, r1);
SET(U1, r2);
SET(U2, r3);
SET(V0, R1);
SET(V1, R2);
SET(V2, R3);
DART_SET(U0, r1);
DART_SET(U1, r2);
DART_SET(U2, r3);
DART_SET(V0, R1);
DART_SET(V1, R2);
DART_SET(V2, R3);

int contactResult = tri_tri_intersect(V0, V1, V2, U0, U1, U2, RES1, RES2);

SET((*res1), RES1);
SET((*res2), RES2);
DART_SET((*res1), RES1);
DART_SET((*res2), RES2);

return contactResult;
}
Expand Down
134 changes: 72 additions & 62 deletions dart/collision/fcl/tri_tri_intersection_test.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -33,56 +33,43 @@
#ifndef DART_COLLISION_TRITRIINTERSECTIONTEST_HPP_
#define DART_COLLISION_TRITRIINTERSECTIONTEST_HPP_

#include <math.h>

#define FABS(x) ((float)fabs(x)) /* implement as is fastest on your machine */

/* if USE_EPSILON_TEST is true then we do a check:
if |dv|<EPSILON then dv=0.0;
else no check is done (which is less robust)
*/
#define USE_EPSILON_TEST TRUE
#define EPSILON 0.000001

#define NO_CONTACT 0
#define COPLANAR_CONTACT -1
#define INTERIAL_CONTACT 1
#include <cmath>

/* some macros */
#define CROSS(dest, v1, v2) \
#define DART_CROSS(dest, v1, v2) \
dest[0] = v1[1] * v2[2] - v1[2] * v2[1]; \
dest[1] = v1[2] * v2[0] - v1[0] * v2[2]; \
dest[2] = v1[0] * v2[1] - v1[1] * v2[0];

#define DOT(v1, v2) (v1[0] * v2[0] + v1[1] * v2[1] + v1[2] * v2[2])
#define DART_DOT(v1, v2) (v1[0] * v2[0] + v1[1] * v2[1] + v1[2] * v2[2])

#define SUB(dest, v1, v2) \
#define DART_SUB(dest, v1, v2) \
dest[0] = v1[0] - v2[0]; \
dest[1] = v1[1] - v2[1]; \
dest[2] = v1[2] - v2[2];

#define ADD(dest, v1, v2) \
#define DART_ADD(dest, v1, v2) \
dest[0] = v1[0] + v2[0]; \
dest[1] = v1[1] + v2[1]; \
dest[2] = v1[2] + v2[2];

#define MULT(dest, v, factor) \
#define DART_MULT(dest, v, factor) \
dest[0] = factor * v[0]; \
dest[1] = factor * v[1]; \
dest[2] = factor * v[2];

#define DIV(dest, v1, v2) \
#define DART_DIV(dest, v1, v2) \
dest[0] = v1[0] / v2[0]; \
dest[1] = v1[1] / 2 [1]; \
dest[2] = v1[2] / v2[2];

#define SET(dest, src) \
#define DART_SET(dest, src) \
dest[0] = src[0]; \
dest[1] = src[1]; \
dest[2] = src[2];

/* sort so that a<=b */
#define SORT(a, b) \
#define DART_SORT(a, b) \
if (a > b) \
{ \
float c; \
Expand All @@ -91,60 +78,80 @@
b = c; \
}

#define SWAP(a, b) \
#define DART_SWAP(a, b) \
{ \
float c; \
c = a; \
a = b; \
b = c; \
}

#define ISECT(VV0, VV1, VV2, D0, D1, D2, isect0, isect1) \
#define DART_ISECT(VV0, VV1, VV2, D0, D1, D2, isect0, isect1) \
isect0 = VV0 + (VV1 - VV0) * D0 / (D0 - D1); \
isect1 = VV0 + (VV2 - VV0) * D0 / (D0 - D2);

#define COMPUTE_INTERVALS( \
#define DART_COMPUTE_INTERVALS( \
VV0, VV1, VV2, D0, D1, D2, D0D1, D0D2, isect0, isect1) \
if (D0D1 > 0.0f) \
{ \
/* here we know that D0D2<=0.0 */ \
/* that is D0, D1 are on the same side, D2 on the other or on the plane */ \
ISECT(VV2, VV0, VV1, D2, D0, D1, isect0, isect1); \
DART_ISECT(VV2, VV0, VV1, D2, D0, D1, isect0, isect1); \
} \
else if (D0D2 > 0.0f) \
{ \
/* here we know that d0d1<=0.0 */ \
ISECT(VV1, VV0, VV2, D1, D0, D2, isect0, isect1); \
DART_ISECT(VV1, VV0, VV2, D1, D0, D2, isect0, isect1); \
} \
else if (D1 * D2 > 0.0f || D0 != 0.0f) \
{ \
/* here we know that d0d1<=0.0 or that D0!=0.0 */ \
ISECT(VV0, VV1, VV2, D0, D1, D2, isect0, isect1); \
DART_ISECT(VV0, VV1, VV2, D0, D1, D2, isect0, isect1); \
} \
else if (D1 != 0.0f) \
{ \
ISECT(VV1, VV0, VV2, D1, D0, D2, isect0, isect1); \
DART_ISECT(VV1, VV0, VV2, D1, D0, D2, isect0, isect1); \
} \
else if (D2 != 0.0f) \
{ \
ISECT(VV2, VV0, VV1, D2, D0, D1, isect0, isect1); \
DART_ISECT(VV2, VV0, VV1, D2, D0, D1, isect0, isect1); \
} \
else \
{ \
/* triangles are coplanar */ \
return COPLANAR_CONTACT; \
}

namespace dart {
namespace collision {

/* if USE_EPSILON_TEST is true then we do a check:
if |dv|<EPSILON then dv=0.0;
else no check is done (which is less robust)
*/
constexpr bool USE_EPSILON_TEST = true;
constexpr double EPSILON = 1e-6;

constexpr int NO_CONTACT = 0;
constexpr int COPLANAR_CONTACT = -1;
constexpr int INTERIAL_CONTACT = 1;

/* implement as is fastest on your machine */
inline float FABS(float x)
{
return ((float)fabs(x));
}

inline void edge_tri_intersect(
float V0[3], float V1[3], float DV0, float DV1, float V[3])
{
float VV0[3], VV1[3];
MULT(VV0, V1, DV0);
MULT(VV1, V0, DV1);
DART_MULT(VV0, V1, DV0);
DART_MULT(VV1, V0, DV1);
float U[3], D;
SUB(U, VV0, VV1);
DART_SUB(U, VV0, VV1);
D = DV0 - DV1;
MULT(V, U, 1.0 / D);
DART_MULT(V, U, 1.0 / D);
}

inline int tri_tri_intersect(
Expand All @@ -169,17 +176,17 @@ inline int tri_tri_intersect(
float b, c, max;

/* compute plane equation of triangle(V0,V1,V2) */
SUB(E1, V1, V0);
SUB(E2, V2, V0);
CROSS(N1, E1, E2);
d1 = -DOT(N1, V0);
DART_SUB(E1, V1, V0);
DART_SUB(E2, V2, V0);
DART_CROSS(N1, E1, E2);
d1 = -DART_DOT(N1, V0);
/* plane equation 1: N1.X+d1=0 */

/* put U0,U1,U2 into plane equation 1 to compute signed distances to the
* plane*/
du0 = DOT(N1, U0) + d1;
du1 = DOT(N1, U1) + d1;
du2 = DOT(N1, U2) + d1;
du0 = DART_DOT(N1, U0) + d1;
du1 = DART_DOT(N1, U1) + d1;
du2 = DART_DOT(N1, U2) + d1;

/* coplanarity robustness check */
#if USE_EPSILON_TEST == TRUE
Expand All @@ -205,16 +212,16 @@ inline int tri_tri_intersect(
return NO_CONTACT; /* no intersection occurs */
}
/* compute plane of triangle (U0,U1,U2) */
SUB(E1, U1, U0);
SUB(E2, U2, U0);
CROSS(N2, E1, E2);
d2 = -DOT(N2, U0);
DART_SUB(E1, U1, U0);
DART_SUB(E2, U2, U0);
DART_CROSS(N2, E1, E2);
d2 = -DART_DOT(N2, U0);
/* plane equation 2: N2.X+d2=0 */

/* put V0,V1,V2 into plane equation 2 */
dv0 = DOT(N2, V0) + d2;
dv1 = DOT(N2, V1) + d2;
dv2 = DOT(N2, V2) + d2;
dv0 = DART_DOT(N2, V0) + d2;
dv1 = DART_DOT(N2, V1) + d2;
dv2 = DART_DOT(N2, V2) + d2;

#if USE_EPSILON_TEST == TRUE
if (fabs(dv0) < EPSILON)
Expand All @@ -239,7 +246,7 @@ inline int tri_tri_intersect(
return NO_CONTACT; /* no intersection occurs */
}
/* compute direction of intersection line */
CROSS(D, N1, N2);
DART_CROSS(D, N1, N2);

/* compute and index to the largest component of D */
max = fabs(D[0]);
Expand All @@ -261,15 +268,15 @@ inline int tri_tri_intersect(
up2 = U2[index];

/* compute interval for triangle 1 */
COMPUTE_INTERVALS(
DART_COMPUTE_INTERVALS(
vp0, vp1, vp2, dv0, dv1, dv2, dv0dv1, dv0dv2, isect1[0], isect1[1]);

/* compute interval for triangle 2 */
COMPUTE_INTERVALS(
DART_COMPUTE_INTERVALS(
up0, up1, up2, du0, du1, du2, du0du1, du0du2, isect2[0], isect2[1]);

SORT(isect1[0], isect1[1]);
SORT(isect2[0], isect2[1]);
DART_SORT(isect1[0], isect1[1]);
DART_SORT(isect2[0], isect2[1]);

// if(isect1[1]<isect2[0] || isect2[1]<isect1[0]) return NO_CONTACT;

Expand All @@ -291,17 +298,17 @@ inline int tri_tri_intersect(
}
else if (du0 == 0)
{
SET(res[0], U0);
DART_SET(res[0], U0);
edge_tri_intersect(U1, U2, du1, du2, res[1]);
}
else if (du1 == 0)
{
SET(res[0], U1);
DART_SET(res[0], U1);
edge_tri_intersect(U0, U2, du0, du2, res[1]);
}
else if (du2 == 0)
{
SET(res[0], U2);
DART_SET(res[0], U2);
edge_tri_intersect(U0, U1, du0, du1, res[1]);
}
else
Expand All @@ -326,17 +333,17 @@ inline int tri_tri_intersect(
}
else if (dv0 == 0)
{
SET(res[2], V0);
DART_SET(res[2], V0);
edge_tri_intersect(V1, V2, dv1, dv2, res[3]);
}
else if (dv1 == 0)
{
SET(res[2], V1);
DART_SET(res[2], V1);
edge_tri_intersect(V0, V2, dv0, dv2, res[3]);
}
else if (dv2 == 0)
{
SET(res[2], V2);
DART_SET(res[2], V2);
edge_tri_intersect(V0, V1, dv0, dv1, res[3]);
}
else
Expand All @@ -350,13 +357,16 @@ inline int tri_tri_intersect(
if (res[j][index] > res[j + 1][index])
{
for (int k = 0; k < 3; k++)
SWAP(res[j][k], res[j + 1][k]);
DART_SWAP(res[j][k], res[j + 1][k]);
}
}
SET(res1, res[1]);
SET(res2, res[2]);
DART_SET(res1, res[1]);
DART_SET(res2, res[2]);

return 1;
}

} // namespace collision
} // namespace dart

#endif // DART_COLLISION_TRITRIINTERSECTIONTEST_HPP_

0 comments on commit 0799362

Please sign in to comment.