admin/opensim-libs
1
0
Fork 0
mirror of git://opensimulator.org/git/opensim-libs synced 2026-08-14 00:58:00 +00:00
Code Issues Packages Projects Releases Wiki Activity

more on ubode

Browse Source
This commit is contained in:
UbitUmarov
2022-08-07 02:47:14 +01:00
parent dfb564e6c1
commit ef2d075395
12 changed files with 862 additions and 883 deletions
Download Patch File Download Diff File Expand all files Collapse all files
trunk/unmanaged/ubODE-OpenSim/include/ode/odemath.h
+45 -35
View File
@@ -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
trunk/unmanaged/ubODE-OpenSim/include/ode/odemath_legacy.h
-136
View File
@@ -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
* <PRE>
* for (int i=0; i<3; ++i)
* a[i] += b[i] + c[i];
* </PRE>
*/
#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_ */
trunk/unmanaged/ubODE-OpenSim/libccd/src/mpr.c
+11 -2
View File
@@ -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);
trunk/unmanaged/ubODE-OpenSim/libccd/src/vec3.c
+11 -2
View File
@@ -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)
trunk/unmanaged/ubODE-OpenSim/ode/src/collision_kernel.cpp
+4 -2
View File
@@ -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;
}
trunk/unmanaged/ubODE-OpenSim/ode/src/collision_kernel.h
+16 -9
View File
@@ -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);
}
trunk/unmanaged/ubODE-OpenSim/ode/src/collision_quadtreespace.cpp
+72 -84
View File
@@ -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);
}
trunk/unmanaged/ubODE-OpenSim/ode/src/collision_sapspace.cpp
+5 -4
View File
@@ -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;
}
trunk/unmanaged/ubODE-OpenSim/ode/src/collision_space.cpp
+646 -588
View File
File diff suppressed because it is too large Load Diff
trunk/unmanaged/ubODE-OpenSim/ode/src/collision_space_internal.h
+38 -7
View File
@@ -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
trunk/unmanaged/ubODE-OpenSim/ode/src/joints/amotor.cpp
+2 -2
View File
@@ -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;
trunk/unmanaged/ubODE-OpenSim/ode/src/matrix.h
+12 -12
View File
@@ -35,23 +35,23 @@
#ifdef __cplusplus
template <typename element_type>
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 <typename element_type>
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;
}
}