diff --git a/trunk/unmanaged/ubODE-OpenSim/include/ode/odemath.h b/trunk/unmanaged/ubODE-OpenSim/include/ode/odemath.h
index 883e66e9..98eed1f8 100644
--- a/trunk/unmanaged/ubODE-OpenSim/include/ode/odemath.h
+++ b/trunk/unmanaged/ubODE-OpenSim/include/ode/odemath.h
@@ -1108,15 +1108,23 @@ ODE_PURE_INLINE dReal dCalcVectorDot4(const dReal *a, const dReal *b)
*/
ODE_PURE_INLINE dReal _dCalcVectorDot3(const dReal *a, const dReal *b, unsigned step_a, unsigned step_b)
{
- return a[0] * b[0] + a[step_a] * b[step_b] + a[2 * step_a] * b[2 * step_b];
+ return a[0] * b[0] + a[step_a] * b[step_b] + a[2 * step_a] * b[2 * step_b];
}
ODE_PURE_INLINE dReal dCalcVectorDot3_13 (const dReal *a, const dReal *b) { return _dCalcVectorDot3(a,b,1,3); }
ODE_PURE_INLINE dReal dCalcVectorDot3_31 (const dReal *a, const dReal *b) { return _dCalcVectorDot3(a,b,3,1); }
ODE_PURE_INLINE dReal dCalcVectorDot3_33 (const dReal *a, const dReal *b) { return _dCalcVectorDot3(a,b,3,3); }
ODE_PURE_INLINE dReal dCalcVectorDot3_14 (const dReal *a, const dReal *b) { return _dCalcVectorDot3(a,b,1,4); }
-ODE_PURE_INLINE dReal dCalcVectorDot3_41 (const dReal *a, const dReal *b) { return _dCalcVectorDot3(a,b,4,1); }
-ODE_PURE_INLINE dReal dCalcVectorDot3_44 (const dReal *a, const dReal *b) { return _dCalcVectorDot3(a,b,4,4); }
+ODE_PURE_INLINE dReal dCalcVectorDot3_41 (const dReal *a, const dReal *b)
+{
+ //return _dCalcVectorDot3(a,b,4,1);
+ return a[0] * b[0] + a[4] * b[1] + a[8] * b[2];
+}
+ODE_PURE_INLINE dReal dCalcVectorDot3_44 (const dReal *a, const dReal *b)
+{
+ //return _dCalcVectorDot3(a,b,4,4);
+ return a[0] * b[0] + a[4] * b[4] + a[8] * b[8];
+}
#if defined(__AVX__)
ODE_PURE_INLINE void dCalcVectorCross3(dReal *res, const dReal *a, const dReal *b)
@@ -1126,12 +1134,15 @@ ODE_PURE_INLINE void dCalcVectorCross3(dReal *res, const dReal *a, const dReal *
mb = _mm_loadu_ps(b);
t1 = _mm_shuffle_ps(ma, ma, _MM_SHUFFLE(3, 0, 2, 1)); // a1 a2 a0 a3
- t2 = _mm_shuffle_ps(mb, mb, _MM_SHUFFLE(3, 1, 0, 2)); // b2 b0 b1 b2
+ t2 = _mm_shuffle_ps(mb, mb, _MM_SHUFFLE(3, 1, 0, 2)); // b2 b0 b1 b3
t3 = _mm_mul_ps(t1, t2); //a1b2 a2b0 a0b1 a3b2
- t1 = _mm_shuffle_ps(t1, t1, _MM_SHUFFLE(3, 0, 2, 1));
- t2 = _mm_shuffle_ps(t2, t2, _MM_SHUFFLE(3, 1, 0, 2));
+ //t1 = _mm_shuffle_ps(t1, t1, _MM_SHUFFLE(3, 0, 2, 1));
+ //t2 = _mm_shuffle_ps(t2, t2, _MM_SHUFFLE(3, 1, 0, 2));
+ t1 = _mm_shuffle_ps(ma, ma, _MM_SHUFFLE(3, 1, 0, 2));
+ t2 = _mm_shuffle_ps(mb, mb, _MM_SHUFFLE(3, 0, 2, 1));
+
t4 = _mm_mul_ps(t1, t2);
ma = _mm_sub_ps(t3, t4);
@@ -1154,12 +1165,14 @@ ODE_PURE_INLINE void dCalcVectorCross3r4(dReal *res, const dReal *a, const dReal
mb = _mm_loadu_ps(b);
t1 = _mm_shuffle_ps(ma, ma, _MM_SHUFFLE(3, 0, 2, 1)); // a1 a2 a0 a3
- t2 = _mm_shuffle_ps(mb, mb, _MM_SHUFFLE(3, 1, 0, 2)); // b2 b0 b1 b2
+ t2 = _mm_shuffle_ps(mb, mb, _MM_SHUFFLE(3, 1, 0, 2)); // b2 b0 b1 b3
t3 = _mm_mul_ps(t1, t2); //a1b2 a2b0 a0b1 a3b2
- t1 = _mm_shuffle_ps(t1, t1, _MM_SHUFFLE(3, 0, 2, 1));
- t2 = _mm_shuffle_ps(t2, t2, _MM_SHUFFLE(3, 1, 0, 2));
+ //t1 = _mm_shuffle_ps(t1, t1, _MM_SHUFFLE(3, 0, 2, 1));
+ //t2 = _mm_shuffle_ps(t2, t2, _MM_SHUFFLE(3, 1, 0, 2));
+ t1 = _mm_shuffle_ps(ma, ma, _MM_SHUFFLE(3, 1, 0, 2));
+ t2 = _mm_shuffle_ps(mb, mb, _MM_SHUFFLE(3, 0, 2, 1));
t4 = _mm_mul_ps(t1, t2);
ma = _mm_sub_ps(t3, t4);
@@ -1181,12 +1194,19 @@ ODE_PURE_INLINE void dCalcVectorCross3r4(dReal *res, const dReal *a, const dReal
ODE_PURE_INLINE void _dCalcVectorCross3(dReal *res, const dReal *a, const dReal *b, unsigned step_res, unsigned step_a, unsigned step_b)
{
- res[ 0] = a[ step_a] * b[2*step_b] - a[2*step_a] *b [ step_b];
- res[ step_res] = a[2*step_a] * b[ 0] - a[ 0] *b [2*step_b];
- res[2 * step_res] = a[ 0] * b[ step_b] - a[ step_a] *b [ 0];
+ res[0] = a[step_a] * b[2 * step_b] - a[2 * step_a] * b[step_b];
+ res[step_res] = a[2 * step_a] * b[0] - a[0] * b[2 * step_b];
+ res[2 * step_res] = a[0] * b[step_b] - a[step_a] * b[0];
+}
+
+ODE_PURE_INLINE void dCalcVectorCross3_114(dReal *res, const dReal *a, const dReal *b)
+{
+ //_dCalcVectorCross3(res, a, b, 1, 1, 4);
+ res[0] = a[1] * b[8] - a[2] * b[4];
+ res[1] = a[2] * b[0] - a[0] * b[8];
+ res[2] = a[0] * b[4] - a[1] * b[0];
}
-ODE_PURE_INLINE void dCalcVectorCross3_114(dReal *res, const dReal *a, const dReal *b) { _dCalcVectorCross3(res, a, b, 1, 1, 4); }
ODE_PURE_INLINE void dCalcVectorCross3_141(dReal *res, const dReal *a, const dReal *b) { _dCalcVectorCross3(res, a, b, 1, 4, 1); }
ODE_PURE_INLINE void dCalcVectorCross3_144(dReal *res, const dReal *a, const dReal *b) { _dCalcVectorCross3(res, a, b, 1, 4, 4); }
ODE_PURE_INLINE void dCalcVectorCross3_411(dReal *res, const dReal *a, const dReal *b) { _dCalcVectorCross3(res, a, b, 4, 1, 1); }
@@ -1562,32 +1582,27 @@ ODE_PURE_INLINE void dMultVector3r4(dReal *res, const dReal *a)
ODE_PURE_INLINE void dMultiply0_331(dReal *res, const dReal *a, const dReal *b)
{
__m128 ma, mb, mc;
- dReal restmp[3];
mb = _mm_loadu_ps(b);
ma = _mm_loadu_ps(a);
mc = _mm_dp_ps(ma, mb, 0x71);
- restmp[0] = (dReal)_mm_cvtss_f32(mc);
+ res[0] = (dReal)_mm_cvtss_f32(mc);
ma = _mm_loadu_ps(a + 4);
mc = _mm_dp_ps(ma, mb, 0x71);
- restmp[1] = (dReal)_mm_cvtss_f32(mc);
+ res[1] = (dReal)_mm_cvtss_f32(mc);
ma = _mm_loadu_ps(a + 8);
mc = _mm_dp_ps(ma, mb, 0x71);
- restmp[2] = (dReal)_mm_cvtss_f32(mc);
-
- res[0] = restmp[0];
- res[1] = restmp[1];
- res[2] = restmp[2];
+ res[2] = (dReal)_mm_cvtss_f32(mc);
}
#else
ODE_PURE_INLINE void dMultiply0_331(dReal *res, const dReal *a, const dReal *b)
{
- res[0] = dCalcVectorDot3(a, b);
- res[1] = dCalcVectorDot3(a + 4, b);
- res[2] = dCalcVectorDot3(a + 8, b);
+ res[0] = a[0] * b[0] + a[1] * b[1] + a[2] * b[2];
+ res[1] = a[4] * b[0] + a[5] * b[1] + a[6] * b[2];
+ res[2] = a[8] * b[0] + a[9] * b[1] + a[10] * b[2];
}
#endif
@@ -1595,7 +1610,6 @@ ODE_PURE_INLINE void dMultiply0_331(dReal *res, const dReal *a, const dReal *b)
ODE_PURE_INLINE void dMultiply1_331(dReal *res, const dReal *a, const dReal *b)
{
__m128 ma, t0, t1, t2, m0, m1, m2, m3;
- dReal restmp[3];
t0 = _mm_loadu_ps(a); // a0 a1 a2 a3
t1 = _mm_loadu_ps(a + 4); // a4 a5 a6 a7
@@ -1613,22 +1627,18 @@ ODE_PURE_INLINE void dMultiply1_331(dReal *res, const dReal *a, const dReal *b)
t2 = _mm_shuffle_ps(m2, m3, _MM_SHUFFLE(2, 2, 2, 0)); // a2 a6 a10 a10
m0 = _mm_dp_ps(ma, t0, 0x71);
- restmp[0] = _mm_cvtss_f32(m0);
+ res[0] = _mm_cvtss_f32(m0);
m1 = _mm_dp_ps(ma, t1, 0x71);
- restmp[1] = _mm_cvtss_f32(m1);
+ res[1] = _mm_cvtss_f32(m1);
m2 = _mm_dp_ps(ma, t2, 0x71);
- restmp[2] = _mm_cvtss_f32(m2);
-
- res[0] = restmp[0];
- res[1] = restmp[1];
- res[2] = restmp[2];
+ res[2] = _mm_cvtss_f32(m2);
}
#else
ODE_PURE_INLINE void dMultiply1_331(dReal *res, const dReal *a, const dReal *b)
{
- res[0] = dCalcVectorDot3_41(a, b);
- res[1] = dCalcVectorDot3_41(a + 1, b);
- res[2] = dCalcVectorDot3_41(a + 2, b);
+ res[0] = a[0] * b[0] + a[4] * b[1] + a[8] * b[2];
+ res[1] = a[1] * b[0] + a[5] * b[1] + a[9] * b[2];
+ res[2] = a[2] * b[0] + a[6] * b[1] + a[10] * b[2];
}
#endif
diff --git a/trunk/unmanaged/ubODE-OpenSim/include/ode/odemath_legacy.h b/trunk/unmanaged/ubODE-OpenSim/include/ode/odemath_legacy.h
index a389588d..78c361dc 100644
--- a/trunk/unmanaged/ubODE-OpenSim/include/ode/odemath_legacy.h
+++ b/trunk/unmanaged/ubODE-OpenSim/include/ode/odemath_legacy.h
@@ -23,140 +23,4 @@
#ifndef _ODE_ODEMATH_LEGACY_H_
#define _ODE_ODEMATH_LEGACY_H_
-
-/*
- * These macros are not used any more inside of ODE
- * They are kept for backward compatibility with external code that
- * might still be using them.
- */
-
-/*
- * General purpose vector operations with other vectors or constants.
- */
-
-#define dOP(a,op,b,c) do { \
- (a)[0] = ((b)[0]) op ((c)[0]); \
- (a)[1] = ((b)[1]) op ((c)[1]); \
- (a)[2] = ((b)[2]) op ((c)[2]); \
-} while (0)
-#define dOPC(a,op,b,c) do { \
- (a)[0] = ((b)[0]) op (c); \
- (a)[1] = ((b)[1]) op (c); \
- (a)[2] = ((b)[2]) op (c); \
-} while (0)
-#define dOPE(a,op,b) do {\
- (a)[0] op ((b)[0]); \
- (a)[1] op ((b)[1]); \
- (a)[2] op ((b)[2]); \
-} while (0)
-#define dOPEC(a,op,c) do { \
- (a)[0] op (c); \
- (a)[1] op (c); \
- (a)[2] op (c); \
-} while (0)
-
-/* Define an equation with operators
- * For example this function can be used to replace
- *
- * for (int i=0; i<3; ++i)
- * a[i] += b[i] + c[i];
- *
- */
-#define dOPE2(a,op1,b,op2,c) do { \
- (a)[0] op1 ((b)[0]) op2 ((c)[0]); \
- (a)[1] op1 ((b)[1]) op2 ((c)[1]); \
- (a)[2] op1 ((b)[2]) op2 ((c)[2]); \
-} while (0)
-
-
-#define dLENGTHSQUARED(a) dCalcVectorLengthSquare3(a)
-#define dLENGTH(a) dCalcVectorLength3(a)
-#define dDISTANCE(a, b) dCalcPointsDistance3(a, b)
-
-
-#define dDOT(a, b) dCalcVectorDot3(a, b)
-#define dDOT13(a, b) dCalcVectorDot3_13(a, b)
-#define dDOT31(a, b) dCalcVectorDot3_31(a, b)
-#define dDOT33(a, b) dCalcVectorDot3_33(a, b)
-#define dDOT14(a, b) dCalcVectorDot3_14(a, b)
-#define dDOT41(a, b) dCalcVectorDot3_41(a, b)
-#define dDOT44(a, b) dCalcVectorDot3_44(a, b)
-
-
-/*
- * cross product, set a = b x c. dCROSSpqr means that elements of `a', `b'
- * and `c' are spaced p, q and r indexes apart respectively.
- * dCROSS() means dCROSS111. `op' is normally `=', but you can set it to
- * +=, -= etc to get other effects.
- */
-
-#define dCROSS(a,op,b,c) \
- do { \
- (a)[0] op ((b)[1]*(c)[2] - (b)[2]*(c)[1]); \
- (a)[1] op ((b)[2]*(c)[0] - (b)[0]*(c)[2]); \
- (a)[2] op ((b)[0]*(c)[1] - (b)[1]*(c)[0]); \
- } while(0)
-#define dCROSSpqr(a,op,b,c,p,q,r) \
- do { \
- (a)[ 0] op ((b)[ q]*(c)[2*r] - (b)[2*q]*(c)[ r]); \
- (a)[ p] op ((b)[2*q]*(c)[ 0] - (b)[ 0]*(c)[2*r]); \
- (a)[2*p] op ((b)[ 0]*(c)[ r] - (b)[ q]*(c)[ 0]); \
- } while(0)
-#define dCROSS114(a,op,b,c) dCROSSpqr(a,op,b,c,1,1,4)
-#define dCROSS141(a,op,b,c) dCROSSpqr(a,op,b,c,1,4,1)
-#define dCROSS144(a,op,b,c) dCROSSpqr(a,op,b,c,1,4,4)
-#define dCROSS411(a,op,b,c) dCROSSpqr(a,op,b,c,4,1,1)
-#define dCROSS414(a,op,b,c) dCROSSpqr(a,op,b,c,4,1,4)
-#define dCROSS441(a,op,b,c) dCROSSpqr(a,op,b,c,4,4,1)
-#define dCROSS444(a,op,b,c) dCROSSpqr(a,op,b,c,4,4,4)
-
-
-/*
-* set a 3x3 submatrix of A to a matrix such that submatrix(A)*b = a x b.
-* A is stored by rows, and has `skip' elements per row. the matrix is
-* assumed to be already zero, so this does not write zero elements!
-* if (plus,minus) is (+,-) then a positive version will be written.
-* if (plus,minus) is (-,+) then a negative version will be written.
-*/
-
-#define dCROSSMAT(A,a,skip,plus,minus) \
- do { \
- (A)[1] = minus (a)[2]; \
- (A)[2] = plus (a)[1]; \
- (A)[(skip)+0] = plus (a)[2]; \
- (A)[(skip)+2] = minus (a)[0]; \
- (A)[2*(skip)+0] = minus (a)[1]; \
- (A)[2*(skip)+1] = plus (a)[0]; \
- } while(0)
-
-
-
-
-/*
-Note: NEVER call any of these functions/macros with the same variable for A and C,
-it is not equivalent to A*=B.
-*/
-
-#define dMULTIPLY0_331(A, B, C) dMultiply0_331(A, B, C)
-#define dMULTIPLY1_331(A, B, C) dMultiply1_331(A, B, C)
-#define dMULTIPLY0_133(A, B, C) dMultiply0_133(A, B, C)
-#define dMULTIPLY0_333(A, B, C) dMultiply0_333(A, B, C)
-#define dMULTIPLY1_333(A, B, C) dMultiply1_333(A, B, C)
-#define dMULTIPLY2_333(A, B, C) dMultiply2_333(A, B, C)
-
-#define dMULTIPLYADD0_331(A, B, C) dMultiplyAdd0_331(A, B, C)
-#define dMULTIPLYADD1_331(A, B, C) dMultiplyAdd1_331(A, B, C)
-#define dMULTIPLYADD0_133(A, B, C) dMultiplyAdd0_133(A, B, C)
-#define dMULTIPLYADD0_333(A, B, C) dMultiplyAdd0_333(A, B, C)
-#define dMULTIPLYADD1_333(A, B, C) dMultiplyAdd1_333(A, B, C)
-#define dMULTIPLYADD2_333(A, B, C) dMultiplyAdd2_333(A, B, C)
-
-
-/*
- * These macros are not used any more inside of ODE
- * They are kept for backward compatibility with external code that
- * might still be using them.
- */
-
-
#endif /* #ifndef _ODE_ODEMATH_LEGACY_H_ */
diff --git a/trunk/unmanaged/ubODE-OpenSim/libccd/src/mpr.c b/trunk/unmanaged/ubODE-OpenSim/libccd/src/mpr.c
index 49dc9fc5..69e016d1 100644
--- a/trunk/unmanaged/ubODE-OpenSim/libccd/src/mpr.c
+++ b/trunk/unmanaged/ubODE-OpenSim/libccd/src/mpr.c
@@ -332,8 +332,17 @@ static void findPenetr(const void *obj1, const void *obj2, const ccd_t *ccd,
&ccdSimplexPoint(portal, 3)->v,
pdir);
*depth = CCD_SQRT(*depth);
- ccdVec3Normalize(pdir);
-
+ if (ccdIsZero(*depth)){
+ // If depth is zero, then we have a touching contact.
+ // So following findPenetrTouch(), we assign zero to
+ // the direction vector (it can actually be anything
+ // according to the decription of ccdMPRPenetration
+ // function).
+ ccdVec3Copy(pdir, ccd_vec3_origin);
+ }else
+ {
+ ccdVec3Normalize(pdir);
+ }
// barycentric coordinates:
findPos(obj1, obj2, ccd, portal, pos);
diff --git a/trunk/unmanaged/ubODE-OpenSim/libccd/src/vec3.c b/trunk/unmanaged/ubODE-OpenSim/libccd/src/vec3.c
index f1a08047..8f15f2c2 100644
--- a/trunk/unmanaged/ubODE-OpenSim/libccd/src/vec3.c
+++ b/trunk/unmanaged/ubODE-OpenSim/libccd/src/vec3.c
@@ -168,8 +168,17 @@ ccd_real_t ccdVec3PointTriDist2(const ccd_vec3_t *P,
q = ccdVec3Dot(&a, &d2);
r = ccdVec3Dot(&d1, &d2);
- s = (q * r - w * p) / (w * v - r * r);
- t = (-s * r - q) / w;
+ ccd_real_t d = (w * v - r * r);
+ if (ccdIsZero(d))
+ {
+ // To avoid division by zero for zero (or near zero) area triangles
+ s = t = -1.;
+ }
+ else
+ {
+ s = (q * r - w * p) / d;
+ t = (-s * r - q) / w;
+ }
if ((ccdIsZero(s) || s > CCD_ZERO)
&& (ccdEq(s, CCD_ONE) || s < CCD_ONE)
diff --git a/trunk/unmanaged/ubODE-OpenSim/ode/src/collision_kernel.cpp b/trunk/unmanaged/ubODE-OpenSim/ode/src/collision_kernel.cpp
index 7dc0147a..b20a5fe7 100644
--- a/trunk/unmanaged/ubODE-OpenSim/ode/src/collision_kernel.cpp
+++ b/trunk/unmanaged/ubODE-OpenSim/ode/src/collision_kernel.cpp
@@ -147,11 +147,13 @@ static int colliders_initialized = 0;
static void setCollider (int i, int j, dColliderFn *fn)
{
- if (colliders[i][j].fn == 0) {
+ if (colliders[i][j].fn == 0)
+ {
colliders[i][j].fn = fn;
colliders[i][j].reverse = 0;
}
- if (colliders[j][i].fn == 0) {
+ if (colliders[j][i].fn == 0)
+ {
colliders[j][i].fn = fn;
colliders[j][i].reverse = 1;
}
diff --git a/trunk/unmanaged/ubODE-OpenSim/ode/src/collision_kernel.h b/trunk/unmanaged/ubODE-OpenSim/ode/src/collision_kernel.h
index 759c9bd1..b11bb987 100644
--- a/trunk/unmanaged/ubODE-OpenSim/ode/src/collision_kernel.h
+++ b/trunk/unmanaged/ubODE-OpenSim/ode/src/collision_kernel.h
@@ -98,7 +98,8 @@ enum dxContactMergeOptions {
// the pos and R of the body (if body nonzero).
// a dGeomID is a pointer to this object.
-struct dxGeom : public dBase {
+struct dxGeom : public dBase
+{
int type; // geom type number, set by subclass constructor
int gflags; // flags used by geom and space
void *data; // user-defined data pointer
@@ -170,8 +171,10 @@ struct dxGeom : public dBase {
// compute the AABB only if it is not current. this function manipulates
// the GEOM_AABB_BAD flag.
- void recomputeAABB() {
- if (gflags & GEOM_AABB_BAD) {
+ void recomputeAABB()
+ {
+ if (gflags & GEOM_AABB_BAD)
+ {
// our aabb functions assume final_posr is up to date
recomputePosr();
computeAABB();
@@ -183,10 +186,12 @@ struct dxGeom : public dBase {
// add and remove this geom from a linked list maintained by a space.
- void spaceAdd (dxGeom **first_ptr) {
+ void spaceAdd (dxGeom **first_ptr)
+ {
next = *first_ptr;
tome = first_ptr;
- if (*first_ptr) (*first_ptr)->tome = &next;
+ if (*first_ptr)
+ (*first_ptr)->tome = &next;
*first_ptr = this;
}
void spaceRemove() {
@@ -196,7 +201,8 @@ struct dxGeom : public dBase {
// add and remove this geom from a linked list maintained by a body.
- void bodyAdd (dxBody *b) {
+ void bodyAdd (dxBody *b)
+ {
body = b;
body_next = b->geom;
b->geom = this;
@@ -221,7 +227,8 @@ struct dxGeom : public dBase {
#define dSPACE_TLS_KIND_MANUAL_VALUE 0
#endif
-struct dxSpace : public dxGeom {
+struct dxSpace : public dxGeom
+{
int count; // number of geoms in this space
dxGeom *first; // first geom in list
int cleanup; // cleanup mode, 1=destroy geoms on exit
@@ -269,8 +276,8 @@ struct dxSpace : public dxGeom {
//////////////////////////////////////////////////////////////////////////
-/*inline */
-void dxGeom::markAABBBad() {
+inline void dxGeom::markAABBBad()
+{
gflags |= (GEOM_DIRTY | GEOM_AABB_BAD);
CHECK_NOT_LOCKED(parent_space);
}
diff --git a/trunk/unmanaged/ubODE-OpenSim/ode/src/collision_quadtreespace.cpp b/trunk/unmanaged/ubODE-OpenSim/ode/src/collision_quadtreespace.cpp
index 19446785..a264e760 100644
--- a/trunk/unmanaged/ubODE-OpenSim/ode/src/collision_quadtreespace.cpp
+++ b/trunk/unmanaged/ubODE-OpenSim/ode/src/collision_quadtreespace.cpp
@@ -32,16 +32,6 @@
#include "collision_space_internal.h"
-
-#define AXIS0 0
-#define AXIS1 1
-#define UP 2
-
-//#define DRAWBLOCKS
-
-const int SPLITAXIS = 2;
-const int SPLITS = SPLITAXIS * SPLITAXIS;
-
#define GEOM_ENABLED(g) (((g)->gflags & GEOM_ENABLE_TEST_MASK) == GEOM_ENABLE_TEST_VALUE)
class Block
@@ -60,7 +50,6 @@ public:
void Collide(void* UserData, dNearCallback* Callback);
void Collide(dGeomID g1, dGeomID g2, void* UserData, dNearCallback* Callback);
-
void CollideLocal(dGeomID g2, void* UserData, dNearCallback* Callback);
void AddObject(dGeomID Object);
@@ -73,11 +62,10 @@ public:
Block* GetBlockChild(const dReal* AABB);
};
-
-void Block::Create(const dReal MinX, const dReal MaxX, const dReal MinZ, const dReal MaxZ, Block* Parent, int Depth, Block*& Blocks)
+void Block::Create(const dReal MinX, const dReal MaxX, const dReal MinY, const dReal MaxY, Block* Parent, int Depth, Block* &Blocks)
{
dIASSERT(MinX <= MaxX);
- dIASSERT(MinZ <= MaxZ);
+ dIASSERT(MinY <= MaxY);
mGeomCount = 0;
mFirst = 0;
@@ -85,43 +73,34 @@ void Block::Create(const dReal MinX, const dReal MaxX, const dReal MinZ, const d
mMinX = MinX;
mMaxX = MaxX;
- mMinZ = MinZ;
- mMaxZ = MaxZ;
+ mMinZ = MinY;
+ mMaxZ = MaxY;
this->mParent = Parent;
if (Depth > 0)
{
- mChildren = Blocks;
- Blocks += SPLITS;
-
- const dReal ChildExtentX = (MaxX - MinX) / SPLITAXIS;
- const dReal ChildExtentZ = (MaxZ - MinZ) / SPLITAXIS;
-
const int ChildDepth = Depth - 1;
- int Index = 0;
- dReal ChildRightX = MinX;
- for (int i = 0; i < SPLITAXIS; i++)
- {
- const dReal ChildLeftX = ChildRightX;
- ChildRightX = (i != SPLITAXIS - 1) ? ChildLeftX + ChildExtentX : MaxX;
+ mChildren = Blocks;
+ Blocks += 4;
- dReal ChildRightZ = MinZ;
- for (int j = 0; j < SPLITAXIS; j++)
- {
- const dReal ChildLeftZ = ChildRightZ;
- ChildRightZ = (j != SPLITAXIS - 1) ? ChildLeftZ + ChildExtentZ : MaxZ;
+ const dReal ChildExtentX = (MaxX - MinX) * dReal(0.5);
+ const dReal ChildExtentY = (MaxY - MinY) * dReal(0.5);
+ const dReal ChildMidX = MinX + ChildExtentX;
+ const dReal ChildMidY = MinY + ChildExtentY;
+ mChildren[0].Create(MinX, ChildMidX, MinY, ChildMidY, this, ChildDepth, Blocks);
+ mChildren[1].Create(MinX, ChildMidX, ChildMidY, MaxY, this, ChildDepth, Blocks);
- mChildren[Index].Create(ChildLeftX, ChildRightX, ChildLeftZ, ChildRightZ, this, ChildDepth, Blocks);
- ++Index;
- }
- }
+ mChildren[2].Create(ChildMidX, MaxX, MinY, ChildMidY, this, ChildDepth, Blocks);
+ mChildren[3].Create(ChildMidX, MaxX, ChildMidY, MaxY, this, ChildDepth, Blocks);
}
- else mChildren = 0;
+ else
+ mChildren = 0;
}
-void Block::Collide(void* UserData, dNearCallback* Callback){
+void Block::Collide(void* UserData, dNearCallback* Callback)
+{
// Collide the local list
dxGeom* g = mFirst;
while (g)
@@ -136,7 +115,7 @@ void Block::Collide(void* UserData, dNearCallback* Callback){
// Recurse for children
if (mChildren)
{
- for (int i = 0; i < SPLITS; i++)
+ for (int i = 0; i < 4; i++)
{
Block &CurrentChild = mChildren[i];
if (CurrentChild.mGeomCount <= 1)
@@ -149,22 +128,20 @@ void Block::Collide(void* UserData, dNearCallback* Callback){
}
// Note: g2 is assumed to be in this Block
-void Block::Collide(dxGeom* g1, dxGeom* g2, void* UserData, dNearCallback* Callback)
+void Block::Collide(dGeomID g1, dGeomID g2, void* UserData, dNearCallback* Callback)
{
// Collide against local list
while (g2)
{
- if (GEOM_ENABLED(g2))
- {
- collideAABBs (g1, g2, UserData, Callback);
- }
+ if (GEOM_ENABLED(g2) && testCollideAABBs(g1, g2))
+ Callback(UserData, g1, g2);
g2 = g2->next_ex;
}
// Collide against children
if (mChildren)
{
- for (int i = 0; i < SPLITS; i++)
+ for (int i = 0; i < 4; i++)
{
Block &CurrentChild = mChildren[i];
// Early out for empty blocks
@@ -177,24 +154,24 @@ void Block::Collide(dxGeom* g1, dxGeom* g2, void* UserData, dNearCallback* Callb
// Don't do AABB tests for single geom blocks.
if (CurrentChild.mGeomCount > 1)
{
- if (g1->aabb[AXIS0 * 2 + 0] >= CurrentChild.mMaxX ||
- g1->aabb[AXIS0 * 2 + 1] < CurrentChild.mMinX ||
- g1->aabb[AXIS1 * 2 + 0] >= CurrentChild.mMaxZ ||
- g1->aabb[AXIS1 * 2 + 1] < CurrentChild.mMinZ)
+ if (g1->aabb[0] > CurrentChild.mMaxX ||
+ g1->aabb[1] < CurrentChild.mMinX ||
+ g1->aabb[2] > CurrentChild.mMaxZ ||
+ g1->aabb[3] < CurrentChild.mMinZ)
continue;
}
CurrentChild.Collide(g1, CurrentChild.mFirst, UserData, Callback);
}
}
}
-
-void Block::CollideLocal(dxGeom* g2, void* UserData, dNearCallback* Callback){
+void Block::CollideLocal(dGeomID g2, void* UserData, dNearCallback* Callback)
+{
// Collide against local list
dxGeom* g1 = mFirst;
- while (g1){
- if (GEOM_ENABLED(g1)){
- collideAABBs (g1, g2, UserData, Callback);
- }
+ while (g1)
+ {
+ if (GEOM_ENABLED(g1) && testCollideAABBs(g1, g2))
+ Callback(UserData, g1, g2);
g1 = g1->next_ex;
}
}
@@ -229,7 +206,6 @@ void Block::DelObject(dGeomID Object)
Last->next_ex = g->next_ex;
else
mFirst = g->next_ex;
-
break;
}
Last = g;
@@ -261,12 +237,12 @@ void Block::Traverse(dGeomID Object)
}
}
-bool Block::Inside(const dReal* AABB)
+inline bool Block::Inside(const dReal* AABB)
{
- return AABB[AXIS0 * 2 + 0] >= mMinX &&
- AABB[AXIS0 * 2 + 1] < mMaxX &&
- AABB[AXIS1 * 2 + 0] >= mMinZ &&
- AABB[AXIS1 * 2 + 1] < mMaxZ;
+ return AABB[0] > mMinX &&
+ AABB[1] < mMaxX &&
+ AABB[2] > mMinZ &&
+ AABB[3] < mMaxZ;
}
Block* Block::GetBlock(const dReal* AABB)
@@ -286,7 +262,7 @@ Block* Block::GetBlockChild(const dReal* AABB)
{
if (mChildren)
{
- for (int i = 0; i < SPLITS; i++)
+ for (int i = 0; i < 4; i++)
{
Block &CurrentChild = mChildren[i];
if (CurrentChild.Inside(AABB))
@@ -350,10 +326,10 @@ dxQuadTreeSpace::dxQuadTreeSpace(dSpaceID _space, const dVector3 Center, const d
return;
}
- dReal MinX = Center[AXIS0] - Extents[AXIS0];
- dReal MaxX = dNextAfter((Center[AXIS0] + Extents[AXIS0]), (dReal)dInfinity);
- dReal MinZ = Center[AXIS1] - Extents[AXIS1];
- dReal MaxZ = dNextAfter((Center[AXIS1] + Extents[AXIS1]), (dReal)dInfinity);
+ dReal MinX = Center[0] - Extents[0];
+ dReal MaxX = dNextAfter((Center[0] + Extents[0]), (dReal)dInfinity);
+ dReal MinZ = Center[1] - Extents[1];
+ dReal MaxZ = dNextAfter((Center[1] + Extents[1]), (dReal)dInfinity);
Block* nBlocks = this->Blocks + 1; // This pointer gets modified!
this->Blocks[0].Create(MinX, MaxX, MinZ, MaxZ, 0, Depth, nBlocks);
@@ -380,7 +356,7 @@ dxGeom* dxQuadTreeSpace::getGeom(int Index){
//@@@
dDebug (0,"dxQuadTreeSpace::getGeom() not yet implemented");
-
+ return 0;
// This doesnt work
/*
if (CurrentIndex == Index){
@@ -446,7 +422,8 @@ PARENTRECURSE:
*/
}
-void dxQuadTreeSpace::add(dxGeom* g){
+void dxQuadTreeSpace::add(dxGeom* g)
+{
CHECK_NOT_LOCKED (this);
dAASSERT(g);
dUASSERT(g->tome_ex == 0 && g->next_ex == 0, "geom is already in a space");
@@ -465,32 +442,38 @@ void dxQuadTreeSpace::remove(dxGeom* g){
// remove
((Block*)g->tome_ex)->DelObject(g);
- for (int i = 0; i < DirtyList.size(); i++){
- if (DirtyList[i] == g){
+ for (int i = 0; i < DirtyList.size(); i++)
+ {
+ if (DirtyList[i] == g)
+ {
DirtyList.remove(i);
// (mg) there can be multiple instances of a dirty object on stack be sure to remove ALL and not just first, for this we decrement i
--i;
}
}
-
dxSpace::remove(g);
}
-void dxQuadTreeSpace::dirty(dxGeom* g){
+void dxQuadTreeSpace::dirty(dxGeom* g)
+{
DirtyList.push(g);
}
-void dxQuadTreeSpace::computeAABB(){
+void dxQuadTreeSpace::computeAABB()
+{
//
}
-void dxQuadTreeSpace::cleanGeoms(){
+void dxQuadTreeSpace::cleanGeoms()
+{
// compute the AABBs of all dirty geoms, and clear the dirty flags
lock_count++;
- for (int i = 0; i < DirtyList.size(); i++){
+ for (int i = 0; i < DirtyList.size(); i++)
+ {
dxGeom* g = DirtyList[i];
- if (IS_SPACE(g)){
+ if (IS_SPACE(g))
+ {
((dxSpace*)g)->cleanGeoms();
}
g->recomputeAABB();
@@ -503,7 +486,8 @@ void dxQuadTreeSpace::cleanGeoms(){
lock_count--;
}
-void dxQuadTreeSpace::collide(void* UserData, dNearCallback* Callback){
+void dxQuadTreeSpace::collide(void* UserData, dNearCallback* Callback)
+{
dAASSERT(Callback);
lock_count++;
@@ -515,7 +499,8 @@ void dxQuadTreeSpace::collide(void* UserData, dNearCallback* Callback){
}
-struct DataCallback {
+struct DataCallback
+{
void *data;
dNearCallback *callback;
};
@@ -527,14 +512,16 @@ static void swap_callback(void *data, dxGeom *g1, dxGeom *g2)
}
-void dxQuadTreeSpace::collide2(void* UserData, dxGeom* g2, dNearCallback* Callback){
+void dxQuadTreeSpace::collide2(void* UserData, dxGeom* g2, dNearCallback* Callback)
+{
dAASSERT(g2 && Callback);
lock_count++;
cleanGeoms();
g2->recomputeAABB();
- if (g2->parent_space == this){
+ if (g2->parent_space == this)
+ {
// The block the geom is in
Block* CurrentBlock = (Block*)g2->tome_ex;
@@ -545,9 +532,9 @@ void dxQuadTreeSpace::collide2(void* UserData, dxGeom* g2, dNearCallback* Callba
// Collide against parents
while ((CurrentBlock = CurrentBlock->mParent))
CurrentBlock->CollideLocal(g2, UserData, Callback);
-
}
- else {
+ else
+ {
DataCallback dc = {UserData, Callback};
Blocks[0].Collide(g2, Blocks[0].mFirst, &dc, swap_callback);
}
@@ -555,6 +542,7 @@ void dxQuadTreeSpace::collide2(void* UserData, dxGeom* g2, dNearCallback* Callba
lock_count--;
}
-dSpaceID dQuadTreeSpaceCreate(dxSpace* space, const dVector3 Center, const dVector3 Extents, int Depth){
+dSpaceID dQuadTreeSpaceCreate(dxSpace* space, const dVector3 Center, const dVector3 Extents, int Depth)
+{
return new dxQuadTreeSpace(space, Center, Extents, Depth);
}
diff --git a/trunk/unmanaged/ubODE-OpenSim/ode/src/collision_sapspace.cpp b/trunk/unmanaged/ubODE-OpenSim/ode/src/collision_sapspace.cpp
index 2b193f10..03ce1b89 100644
--- a/trunk/unmanaged/ubODE-OpenSim/ode/src/collision_sapspace.cpp
+++ b/trunk/unmanaged/ubODE-OpenSim/ode/src/collision_sapspace.cpp
@@ -210,7 +210,8 @@ private:
};
// Creation
-dSpaceID dSweepAndPruneSpaceCreate( dxSpace* space, int axisorder ) {
+dSpaceID dSweepAndPruneSpaceCreate( dxSpace* space, int axisorder )
+{
return new dxSAPSpace( space, axisorder );
}
@@ -237,11 +238,11 @@ static void collideGeomsNoAABBs( dxGeom *g1, dxGeom *g2, void *data, dNearCallba
dIASSERT( (g2->gflags & GEOM_AABB_BAD)==0 );
// no contacts if both geoms on the same body, and the body is not 0
- if (g1->body == g2->body && g1->body) return;
+ if (g1->body && g1->body == g2->body) return;
// test if the category and collide bitfields match
- if ( ((g1->category_bits & g2->collide_bits) ||
- (g2->category_bits & g1->collide_bits)) == 0) {
+ if ( ((g1->category_bits & g2->collide_bits) || (g2->category_bits & g1->collide_bits)) == 0)
+ {
return;
}
diff --git a/trunk/unmanaged/ubODE-OpenSim/ode/src/collision_space.cpp b/trunk/unmanaged/ubODE-OpenSim/ode/src/collision_space.cpp
index dc447af1..e2fb15af 100644
--- a/trunk/unmanaged/ubODE-OpenSim/ode/src/collision_space.cpp
+++ b/trunk/unmanaged/ubODE-OpenSim/ode/src/collision_space.cpp
@@ -20,11 +20,11 @@
* *
*************************************************************************/
-/*
+ /*
-spaces
+ spaces
-*/
+ */
#include
#include
@@ -39,39 +39,40 @@ spaces
#pragma warning(disable:4291) // for VC++, no complaints about "no matching operator delete found"
#endif
-//****************************************************************************
-// make the geom dirty by setting the GEOM_DIRTY and GEOM_BAD_AABB flags
-// and moving it to the front of the space's list. all the parents of a
-// dirty geom also become dirty.
+ //****************************************************************************
+ // make the geom dirty by setting the GEOM_DIRTY and GEOM_BAD_AABB flags
+ // and moving it to the front of the space's list. all the parents of a
+ // dirty geom also become dirty.
-void dGeomMoved (dxGeom *geom)
+void dGeomMoved(dxGeom *geom)
{
- dAASSERT (geom);
-
- // if geom is offset, mark it as needing a calculate
- if (geom->offset_posr)
- {
- geom->gflags |= GEOM_POSR_BAD;
- }
-
- // from the bottom of the space heirarchy up, process all clean geoms
- // turning them into dirty geoms.
- dxSpace *parent = geom->parent_space;
+ dAASSERT(geom);
- while (parent && (geom->gflags & GEOM_DIRTY) == 0)
- {
- geom->markAABBBad();
- parent->dirty (geom);
- geom = parent;
- parent = parent->parent_space;
- }
+ // if geom is offset, mark it as needing a calculate
+ if (geom->offset_posr)
+ {
+ geom->gflags |= GEOM_POSR_BAD;
+ }
- // all the remaining dirty geoms must have their AABB_BAD flags set, to
- // ensure that their AABBs get recomputed
- while (geom) {
- geom->markAABBBad();
- geom = geom->parent_space;
- }
+ // from the bottom of the space heirarchy up, process all clean geoms
+ // turning them into dirty geoms.
+ dxSpace *parent = geom->parent_space;
+
+ while (parent && (geom->gflags & GEOM_DIRTY) == 0)
+ {
+ geom->markAABBBad();
+ parent->dirty(geom);
+ geom = parent;
+ parent = parent->parent_space;
+ }
+
+ // all the remaining dirty geoms must have their AABB_BAD flags set, to
+ // ensure that their AABBs get recomputed
+ while (geom)
+ {
+ geom->markAABBBad();
+ geom = geom->parent_space;
+ }
}
#define GEOM_ENABLED(g) (((g)->gflags & GEOM_ENABLE_TEST_MASK) == GEOM_ENABLE_TEST_VALUE)
@@ -79,234 +80,266 @@ void dGeomMoved (dxGeom *geom)
//****************************************************************************
// dxSpace
-dxSpace::dxSpace (dSpaceID _space) : dxGeom (_space,0)
+dxSpace::dxSpace(dSpaceID _space) : dxGeom(_space, 0)
{
- count = 0;
- first = 0;
- cleanup = 1;
- sublevel = 0;
- tls_kind = dSPACE_TLS_KIND_INIT_VALUE;
- current_index = 0;
- current_geom = 0;
- lock_count = 0;
+ count = 0;
+ first = 0;
+ cleanup = 1;
+ sublevel = 0;
+ tls_kind = dSPACE_TLS_KIND_INIT_VALUE;
+ current_index = 0;
+ current_geom = 0;
+ lock_count = 0;
}
dxSpace::~dxSpace()
{
- CHECK_NOT_LOCKED (this);
- if (cleanup) {
- // note that destroying each geom will call remove()
- dxGeom *g,*n;
- for (g = first; g; g=n) {
- n = g->next;
- dGeomDestroy (g);
+ CHECK_NOT_LOCKED(this);
+ dxGeom *g, *n;
+ if (cleanup)
+ {
+ // note that destroying each geom will call remove()
+ for (g = first; g; g = n)
+ {
+ n = g->next;
+ dGeomDestroy(g);
+ }
}
- }
- else {
- dxGeom *g,*n;
- for (g = first; g; g=n) {
- n = g->next;
- remove (g);
+ else
+ {
+ for (g = first; g; g = n)
+ {
+ n = g->next;
+ remove(g);
+ }
}
- }
}
-
void dxSpace::computeAABB()
{
- if (first) {
- int i;
- dReal a[6];
- a[0] = dInfinity;
- a[1] = -dInfinity;
- a[2] = dInfinity;
- a[3] = -dInfinity;
- a[4] = dInfinity;
- a[5] = -dInfinity;
- for (dxGeom *g=first; g; g=g->next) {
- g->recomputeAABB();
- for (i=0; i<6; i += 2) if (g->aabb[i] < a[i]) a[i] = g->aabb[i];
- for (i=1; i<6; i += 2) if (g->aabb[i] > a[i]) a[i] = g->aabb[i];
+ if (first)
+ {
+ first->recomputeAABB();
+ dxGeom *g = first->next;
+ if (g)
+ {
+ dReal a[6];
+ memcpy(a, first->aabb, 6 * sizeof(dReal));
+ do
+ {
+ g->recomputeAABB();
+ if (g->aabb[0] < a[0]) a[0] = g->aabb[0];
+ if (g->aabb[1] > a[1]) a[1] = g->aabb[1];
+
+ if (g->aabb[2] < a[2]) a[2] = g->aabb[2];
+ if (g->aabb[3] > a[3]) a[3] = g->aabb[3];
+
+ if (g->aabb[4] < a[4]) a[4] = g->aabb[4];
+ if (g->aabb[5] > a[5]) a[5] = g->aabb[5];
+ } while ((g = g->next) != NULL);
+
+ memcpy(aabb, a, 6 * sizeof(dReal));
+ }
+ else
+ {
+ memcpy(aabb, first->aabb, 6 * sizeof(dReal));
+ }
+ }
+ else
+ {
+ dSetZero(aabb, 6);
}
- memcpy(aabb,a,6*sizeof(dReal));
- }
- else {
- dSetZero (aabb,6);
- }
}
// the dirty geoms are numbered 0..k, the clean geoms are numbered k+1..count-1
-dxGeom *dxSpace::getGeom (int i)
+dxGeom *dxSpace::getGeom(int i)
{
- dUASSERT (i >= 0 && i < count,"index out of range");
- if (current_geom && current_index == i-1) {
- current_geom = current_geom->next;
- current_index = i;
- return current_geom;
- }
- else {
- dxGeom *g=first;
- for (int j=0; jnext; else return 0;
+ dUASSERT(i >= 0 && i < count, "index out of range");
+ if (current_geom && current_index == i - 1)
+ {
+ current_geom = current_geom->next;
+ current_index = i;
+ return current_geom;
+ }
+ else
+ {
+ dxGeom *g;
+ if (current_geom && current_index < i - 1)
+ {
+ g = current_geom;
+ for (int j = current_index; j < i; j++)
+ {
+ if (g) g = g->next; else return 0;
+ }
+ }
+ else
+ {
+ g = first;
+ for (int j = 0; j < i; j++)
+ {
+ if (g) g = g->next; else return 0;
+ }
+ }
+
+ current_geom = g;
+ current_index = i;
+ return g;
}
- current_geom = g;
- current_index = i;
- return g;
- }
}
-void dxSpace::add (dxGeom *geom)
+void dxSpace::add(dxGeom *geom)
{
- CHECK_NOT_LOCKED (this);
- dAASSERT (geom);
- dUASSERT (geom->parent_space == 0 && geom->next == 0,
- "geom is already in a space");
+ CHECK_NOT_LOCKED(this);
+ dAASSERT(geom);
+ dUASSERT(geom->parent_space == 0 && geom->next == 0,
+ "geom is already in a space");
- // add
- geom->parent_space = this;
- geom->spaceAdd (&first);
- count++;
+ // add
+ geom->parent_space = this;
+ geom->spaceAdd(&first);
+ count++;
- // enumerator has been invalidated
- current_geom = 0;
+ // enumerator has been invalidated
+ current_geom = 0;
- dGeomMoved (this);
+ dGeomMoved(this);
}
-void dxSpace::remove (dxGeom *geom)
+void dxSpace::remove(dxGeom *geom)
{
- CHECK_NOT_LOCKED (this);
- dAASSERT (geom);
- dUASSERT (geom->parent_space == this,"object is not in this space");
+ CHECK_NOT_LOCKED(this);
+ dAASSERT(geom);
+ dUASSERT(geom->parent_space == this, "object is not in this space");
- // remove
- geom->spaceRemove();
- count--;
+ // remove
+ geom->spaceRemove();
+ count--;
- // safeguard
- geom->next = 0;
- geom->tome = 0;
- geom->next_ex = 0;
- geom->tome_ex = 0;
- geom->parent_space = 0;
+ // safeguard
+ geom->next = 0;
+ geom->tome = 0;
+ geom->next_ex = 0;
+ geom->tome_ex = 0;
+ geom->parent_space = 0;
- // enumerator has been invalidated
- current_geom = 0;
+ // enumerator has been invalidated
+ current_geom = 0;
- // the bounding box of this space (and that of all the parents) may have
- // changed as a consequence of the removal.
- dGeomMoved (this);
+ // the bounding box of this space (and that of all the parents) may have
+ // changed as a consequence of the removal.
+ dGeomMoved(this);
}
-void dxSpace::dirty (dxGeom *geom)
+void dxSpace::dirty(dxGeom *geom)
{
- geom->spaceRemove();
- geom->spaceAdd (&first);
+ geom->spaceRemove();
+ geom->spaceAdd(&first);
}
//****************************************************************************
// simple space - reports all n^2 object intersections
-struct dxSimpleSpace : public dxSpace {
- dxSimpleSpace (dSpaceID _space);
- void cleanGeoms();
- void collide (void *data, dNearCallback *callback);
- void collide2 (void *data, dxGeom *geom, dNearCallback *callback);
+struct dxSimpleSpace : public dxSpace
+{
+ dxSimpleSpace(dSpaceID _space);
+ void cleanGeoms();
+ void collide(void *data, dNearCallback *callback);
+ void collide2(void *data, dxGeom *geom, dNearCallback *callback);
};
-dxSimpleSpace::dxSimpleSpace (dSpaceID _space) : dxSpace (_space)
+dxSimpleSpace::dxSimpleSpace(dSpaceID _space) : dxSpace(_space)
{
- type = dSimpleSpaceClass;
+ type = dSimpleSpaceClass;
}
void dxSimpleSpace::cleanGeoms()
{
- // compute the AABBs of all dirty geoms, and clear the dirty flags
- lock_count++;
- bool needrecon = (gflags & GEOM_AABB_BAD) != 0;
- if (gflags & GEOM_DIRTY)
- {
- for (dxGeom *g = first; g && (g->gflags & GEOM_DIRTY); g = g->next)
- {
- if (IS_SPACE(g))
- {
- ((dxSpace*)g)->cleanGeoms();
- }
- g->recomputeAABB();
- dIASSERT((g->gflags & GEOM_AABB_BAD) == 0);
- g->gflags &= ~GEOM_DIRTY;
- needrecon = true;
- }
- if(needrecon)
- recomputeAABB();
- gflags &= ~GEOM_DIRTY;
- }
- lock_count--;
-}
-
-
-void dxSimpleSpace::collide (void *data, dNearCallback *callback)
-{
- dAASSERT (callback);
-
- lock_count++;
- cleanGeoms();
-
- // intersect all bounding boxes
- for (dxGeom *g1=first; g1; g1=g1->next)
- {
- if (GEOM_ENABLED(g1))
+ // compute the AABBs of all dirty geoms, and clear the dirty flags
+ lock_count++;
+ bool needrecon = (gflags & GEOM_AABB_BAD) != 0;
+ if (gflags & GEOM_DIRTY)
{
- for (dxGeom *g2=g1->next; g2; g2=g2->next)
- {
- if (GEOM_ENABLED(g2))
+ for (dxGeom *g = first; g && (g->gflags & GEOM_DIRTY); g = g->next)
{
- collideAABBs (g1,g2,data,callback);
- }
- }
+ if (IS_SPACE(g))
+ {
+ ((dxSpace*)g)->cleanGeoms();
+ }
+ g->recomputeAABB();
+ dIASSERT((g->gflags & GEOM_AABB_BAD) == 0);
+ g->gflags &= ~GEOM_DIRTY;
+ needrecon = true;
+ }
+ if (needrecon)
+ recomputeAABB();
+ gflags &= ~GEOM_DIRTY;
}
- }
-
- lock_count--;
+ lock_count--;
}
-void dxSimpleSpace::collide2 (void *data, dxGeom *geom,
- dNearCallback *callback)
+void dxSimpleSpace::collide(void *cdata, dNearCallback *callback)
{
- dAASSERT (geom && callback);
+ dAASSERT(callback);
- if (!first)
- return;
- if (IS_SPACE(geom))
- {
- if (!((dxSpace*)geom)->first)
- return;
- }
+ lock_count++;
+ cleanGeoms();
- if (((category_bits & geom->collide_bits) ||
- (geom->category_bits &collide_bits)) == 0)
- return;
-
- lock_count++;
- cleanGeoms();
- geom->recomputeAABB();
-
- // intersect bounding boxes
- for (dxGeom *g=first; g; g=g->next) {
- if (GEOM_ENABLED(g)){
- collideAABBs (g,geom,data,callback);
+ // intersect all bounding boxes
+ for (dxGeom *g1 = first; g1; g1 = g1->next)
+ {
+ if (GEOM_ENABLED(g1))
+ {
+ for (dxGeom *g2 = g1->next; g2; g2 = g2->next)
+ {
+ if (GEOM_ENABLED(g2))
+ {
+ if(testCollideAABBs(g1, g2))
+ callback(cdata, g1, g2);
+ }
+ }
+ }
}
- }
- lock_count--;
+
+ lock_count--;
+}
+
+
+void dxSimpleSpace::collide2(void *cdata, dxGeom *geom,
+ dNearCallback *callback)
+{
+ dAASSERT(geom && callback);
+
+ if (!first)
+ return;
+ if (IS_SPACE(geom))
+ {
+ if (!((dxSpace*)geom)->first)
+ return;
+ }
+
+ if (((category_bits & geom->collide_bits) || (geom->category_bits & collide_bits)) == 0)
+ return;
+
+ lock_count++;
+ cleanGeoms();
+ geom->recomputeAABB();
+
+ // intersect bounding boxes
+ for (dxGeom *g = first; g; g = g->next)
+ {
+ if (GEOM_ENABLED(g) && testCollideAABBs(g, geom))
+ callback(cdata, g, geom);
+ }
+ lock_count--;
}
//****************************************************************************
@@ -320,28 +353,28 @@ void dxSimpleSpace::collide2 (void *data, dxGeom *geom,
// prime[i] is the largest prime smaller than 2^i
#define NUM_PRIMES 31
-static const long int prime[NUM_PRIMES] = {1L,2L,3L,7L,13L,31L,61L,127L,251L,509L,
+static const long int prime[NUM_PRIMES] = { 1L,2L,3L,7L,13L,31L,61L,127L,251L,509L,
1021L,2039L,4093L,8191L,16381L,32749L,65521L,131071L,262139L,
524287L,1048573L,2097143L,4194301L,8388593L,16777213L,33554393L,
- 67108859L,134217689L,268435399L,536870909L,1073741789L};
+ 67108859L,134217689L,268435399L,536870909L,1073741789L };
// an axis aligned bounding box in the hash table
struct dxAABB {
- dxAABB *next; // next in the list of all AABBs
- int level; // the level this is stored in (cell size = 2^level)
- int dbounds[6]; // AABB bounds, discretized to cell size
- dxGeom *geom; // corresponding geometry object (AABB stored here)
- size_t index; // index of this AABB, starting from 0
+ dxAABB *next; // next in the list of all AABBs
+ int level; // the level this is stored in (cell size = 2^level)
+ int dbounds[6]; // AABB bounds, discretized to cell size
+ dxGeom *geom; // corresponding geometry object (AABB stored here)
+ size_t index; // index of this AABB, starting from 0
};
// a hash table node that represents an AABB that intersects a particular cell
// at a particular level
struct Node {
- Node *next; // next node in hash table collision list, 0 if none
- int x,y,z; // cell position in space, discretized to cell size
- dxAABB *aabb; // axis aligned bounding box that intersects this cell
+ Node *next; // next node in hash table collision list, 0 if none
+ int x, y, z; // cell position in space, discretized to cell size
+ dxAABB *aabb; // axis aligned bounding box that intersects this cell
};
@@ -352,26 +385,26 @@ struct Node {
// size/2 < q <= size
// where q is the maximum AABB dimension.
-static int findLevel (dReal bounds[6])
+static int findLevel(dReal bounds[6])
{
- if (bounds[0] <= -dInfinity || bounds[1] >= dInfinity ||
- bounds[2] <= -dInfinity || bounds[3] >= dInfinity ||
- bounds[4] <= -dInfinity || bounds[5] >= dInfinity) {
- return MAXINT;
- }
+ if (bounds[0] <= -dInfinity || bounds[1] >= dInfinity ||
+ bounds[2] <= -dInfinity || bounds[3] >= dInfinity ||
+ bounds[4] <= -dInfinity || bounds[5] >= dInfinity) {
+ return MAXINT;
+ }
- // compute q
- dReal q,q2;
- q = bounds[1] - bounds[0]; // x bounds
- q2 = bounds[3] - bounds[2]; // y bounds
- if (q2 > q) q = q2;
- q2 = bounds[5] - bounds[4]; // z bounds
- if (q2 > q) q = q2;
+ // compute q
+ dReal q, q2;
+ q = bounds[1] - bounds[0]; // x bounds
+ q2 = bounds[3] - bounds[2]; // y bounds
+ if (q2 > q) q = q2;
+ q2 = bounds[5] - bounds[4]; // z bounds
+ if (q2 > q) q = q2;
- // find level such that 0.5 * 2^level < q <= 2^level
- int level;
- frexp (q,&level); // q = (0.5 .. 1.0) * 2^level (definition of frexp)
- return level;
+ // find level such that 0.5 * 2^level < q <= 2^level
+ int level;
+ frexp(q, &level); // q = (0.5 .. 1.0) * 2^level (definition of frexp)
+ return level;
}
@@ -381,487 +414,512 @@ static int findLevel (dReal bounds[6])
// factors could be better designed to avoid collisions, and they should
// probably depend on the hash table physical size.
-static unsigned long getVirtualAddress (int level, int x, int y, int z)
+static unsigned long getVirtualAddress(int level, int x, int y, int z)
{
- return level*1000 + x*100 + y*10 + z;
+ return level * 1000 + x * 100 + y * 10 + z;
}
//****************************************************************************
// hash space
struct dxHashSpace : public dxSpace {
- int global_minlevel; // smallest hash table level to put AABBs in
- int global_maxlevel; // objects that need a level larger than this will be
- // put in a "big objects" list instead of a hash table
+ int global_minlevel; // smallest hash table level to put AABBs in
+ int global_maxlevel; // objects that need a level larger than this will be
+ // put in a "big objects" list instead of a hash table
- dxHashSpace (dSpaceID _space);
- void setLevels (int minlevel, int maxlevel);
- void getLevels (int *minlevel, int *maxlevel);
- void cleanGeoms();
- void collide (void *data, dNearCallback *callback);
- void collide2 (void *data, dxGeom *geom, dNearCallback *callback);
+ dxHashSpace(dSpaceID _space);
+ void setLevels(int minlevel, int maxlevel);
+ void getLevels(int *minlevel, int *maxlevel);
+ void cleanGeoms();
+ void collide(void *data, dNearCallback *callback);
+ void collide2(void *data, dxGeom *geom, dNearCallback *callback);
};
-dxHashSpace::dxHashSpace (dSpaceID _space) : dxSpace (_space)
+dxHashSpace::dxHashSpace(dSpaceID _space) : dxSpace(_space)
{
- type = dHashSpaceClass;
- global_minlevel = -3;
- global_maxlevel = 10;
+ type = dHashSpaceClass;
+ global_minlevel = -3;
+ global_maxlevel = 10;
}
-void dxHashSpace::setLevels (int minlevel, int maxlevel)
+void dxHashSpace::setLevels(int minlevel, int maxlevel)
{
- dAASSERT (minlevel <= maxlevel);
- global_minlevel = minlevel;
- global_maxlevel = maxlevel;
+ dAASSERT(minlevel <= maxlevel);
+ global_minlevel = minlevel;
+ global_maxlevel = maxlevel;
}
-void dxHashSpace::getLevels (int *minlevel, int *maxlevel)
+void dxHashSpace::getLevels(int *minlevel, int *maxlevel)
{
- if (minlevel) *minlevel = global_minlevel;
- if (maxlevel) *maxlevel = global_maxlevel;
+ if (minlevel) *minlevel = global_minlevel;
+ if (maxlevel) *maxlevel = global_maxlevel;
}
void dxHashSpace::cleanGeoms()
{
- // compute the AABBs of all dirty geoms, and clear the dirty flags
- lock_count++;
- for (dxGeom *g=first; g && (g->gflags & GEOM_DIRTY); g=g->next)
- {
- if (IS_SPACE(g))
+ // compute the AABBs of all dirty geoms, and clear the dirty flags
+ lock_count++;
+ for (dxGeom *g = first; g && (g->gflags & GEOM_DIRTY); g = g->next)
{
- ((dxSpace*)g)->cleanGeoms();
- }
- g->recomputeAABB();
- dIASSERT((g->gflags & GEOM_AABB_BAD) == 0);
- g->gflags &= ~GEOM_DIRTY;
- }
- lock_count--;
-}
-
-
-void dxHashSpace::collide (void *data, dNearCallback *callback)
-{
- dAASSERT(this && callback);
- dxGeom *geom;
- dxAABB *aabb;
- int i,maxlevel;
-
- // 0 or 1 geoms can't collide with anything
- if (count < 2) return;
-
- lock_count++;
- cleanGeoms();
-
- // create a list of auxiliary information for all geom axis aligned bounding
- // boxes. set the level for all AABBs. put AABBs larger than the space's
- // global_maxlevel in the big_boxes list, check everything else against
- // that list at the end. for AABBs that are not too big, record the maximum
- // level that we need.
-
- int n = 0; // number of AABBs in main list
- dxAABB *first_aabb = 0; // list of AABBs in hash table
- dxAABB *big_boxes = 0; // list of AABBs too big for hash table
- maxlevel = global_minlevel - 1;
- for (geom = first; geom; geom=geom->next)
- {
- if (!GEOM_ENABLED(geom))
- {
- continue;
- }
- dxAABB *aabb = (dxAABB*) ALLOCA (sizeof(dxAABB));
- aabb->geom = geom;
- // compute level, but prevent cells from getting too small
- int level = findLevel (geom->aabb);
- if (level < global_minlevel) level = global_minlevel;
- if (level <= global_maxlevel) {
- // aabb goes in main list
- aabb->next = first_aabb;
- first_aabb = aabb;
- aabb->level = level;
- if (level > maxlevel) maxlevel = level;
- // cellsize = 2^level
- dReal cellSizeRecip = dRecip((dReal) ldexp(1.0, level));
- // discretize AABB position to cell size
- for (i=0; i < 6; i++) aabb->dbounds[i] = (int)
- floor (geom->aabb[i] * cellSizeRecip);
- // set AABB index
- aabb->index = n;
- n++;
- }
- else {
- // aabb is too big, put it in the big_boxes list. we don't care about
- // setting level, dbounds, index, or the maxlevel
- aabb->next = big_boxes;
- big_boxes = aabb;
- }
- }
-
- // for `n' objects, an n*n array of bits is used to record if those objects
- // have been intersection-tested against each other yet. this array can
- // grow large with high n, but oh well...
- int tested_rowsize = (n+7) >> 3; // number of bytes needed for n bits
- unsigned char *tested = (unsigned char *) ALLOCA (n * tested_rowsize);
- memset (tested,0,n * tested_rowsize);
-
- // create a hash table to store all AABBs. each AABB may take up to 8 cells.
- // we use chaining to resolve collisions, but we use a relatively large table
- // to reduce the chance of collisions.
-
- // compute hash table size sz to be a prime > 8*n
- for (i=0; i= (8*n)) break;
- }
- if (i >= NUM_PRIMES) i = NUM_PRIMES-1; // probably pointless
- const int sz = prime[i];
-
- // allocate and initialize hash table node pointers
- Node **table = (Node **) ALLOCA (sizeof(Node*) * sz);
- for (i=0; inext)
- {
- const int *dbounds = aabb->dbounds;
- for (int xi = dbounds[0]; xi <= dbounds[1]; xi++)
- {
- for (int yi = dbounds[2]; yi <= dbounds[3]; yi++)
- {
- for (int zi = dbounds[4]; zi <= dbounds[5]; zi++)
+ if (IS_SPACE(g))
{
- // get the hash index
- unsigned long hi = getVirtualAddress (aabb->level,xi,yi,zi) % sz;
- // add a new node to the hash table
- Node *node = (Node*) ALLOCA (sizeof (Node));
- node->x = xi;
- node->y = yi;
- node->z = zi;
- node->aabb = aabb;
- node->next = table[hi];
- table[hi] = node;
- }
- }
+ ((dxSpace*)g)->cleanGeoms();
+ }
+ g->recomputeAABB();
+ dIASSERT((g->gflags & GEOM_AABB_BAD) == 0);
+ g->gflags &= ~GEOM_DIRTY;
}
- }
-
- // now that all AABBs are loaded into the hash table, we do the actual
- // collision detection. for all AABBs, check for other AABBs in the
- // same cells for collisions, and then check for other AABBs in all
- // intersecting higher level cells.
-
- int db[6]; // discrete bounds at current level
- for (aabb=first_aabb; aabb; aabb=aabb->next) {
- // we are searching for collisions with aabb
- for (i=0; i<6; i++) db[i] = aabb->dbounds[i];
- for (int level = aabb->level; level <= maxlevel; level++) {
- for (int xi = db[0]; xi <= db[1]; xi++) {
- for (int yi = db[2]; yi <= db[3]; yi++) {
- for (int zi = db[4]; zi <= db[5]; zi++) {
- // get the hash index
- unsigned long hi = getVirtualAddress (level,xi,yi,zi) % sz;
- // search all nodes at this index
- Node *node;
- for (node = table[hi]; node; node=node->next) {
- // node points to an AABB that may intersect aabb
- if (node->aabb == aabb) continue;
- if (node->aabb->level == level &&
- node->x == xi && node->y == yi && node->z == zi) {
- // see if aabb and node->aabb have already been tested
- // against each other
- unsigned char mask;
- if (aabb->index <= node->aabb->index) {
- i = (aabb->index * tested_rowsize)+(node->aabb->index >> 3);
- mask = 1 << (node->aabb->index & 7);
- }
- else {
- i = (node->aabb->index * tested_rowsize)+(aabb->index >> 3);
- mask = 1 << (aabb->index & 7);
- }
- dIASSERT (i >= 0 && i < (tested_rowsize*n));
- if ((tested[i] & mask)==0) {
- collideAABBs (aabb->geom,node->aabb->geom,data,callback);
- }
- tested[i] |= mask;
- }
- }
- }
- }
- }
- // get the discrete bounds for the next level up
- for (i=0; i<6; i++) db[i] >>= 1;
- }
- }
-
- // every AABB in the normal list must now be intersected against every
- // AABB in the big_boxes list. so let's hope there are not too many objects
- // in the big_boxes list.
- for (aabb=first_aabb; aabb; aabb=aabb->next) {
- for (dxAABB *aabb2=big_boxes; aabb2; aabb2=aabb2->next) {
- collideAABBs (aabb->geom,aabb2->geom,data,callback);
- }
- }
-
- // intersected all AABBs in the big_boxes list together
- for (aabb=big_boxes; aabb; aabb=aabb->next) {
- for (dxAABB *aabb2=aabb->next; aabb2; aabb2=aabb2->next) {
- collideAABBs (aabb->geom,aabb2->geom,data,callback);
- }
- }
-
- lock_count--;
+ lock_count--;
}
-void dxHashSpace::collide2 (void *data, dxGeom *geom,
- dNearCallback *callback)
+void dxHashSpace::collide(void *cdata, dNearCallback *callback)
{
- dAASSERT (geom && callback);
-
- // this could take advantage of the hash structure to avoid
- // O(n2) complexity, but it does not yet.
-
- lock_count++;
- cleanGeoms();
- geom->recomputeAABB();
-
- // intersect bounding boxes
- for (dxGeom *g=first; g; g=g->next) {
- if (GEOM_ENABLED(g)) collideAABBs (g,geom,data,callback);
- }
-
- lock_count--;
+ dAASSERT(this && callback);
+ dxGeom *geom;
+ dxAABB *aabb;
+ int i, maxlevel;
+
+ // 0 or 1 geoms can't collide with anything
+ if (count < 2) return;
+
+ lock_count++;
+ cleanGeoms();
+
+ // create a list of auxiliary information for all geom axis aligned bounding
+ // boxes. set the level for all AABBs. put AABBs larger than the space's
+ // global_maxlevel in the big_boxes list, check everything else against
+ // that list at the end. for AABBs that are not too big, record the maximum
+ // level that we need.
+
+ int n = 0; // number of AABBs in main list
+ dxAABB *first_aabb = 0; // list of AABBs in hash table
+ dxAABB *big_boxes = 0; // list of AABBs too big for hash table
+ maxlevel = global_minlevel - 1;
+ for (geom = first; geom; geom = geom->next)
+ {
+ if (!GEOM_ENABLED(geom))
+ {
+ continue;
+ }
+ aabb = (dxAABB*)ALLOCA(sizeof(dxAABB));
+ aabb->geom = geom;
+ // compute level, but prevent cells from getting too small
+ int level = findLevel(geom->aabb);
+ if (level < global_minlevel) level = global_minlevel;
+ if (level <= global_maxlevel)
+ {
+ // aabb goes in main list
+ aabb->next = first_aabb;
+ first_aabb = aabb;
+ aabb->level = level;
+ if (level > maxlevel) maxlevel = level;
+ // cellsize = 2^level
+ dReal cellSizeRecip = dRecip((dReal)ldexp(1.0, level));
+ // discretize AABB position to cell size
+ for (i = 0; i < 6; i++) aabb->dbounds[i] = (int)
+ floor(geom->aabb[i] * cellSizeRecip);
+ // set AABB index
+ aabb->index = n;
+ n++;
+ }
+ else
+ {
+ // aabb is too big, put it in the big_boxes list. we don't care about
+ // setting level, dbounds, index, or the maxlevel
+ aabb->next = big_boxes;
+ big_boxes = aabb;
+ }
+ }
+
+ // for `n' objects, an n*n array of bits is used to record if those objects
+ // have been intersection-tested against each other yet. this array can
+ // grow large with high n, but oh well...
+ int tested_rowsize = (n + 7) >> 3; // number of bytes needed for n bits
+ unsigned char *tested = (unsigned char *)ALLOCA(n * tested_rowsize);
+ memset(tested, 0, n * tested_rowsize);
+
+ // create a hash table to store all AABBs. each AABB may take up to 8 cells.
+ // we use chaining to resolve collisions, but we use a relatively large table
+ // to reduce the chance of collisions.
+
+ // compute hash table size sz to be a prime > 8*n
+ for (i = 0; i < NUM_PRIMES; i++) {
+ if (prime[i] >= (8 * n)) break;
+ }
+ if (i >= NUM_PRIMES) i = NUM_PRIMES - 1; // probably pointless
+ const int sz = prime[i];
+
+ // allocate and initialize hash table node pointers
+ Node **table = (Node **)ALLOCA(sizeof(Node*) * sz);
+ for (i = 0; i < sz; i++)
+ table[i] = 0;
+
+ // add each AABB to the hash table (may need to add it to up to 8 cells)
+ for (aabb = first_aabb; aabb; aabb = aabb->next)
+ {
+ const int *dbounds = aabb->dbounds;
+ for (int xi = dbounds[0]; xi <= dbounds[1]; xi++)
+ {
+ for (int yi = dbounds[2]; yi <= dbounds[3]; yi++)
+ {
+ for (int zi = dbounds[4]; zi <= dbounds[5]; zi++)
+ {
+ // get the hash index
+ unsigned long hi = getVirtualAddress(aabb->level, xi, yi, zi) % sz;
+ // add a new node to the hash table
+ Node *node = (Node*)ALLOCA(sizeof(Node));
+ node->x = xi;
+ node->y = yi;
+ node->z = zi;
+ node->aabb = aabb;
+ node->next = table[hi];
+ table[hi] = node;
+ }
+ }
+ }
+ }
+
+ // now that all AABBs are loaded into the hash table, we do the actual
+ // collision detection. for all AABBs, check for other AABBs in the
+ // same cells for collisions, and then check for other AABBs in all
+ // intersecting higher level cells.
+
+ int db[6]; // discrete bounds at current level
+ for (aabb = first_aabb; aabb; aabb = aabb->next) {
+ // we are searching for collisions with aabb
+ for (i = 0; i < 6; i++) db[i] = aabb->dbounds[i];
+ for (int level = aabb->level; level <= maxlevel; level++) {
+ for (int xi = db[0]; xi <= db[1]; xi++) {
+ for (int yi = db[2]; yi <= db[3]; yi++) {
+ for (int zi = db[4]; zi <= db[5]; zi++) {
+ // get the hash index
+ unsigned long hi = getVirtualAddress(level, xi, yi, zi) % sz;
+ // search all nodes at this index
+ Node *node;
+ for (node = table[hi]; node; node = node->next) {
+ // node points to an AABB that may intersect aabb
+ if (node->aabb == aabb) continue;
+ if (node->aabb->level == level &&
+ node->x == xi && node->y == yi && node->z == zi) {
+ // see if aabb and node->aabb have already been tested
+ // against each other
+ unsigned char mask;
+ if (aabb->index <= node->aabb->index) {
+ i = (aabb->index * tested_rowsize) + (node->aabb->index >> 3);
+ mask = 1 << (node->aabb->index & 7);
+ }
+ else {
+ i = (node->aabb->index * tested_rowsize) + (aabb->index >> 3);
+ mask = 1 << (aabb->index & 7);
+ }
+ dIASSERT(i >= 0 && i < (tested_rowsize*n));
+ if ((tested[i] & mask) == 0) {
+ if (testCollideAABBs(aabb->geom, node->aabb->geom))
+ callback(cdata, aabb->geom, node->aabb->geom);
+ }
+ tested[i] |= mask;
+ }
+ }
+ }
+ }
+ }
+ // get the discrete bounds for the next level up
+ for (i = 0; i < 6; i++) db[i] >>= 1;
+ }
+ }
+
+ // every AABB in the normal list must now be intersected against every
+ // AABB in the big_boxes list. so let's hope there are not too many objects
+ // in the big_boxes list.
+ for (aabb = first_aabb; aabb; aabb = aabb->next)
+ {
+ for (dxAABB *aabb2 = big_boxes; aabb2; aabb2 = aabb2->next)
+ {
+ if (testCollideAABBs(aabb->geom, aabb2->geom))
+ callback(data, aabb->geom, aabb2->geom);
+ }
+ }
+
+ // intersected all AABBs in the big_boxes list together
+ for (aabb = big_boxes; aabb; aabb = aabb->next)
+ {
+ for (dxAABB *aabb2 = aabb->next; aabb2; aabb2 = aabb2->next)
+ {
+ if (testCollideAABBs(aabb->geom, aabb2->geom))
+ callback(data, aabb->geom, aabb2->geom);
+ }
+ }
+
+ lock_count--;
+}
+
+
+void dxHashSpace::collide2(void *cdata, dxGeom *geom, dNearCallback *callback)
+{
+ dAASSERT(geom && callback);
+
+ // this could take advantage of the hash structure to avoid
+ // O(n2) complexity, but it does not yet.
+
+ lock_count++;
+ cleanGeoms();
+ geom->recomputeAABB();
+
+ // intersect bounding boxes
+ for (dxGeom *g = first; g; g = g->next) {
+ if (GEOM_ENABLED(g) && testCollideAABBs(g, geom))
+ callback(cdata, g, geom);
+ }
+
+ lock_count--;
}
//****************************************************************************
// space functions
-dxSpace *dSimpleSpaceCreate (dxSpace *space)
+dxSpace *dSimpleSpaceCreate(dxSpace *space)
{
- return new dxSimpleSpace (space);
+ return new dxSimpleSpace(space);
}
-dxSpace *dHashSpaceCreate (dxSpace *space)
+dxSpace *dHashSpaceCreate(dxSpace *space)
{
- return new dxHashSpace (space);
+ return new dxHashSpace(space);
}
-void dHashSpaceSetLevels (dxSpace *space, int minlevel, int maxlevel)
+void dHashSpaceSetLevels(dxSpace *space, int minlevel, int maxlevel)
{
- dAASSERT (space);
- dUASSERT (minlevel <= maxlevel,"must have minlevel <= maxlevel");
- dUASSERT (space->type == dHashSpaceClass,"argument must be a hash space");
- dxHashSpace *hspace = (dxHashSpace*) space;
- hspace->setLevels (minlevel,maxlevel);
+ dAASSERT(space);
+ dUASSERT(minlevel <= maxlevel, "must have minlevel <= maxlevel");
+ dUASSERT(space->type == dHashSpaceClass, "argument must be a hash space");
+ dxHashSpace *hspace = (dxHashSpace*)space;
+ hspace->setLevels(minlevel, maxlevel);
}
-void dHashSpaceGetLevels (dxSpace *space, int *minlevel, int *maxlevel)
+void dHashSpaceGetLevels(dxSpace *space, int *minlevel, int *maxlevel)
{
- dAASSERT (space);
- dUASSERT (space->type == dHashSpaceClass,"argument must be a hash space");
- dxHashSpace *hspace = (dxHashSpace*) space;
- hspace->getLevels (minlevel,maxlevel);
+ dAASSERT(space);
+ dUASSERT(space->type == dHashSpaceClass, "argument must be a hash space");
+ dxHashSpace *hspace = (dxHashSpace*)space;
+ hspace->getLevels(minlevel, maxlevel);
}
-void dSpaceDestroy (dxSpace *space)
+void dSpaceDestroy(dxSpace *space)
{
- dAASSERT (space);
- dUASSERT (dGeomIsSpace(space),"argument not a space");
- dGeomDestroy (space);
+ dAASSERT(space);
+ dUASSERT(dGeomIsSpace(space), "argument not a space");
+ dGeomDestroy(space);
}
-void dSpaceSetCleanup (dxSpace *space, int mode)
+void dSpaceSetCleanup(dxSpace *space, int mode)
{
- dAASSERT (space);
- dUASSERT (dGeomIsSpace(space),"argument not a space");
- space->setCleanup (mode);
+ dAASSERT(space);
+ dUASSERT(dGeomIsSpace(space), "argument not a space");
+ space->setCleanup(mode);
}
-int dSpaceGetCleanup (dxSpace *space)
+int dSpaceGetCleanup(dxSpace *space)
{
- dAASSERT (space);
- dUASSERT (dGeomIsSpace(space),"argument not a space");
- return space->getCleanup();
+ dAASSERT(space);
+ dUASSERT(dGeomIsSpace(space), "argument not a space");
+ return space->getCleanup();
}
-void dSpaceSetSublevel (dSpaceID space, int sublevel)
+void dSpaceSetSublevel(dSpaceID space, int sublevel)
{
- dAASSERT (space);
- dUASSERT (dGeomIsSpace(space),"argument not a space");
- space->setSublevel (sublevel);
+ dAASSERT(space);
+ dUASSERT(dGeomIsSpace(space), "argument not a space");
+ space->setSublevel(sublevel);
}
-int dSpaceGetSublevel (dSpaceID space)
+int dSpaceGetSublevel(dSpaceID space)
{
- dAASSERT (space);
- dUASSERT (dGeomIsSpace(space),"argument not a space");
- return space->getSublevel();
+ dAASSERT(space);
+ dUASSERT(dGeomIsSpace(space), "argument not a space");
+ return space->getSublevel();
}
-void dSpaceSetManualCleanup (dSpaceID space, int mode)
+void dSpaceSetManualCleanup(dSpaceID space, int mode)
{
- dAASSERT (space);
- dUASSERT (dGeomIsSpace(space),"argument not a space");
- space->setManulCleanup(mode);
+ dAASSERT(space);
+ dUASSERT(dGeomIsSpace(space), "argument not a space");
+ space->setManulCleanup(mode);
}
-int dSpaceGetManualCleanup (dSpaceID space)
+int dSpaceGetManualCleanup(dSpaceID space)
{
- dAASSERT (space);
- dUASSERT (dGeomIsSpace(space),"argument not a space");
- return space->getManualCleanup();
+ dAASSERT(space);
+ dUASSERT(dGeomIsSpace(space), "argument not a space");
+ return space->getManualCleanup();
}
-void dSpaceAdd (dxSpace *space, dxGeom *g)
+void dSpaceAdd(dxSpace *space, dxGeom *g)
{
- dAASSERT (space);
- dUASSERT (dGeomIsSpace(space),"argument not a space");
- CHECK_NOT_LOCKED (space);
- space->add (g);
+ dAASSERT(space);
+ dUASSERT(dGeomIsSpace(space), "argument not a space");
+ CHECK_NOT_LOCKED(space);
+ space->add(g);
}
-void dSpaceRemove (dxSpace *space, dxGeom *g)
+void dSpaceRemove(dxSpace *space, dxGeom *g)
{
- dAASSERT (space);
- dUASSERT (dGeomIsSpace(space),"argument not a space");
- CHECK_NOT_LOCKED (space);
- space->remove (g);
+ dAASSERT(space);
+ dUASSERT(dGeomIsSpace(space), "argument not a space");
+ CHECK_NOT_LOCKED(space);
+ space->remove(g);
}
-int dSpaceQuery (dxSpace *space, dxGeom *g)
+int dSpaceQuery(dxSpace *space, dxGeom *g)
{
- dAASSERT (space);
- dUASSERT (dGeomIsSpace(space),"argument not a space");
- return space->query (g);
+ dAASSERT(space);
+ dUASSERT(dGeomIsSpace(space), "argument not a space");
+ return space->query(g);
}
-void dSpaceClean (dxSpace *space){
- dAASSERT (space);
- dUASSERT (dGeomIsSpace(space),"argument not a space");
+void dSpaceClean(dxSpace *space) {
+ dAASSERT(space);
+ dUASSERT(dGeomIsSpace(space), "argument not a space");
- space->cleanGeoms();
+ space->cleanGeoms();
}
-int dSpaceGetNumGeoms (dxSpace *space)
+int dSpaceGetNumGeoms(dxSpace *space)
{
- dAASSERT (space);
- dUASSERT (dGeomIsSpace(space),"argument not a space");
- return space->getNumGeoms();
+ dAASSERT(space);
+ dUASSERT(dGeomIsSpace(space), "argument not a space");
+ return space->getNumGeoms();
}
-dGeomID dSpaceGetGeom (dxSpace *space, int i)
+dGeomID dSpaceGetGeom(dxSpace *space, int i)
{
- dAASSERT (space);
- dUASSERT (dGeomIsSpace(space),"argument not a space");
- return space->getGeom (i);
+ dAASSERT(space);
+ dUASSERT(dGeomIsSpace(space), "argument not a space");
+ return space->getGeom(i);
}
-int dSpaceGetClass (dxSpace *space)
+int dSpaceGetClass(dxSpace *space)
{
- dAASSERT (space);
- dUASSERT (dGeomIsSpace(space),"argument not a space");
- return space->type;
+ dAASSERT(space);
+ dUASSERT(dGeomIsSpace(space), "argument not a space");
+ return space->type;
}
-void dSpaceCollide (dxSpace *space, void *data, dNearCallback *callback)
+void dSpaceCollide(dxSpace *space, void *data, dNearCallback *callback)
{
- dAASSERT (space && callback);
- dUASSERT (dGeomIsSpace(space),"argument not a space");
- space->collide (data,callback);
+ dAASSERT(space && callback);
+ dUASSERT(dGeomIsSpace(space), "argument not a space");
+ space->collide(data, callback);
}
struct DataCallback {
- void *data;
- dNearCallback *callback;
+ void *data;
+ dNearCallback *callback;
};
// Invokes the callback with arguments swapped
static void swap_callback(void *data, dxGeom *g1, dxGeom *g2)
{
- DataCallback *dc = (DataCallback*)data;
- dc->callback(dc->data, g2, g1);
+ DataCallback *dc = (DataCallback*)data;
+ dc->callback(dc->data, g2, g1);
}
-void dSpaceCollide2 (dxGeom *g1, dxGeom *g2, void *data,
- dNearCallback *callback)
+void dSpaceCollide2(dxGeom *g1, dxGeom *g2, void *data, dNearCallback *callback)
{
- dAASSERT (g1 && g2 && callback);
- dxSpace *s1,*s2;
+ dAASSERT(g1 && g2 && callback);
+ dxSpace *s1, *s2;
- // see if either geom is a space
- if (IS_SPACE(g1)) s1 = (dxSpace*) g1; else s1 = 0;
- if (IS_SPACE(g2)) s2 = (dxSpace*) g2; else s2 = 0;
+ // see if either geom is a space
+ if (IS_SPACE(g1)) s1 = (dxSpace*)g1; else s1 = 0;
+ if (IS_SPACE(g2)) s2 = (dxSpace*)g2; else s2 = 0;
- if (s1 && s2) {
- int l1 = s1->getSublevel();
- int l2 = s2->getSublevel();
- if (l1 != l2) {
- if (l1 > l2) {
- s2 = 0;
- } else {
- s1 = 0;
- }
- }
- }
+ if (s1 && s2)
+ {
+ int l1 = s1->getSublevel();
+ int l2 = s2->getSublevel();
+ if (l1 != l2)
+ {
+ if (l1 > l2)
+ {
+ s2 = 0;
+ }
+ else
+ {
+ s1 = 0;
+ }
+ }
+ }
- // handle the four space/geom cases
- if (s1) {
- if (s2) {
- // g1 and g2 are spaces.
- if (s1==s2) {
- // collide a space with itself --> interior collision
- s1->collide (data,callback);
- }
- else {
- // iterate through the space that has the fewest geoms, calling
- // collide2 in the other space for each one.
- if (s1->count < s2->count) {
- DataCallback dc = {data, callback};
- for (dxGeom *g = s1->first; g; g=g->next) {
- s2->collide2 (&dc,g,swap_callback);
- }
- }
- else {
- for (dxGeom *g = s2->first; g; g=g->next) {
- s1->collide2 (data,g,callback);
- }
- }
- }
- }
- else {
- // g1 is a space, g2 is a geom
- s1->collide2 (data,g2,callback);
- }
- }
- else {
- if (s2) {
- // g1 is a geom, g2 is a space
- DataCallback dc = {data, callback};
- s2->collide2 (&dc,g1,swap_callback);
- }
- else {
- // g1 and g2 are geoms
- // make sure they have valid AABBs
- g1->recomputeAABB();
- g2->recomputeAABB();
- collideAABBs(g1,g2, data, callback);
- }
- }
+ // handle the four space/geom cases
+ if (s1)
+ {
+ if (s2)
+ {
+ // g1 and g2 are spaces.
+ if (s1 == s2)
+ {
+ // collide a space with itself --> interior collision
+ s1->collide(data, callback);
+ }
+ else
+ {
+ // iterate through the space that has the fewest geoms, calling
+ // collide2 in the other space for each one.
+ if (s1->count < s2->count)
+ {
+ DataCallback dc = { data, callback };
+ for (dxGeom *g = s1->first; g; g = g->next)
+ {
+ s2->collide2(&dc, g, swap_callback);
+ }
+ }
+ else
+ {
+ for (dxGeom *g = s2->first; g; g = g->next)
+ {
+ s1->collide2(data, g, callback);
+ }
+ }
+ }
+ }
+ else
+ {
+ // g1 is a space, g2 is a geom
+ s1->collide2(data, g2, callback);
+ }
+ }
+ else
+ {
+ if (s2)
+ {
+ // g1 is a geom, g2 is a space
+ DataCallback dc = { data, callback };
+ s2->collide2(&dc, g1, swap_callback);
+ }
+ else
+ {
+ // g1 and g2 are geoms
+ // make sure they have valid AABBs
+ g1->recomputeAABB();
+ g2->recomputeAABB();
+ collideAABBs(g1, g2, data, callback);
+ }
+ }
}
diff --git a/trunk/unmanaged/ubODE-OpenSim/ode/src/collision_space_internal.h b/trunk/unmanaged/ubODE-OpenSim/ode/src/collision_space_internal.h
index f819bfab..a75bfe95 100644
--- a/trunk/unmanaged/ubODE-OpenSim/ode/src/collision_space_internal.h
+++ b/trunk/unmanaged/ubODE-OpenSim/ode/src/collision_space_internal.h
@@ -40,19 +40,19 @@ stuff common to all spaces
// NOTE: this assumes that the geom AABBs are valid on entry
// and that both geoms are enabled.
-static inline void collideAABBs (dxGeom *g1, dxGeom *g2,
+static inline void collideAABBs(dxGeom *g1, dxGeom *g2,
void *data, dNearCallback *callback)
{
dIASSERT((g1->gflags & GEOM_AABB_BAD)==0);
dIASSERT((g2->gflags & GEOM_AABB_BAD)==0);
// no contacts if both geoms on the same body, and the body is not 0
- if (g1->body == g2->body && g1->body) return;
+ if (g1->body && g1->body == g2->body) return;
// test if the category and collide bitfields match
- if ( ((g1->category_bits & g2->collide_bits) ||
- (g2->category_bits & g1->collide_bits)) == 0) {
- return;
+ if ( ((g1->category_bits & g2->collide_bits) || (g2->category_bits & g1->collide_bits)) == 0)
+ {
+ return;
}
// if the bounding boxes are disjoint then don't do anything
@@ -69,11 +69,42 @@ static inline void collideAABBs (dxGeom *g1, dxGeom *g2,
// check if either object is able to prove that it doesn't intersect the
// AABB of the other
- if (g1->AABBTest (g2,bounds2) == 0) return;
- if (g2->AABBTest (g1,bounds1) == 0) return;
+ //if (g1->AABBTest(g2, bounds2) == 0) return;
+ //if (g2->AABBTest(g1, bounds1) == 0) return;
// the objects might actually intersect - call the space callback function
callback (data,g1,g2);
}
+static inline bool testCollideAABBs(dxGeom *g1, dxGeom *g2)
+{
+ dIASSERT((g1->gflags & GEOM_AABB_BAD) == 0);
+ dIASSERT((g2->gflags & GEOM_AABB_BAD) == 0);
+
+ // no contacts if both geoms on the same body, and the body is not 0
+ if (g1->body && g1->body == g2->body) return false;
+
+ // test if the category and collide bitfields match
+ if (((g1->category_bits & g2->collide_bits) || (g2->category_bits & g1->collide_bits)) == 0)
+ {
+ return false;
+ }
+
+ // if the bounding boxes are disjoint then don't do anything
+ dReal *bounds1 = g1->aabb;
+ dReal *bounds2 = g2->aabb;
+ if (bounds1[0] > bounds2[1] ||
+ bounds1[1] < bounds2[0] ||
+ bounds1[2] > bounds2[3] ||
+ bounds1[3] < bounds2[2] ||
+ bounds1[4] > bounds2[5] ||
+ bounds1[5] < bounds2[4]) return false;
+
+ // check if either object is able to prove that it doesn't intersect the
+ // AABB of the other
+ //if (g1->AABBTest(g2, bounds2) == 0) return false;
+ //if (g2->AABBTest(g1, bounds1) == 0) return false;
+ return true;
+}
+
#endif
diff --git a/trunk/unmanaged/ubODE-OpenSim/ode/src/joints/amotor.cpp b/trunk/unmanaged/ubODE-OpenSim/ode/src/joints/amotor.cpp
index 8531a66d..56a352e5 100644
--- a/trunk/unmanaged/ubODE-OpenSim/ode/src/joints/amotor.cpp
+++ b/trunk/unmanaged/ubODE-OpenSim/ode/src/joints/amotor.cpp
@@ -474,8 +474,8 @@ dReal dJointGetAMotorAngleRate( dJointID j, int anum )
if (joint->node[0].body) {
dVector3 axis;
dJointGetAMotorAxis (joint, anum, axis);
- dReal rate = dDOT(axis,joint->node[0].body->avel);
- if (joint->node[1].body) rate -= dDOT(axis,joint->node[1].body->avel);
+ dReal rate = dCalcVectorDot3(axis,joint->node[0].body->avel);
+ if (joint->node[1].body) rate -= dCalcVectorDot3(axis,joint->node[1].body->avel);
return rate;
}
return 0;
diff --git a/trunk/unmanaged/ubODE-OpenSim/ode/src/matrix.h b/trunk/unmanaged/ubODE-OpenSim/ode/src/matrix.h
index ed17a292..5e5436e5 100644
--- a/trunk/unmanaged/ubODE-OpenSim/ode/src/matrix.h
+++ b/trunk/unmanaged/ubODE-OpenSim/ode/src/matrix.h
@@ -35,23 +35,23 @@
#ifdef __cplusplus
template
-ODE_INLINE
-void _dSetZero (element_type *a, size_t n)
+ODE_INLINE void _dSetZero (element_type *a, size_t n)
{
element_type *acurr = a;
element_type *const aend = a + n;
- while (acurr != aend) {
+ while (acurr != aend)
+ {
*(acurr++) = 0;
}
}
template
-ODE_INLINE
-void _dSetValue (element_type *a, size_t n, element_type value)
+ODE_INLINE void _dSetValue (element_type *a, size_t n, element_type value)
{
element_type *acurr = a;
element_type *const aend = a + n;
- while (acurr != aend) {
+ while (acurr != aend)
+ {
*(acurr++) = value;
}
}
@@ -59,22 +59,22 @@ void _dSetValue (element_type *a, size_t n, element_type value)
#else // #ifndef __cplusplus
-ODE_PURE_INLINE
-void _dSetZero (dReal *a, size_t n)
+ODE_PURE_INLINE void _dSetZero (dReal *a, size_t n)
{
dReal *acurr = a;
dReal *const aend = a + n;
- while (acurr != aend) {
+ while (acurr != aend)
+ {
*(acurr++) = 0;
}
}
-ODE_PURE_INLINE
-void _dSetValue (dReal *a, size_t n, dReal value)
+ODE_PURE_INLINE void _dSetValue (dReal *a, size_t n, dReal value)
{
dReal *acurr = a;
dReal *const aend = a + n;
- while (acurr != aend) {
+ while (acurr != aend)
+ {
*(acurr++) = value;
}
}