From ef2d075395c98354572360b018a588c45aca830a Mon Sep 17 00:00:00 2001 From: UbitUmarov Date: Sun, 7 Aug 2022 02:47:14 +0100 Subject: [PATCH] more on ubode --- .../ubODE-OpenSim/include/ode/odemath.h | 80 +- .../include/ode/odemath_legacy.h | 136 -- .../unmanaged/ubODE-OpenSim/libccd/src/mpr.c | 13 +- .../unmanaged/ubODE-OpenSim/libccd/src/vec3.c | 13 +- .../ode/src/collision_kernel.cpp | 6 +- .../ubODE-OpenSim/ode/src/collision_kernel.h | 25 +- .../ode/src/collision_quadtreespace.cpp | 156 +-- .../ode/src/collision_sapspace.cpp | 9 +- .../ubODE-OpenSim/ode/src/collision_space.cpp | 1234 +++++++++-------- .../ode/src/collision_space_internal.h | 45 +- .../ubODE-OpenSim/ode/src/joints/amotor.cpp | 4 +- .../unmanaged/ubODE-OpenSim/ode/src/matrix.h | 24 +- 12 files changed, 862 insertions(+), 883 deletions(-) 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; } }