diff --git a/Externals/ode/ode/src/collision_kernel.cpp b/Externals/ode/ode/src/collision_kernel.cpp index 2e76d4d9d06..1a99392e2f7 100644 --- a/Externals/ode/ode/src/collision_kernel.cpp +++ b/Externals/ode/ode/src/collision_kernel.cpp @@ -313,8 +313,8 @@ void dGeomSetBody (dxGeom *g, dxBody *b) if (b) { if (!g->body) dFree (g->pos,sizeof(dxPosR)); - g->pos = b->pos; - g->R = b->R; + g->pos = b->posr.pos; + g->R = b->posr.R; dGeomMoved (g); if (g->body != b) { g->bodyRemove(); @@ -326,8 +326,8 @@ void dGeomSetBody (dxGeom *g, dxBody *b) dxPosR *pr = (dxPosR*) dAlloc (sizeof(dxPosR)); g->pos = pr->pos; g->R = pr->R; - memcpy (g->pos,g->body->pos,sizeof(dVector3)); - memcpy (g->R,g->body->R,sizeof(dMatrix3)); + memcpy (g->pos,g->body->posr.pos,sizeof(dVector3)); + memcpy (g->R,g->body->posr.R,sizeof(dMatrix3)); g->bodyRemove(); } // dGeomMoved() should not be called if the body is being set to 0, as the diff --git a/Externals/ode/ode/src/collision_kernel.h b/Externals/ode/ode/src/collision_kernel.h index e5f2fb0bf90..c363ec77f5c 100644 --- a/Externals/ode/ode/src/collision_kernel.h +++ b/Externals/ode/ode/src/collision_kernel.h @@ -46,14 +46,6 @@ internal data structures and functions for collision detection. //**************************************************************************** // geometry object base class -// position vector and rotation matrix for geometry objects that are not -// connected to bodies. - -struct dxPosR { - dVector3 pos; - dMatrix3 R; -}; - // geom flags. // diff --git a/Externals/ode/ode/src/collision_quadtreespace.cpp b/Externals/ode/ode/src/collision_quadtreespace.cpp index 6f4a86ee22f..86a1833b163 100644 --- a/Externals/ode/ode/src/collision_quadtreespace.cpp +++ b/Externals/ode/ode/src/collision_quadtreespace.cpp @@ -351,7 +351,7 @@ dxQuadTreeSpace::dxQuadTreeSpace(dSpaceID _space, dVector3 Center, dVector3 Exte int BlockCount = 0; for (int i = 0; i <= Depth; i++){ - BlockCount += (int)powf(SPLITS, i); + BlockCount += (int)pow((dReal)SPLITS, i); } Blocks = (Block*)dAlloc(BlockCount * sizeof(Block)); @@ -384,7 +384,7 @@ dxQuadTreeSpace::~dxQuadTreeSpace(){ int BlockCount = 0; for (int i = 0; i < Depth; i++){ - BlockCount += (int)powf(SPLITS, i); + BlockCount += (int)pow((dReal)SPLITS, i); } dFree(Blocks, BlockCount * sizeof(Block)); @@ -495,7 +495,8 @@ void dxQuadTreeSpace::remove(dxGeom* g){ for (int i = 0; i < DirtyList.size(); i++){ if (DirtyList[i] == g){ DirtyList.remove(i); - break; + // (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; } } diff --git a/Externals/ode/ode/src/error.cpp b/Externals/ode/ode/src/error.cpp index 7250d21126f..9b33db55f0c 100644 --- a/Externals/ode/ode/src/error.cpp +++ b/Externals/ode/ode/src/error.cpp @@ -23,7 +23,6 @@ #include #include -#pragma warning(disable:4996) static dMessageFunction *error_function = 0; static dMessageFunction *debug_function = 0; @@ -119,10 +118,17 @@ extern "C" void dMessage (int num, const char *msg, ...) #ifdef WIN32 +// isn't cygwin annoying! +#ifdef CYGWIN +#define _snprintf snprintf +#define _vsnprintf vsnprintf +#endif + + #include "windows.h" -//#ifdef _DEBUG_ -void _cdecl dError (int num, const char *msg, ...) + +extern "C" void dError (int num, const char *msg, ...) { va_list ap; va_start (ap,msg); @@ -138,29 +144,29 @@ void _cdecl dError (int num, const char *msg, ...) } -void _cdecl dDebug (int num, const char *msg, ...) +extern "C" void dDebug (int num, const char *msg, ...) { va_list ap; va_start (ap,msg); if (debug_function) debug_function (num,msg,ap); else { char s[1000],title[100]; - _snprintf (title,sizeof(title),"ODE INTERNAL ERROR %d",num); - _vsnprintf (s,sizeof(s),msg,ap); + _snprintf (title,sizeof(title),"ODE INTERNAL ERROR %d",num); + _vsnprintf (s,sizeof(s),msg,ap); s[sizeof(s)-1] = 0; MessageBox(0,s,title,MB_OK | MB_ICONSTOP); } abort(); } -void _cdecl dMessage (int num, const char *msg, ...) + +extern "C" void dMessage (int num, const char *msg, ...) { va_list ap; va_start (ap,msg); if (message_function) message_function (num,msg,ap); else printMessage (num,"ODE Message",msg,ap); } -//#endif #endif diff --git a/Externals/ode/ode/src/joint.cpp b/Externals/ode/ode/src/joint.cpp index 17a7b4a8001..2664f194740 100644 --- a/Externals/ode/ode/src/joint.cpp +++ b/Externals/ode/ode/src/joint.cpp @@ -78,13 +78,13 @@ static inline void setBall (dxJoint *joint, dxJoint::Info2 *info, info->J1l[0] = 1; info->J1l[s+1] = 1; info->J1l[2*s+2] = 1; - dMULTIPLY0_331 (a1,joint->node[0].body->R,anchor1); + dMULTIPLY0_331 (a1,joint->node[0].body->posr.R,anchor1); dCROSSMAT (info->J1a,a1,s,-,+); if (joint->node[1].body) { info->J2l[0] = -1; info->J2l[s+1] = -1; info->J2l[2*s+2] = -1; - dMULTIPLY0_331 (a2,joint->node[1].body->R,anchor2); + dMULTIPLY0_331 (a2,joint->node[1].body->posr.R,anchor2); dCROSSMAT (info->J2a,a2,s,+,-); } @@ -92,8 +92,8 @@ static inline void setBall (dxJoint *joint, dxJoint::Info2 *info, dReal k = info->fps * info->erp; if (joint->node[1].body) { for (int j=0; j<3; j++) { - dReal err=a2[j] + joint->node[1].body->pos[j] - - a1[j] - joint->node[0].body->pos[j]; + dReal err=a2[j] + joint->node[1].body->posr.pos[j] - + a1[j] - joint->node[0].body->posr.pos[j]; #ifdef USE_BALL_MIN_ERR add_min_err(err,min_ball_err); #endif @@ -103,7 +103,7 @@ static inline void setBall (dxJoint *joint, dxJoint::Info2 *info, else { for (int j=0; j<3; j++) { info->c[j] = k * (anchor2[j] - a1[j] - - joint->node[0].body->pos[j]); + joint->node[0].body->posr.pos[j]); } } } @@ -132,7 +132,7 @@ static inline void setBall2 (dxJoint *joint, dxJoint::Info2 *info, for (i=0; i<3; i++) info->J1l[i] = axis[i]; for (i=0; i<3; i++) info->J1l[s+i] = q1[i]; for (i=0; i<3; i++) info->J1l[2*s+i] = q2[i]; - dMULTIPLY0_331 (a1,joint->node[0].body->R,anchor1); + dMULTIPLY0_331 (a1,joint->node[0].body->posr.R,anchor1); dCROSS (info->J1a,=,a1,axis); dCROSS (info->J1a+s,=,a1,q1); dCROSS (info->J1a+2*s,=,a1,q2); @@ -140,7 +140,7 @@ static inline void setBall2 (dxJoint *joint, dxJoint::Info2 *info, for (i=0; i<3; i++) info->J2l[i] = -axis[i]; for (i=0; i<3; i++) info->J2l[s+i] = -q1[i]; for (i=0; i<3; i++) info->J2l[2*s+i] = -q2[i]; - dMULTIPLY0_331 (a2,joint->node[1].body->R,anchor2); + dMULTIPLY0_331 (a2,joint->node[1].body->posr.R,anchor2); dCROSS (info->J2a,= -,a2,axis); dCROSS (info->J2a+s,= -,a2,q1); dCROSS (info->J2a+2*s,= -,a2,q2); @@ -150,9 +150,9 @@ static inline void setBall2 (dxJoint *joint, dxJoint::Info2 *info, dReal k1 = info->fps * erp1; dReal k = info->fps * info->erp; - for (i=0; i<3; i++) a1[i] += joint->node[0].body->pos[i]; + for (i=0; i<3; i++) a1[i] += joint->node[0].body->posr.pos[i]; if (joint->node[1].body) { - for (i=0; i<3; i++) a2[i] += joint->node[1].body->pos[i]; + for (i=0; i<3; i++) a2[i] += joint->node[1].body->posr.pos[i]; info->c[0] = k1 * (dDOT(axis,a2) - dDOT(axis,a1)); info->c[1] = k * (dDOT(q1,a2) - dDOT(q1,a1)); info->c[2] = k * (dDOT(q2,a2) - dDOT(q2,a1)); @@ -212,7 +212,7 @@ static void setFixedOrientation(dxJoint *joint, dxJoint::Info2 *info, dQuaternio qerr[2] = -qerr[2]; qerr[3] = -qerr[3]; } - dMULTIPLY0_331 (e,joint->node[0].body->R,qerr+1); // @@@ bad SIMD padding! + dMULTIPLY0_331 (e,joint->node[0].body->posr.R,qerr+1); // @@@ bad SIMD padding! dReal k = info->fps * info->erp; info->c[start_row] = 2*k * e[0]; info->c[start_row+1] = 2*k * e[1]; @@ -227,17 +227,17 @@ static void setAnchors (dxJoint *j, dReal x, dReal y, dReal z, { if (j->node[0].body) { dReal q[4]; - q[0] = x - j->node[0].body->pos[0]; - q[1] = y - j->node[0].body->pos[1]; - q[2] = z - j->node[0].body->pos[2]; + q[0] = x - j->node[0].body->posr.pos[0]; + q[1] = y - j->node[0].body->posr.pos[1]; + q[2] = z - j->node[0].body->posr.pos[2]; q[3] = 0; - dMULTIPLY1_331 (anchor1,j->node[0].body->R,q); + dMULTIPLY1_331 (anchor1,j->node[0].body->posr.R,q); if (j->node[1].body) { - q[0] = x - j->node[1].body->pos[0]; - q[1] = y - j->node[1].body->pos[1]; - q[2] = z - j->node[1].body->pos[2]; + q[0] = x - j->node[1].body->posr.pos[0]; + q[1] = y - j->node[1].body->posr.pos[1]; + q[2] = z - j->node[1].body->posr.pos[2]; q[3] = 0; - dMULTIPLY1_331 (anchor2,j->node[1].body->R,q); + dMULTIPLY1_331 (anchor2,j->node[1].body->posr.R,q); } else { anchor2[0] = x; @@ -263,12 +263,12 @@ static void setAxes (dxJoint *j, dReal x, dReal y, dReal z, q[3] = 0; dNormalize3 (q); if (axis1) { - dMULTIPLY1_331 (axis1,j->node[0].body->R,q); + dMULTIPLY1_331 (axis1,j->node[0].body->posr.R,q); axis1[3] = 0; } if (axis2) { if (j->node[1].body) { - dMULTIPLY1_331 (axis2,j->node[1].body->R,q); + dMULTIPLY1_331 (axis2,j->node[1].body->posr.R,q); } else { axis2[0] = x; @@ -284,10 +284,10 @@ static void setAxes (dxJoint *j, dReal x, dReal y, dReal z, static void getAnchor (dxJoint *j, dVector3 result, dVector3 anchor1) { if (j->node[0].body) { - dMULTIPLY0_331 (result,j->node[0].body->R,anchor1); - result[0] += j->node[0].body->pos[0]; - result[1] += j->node[0].body->pos[1]; - result[2] += j->node[0].body->pos[2]; + dMULTIPLY0_331 (result,j->node[0].body->posr.R,anchor1); + result[0] += j->node[0].body->posr.pos[0]; + result[1] += j->node[0].body->posr.pos[1]; + result[2] += j->node[0].body->posr.pos[2]; } } @@ -295,10 +295,10 @@ static void getAnchor (dxJoint *j, dVector3 result, dVector3 anchor1) static void getAnchor2 (dxJoint *j, dVector3 result, dVector3 anchor2) { if (j->node[1].body) { - dMULTIPLY0_331 (result,j->node[1].body->R,anchor2); - result[0] += j->node[1].body->pos[0]; - result[1] += j->node[1].body->pos[1]; - result[2] += j->node[1].body->pos[2]; + dMULTIPLY0_331 (result,j->node[1].body->posr.R,anchor2); + result[0] += j->node[1].body->posr.pos[0]; + result[1] += j->node[1].body->posr.pos[1]; + result[2] += j->node[1].body->posr.pos[2]; } else { result[0] = anchor2[0]; @@ -311,7 +311,7 @@ static void getAnchor2 (dxJoint *j, dVector3 result, dVector3 anchor2) static void getAxis (dxJoint *j, dVector3 result, dVector3 axis1) { if (j->node[0].body) { - dMULTIPLY0_331 (result,j->node[0].body->R,axis1); + dMULTIPLY0_331 (result,j->node[0].body->posr.R,axis1); } } @@ -319,7 +319,7 @@ static void getAxis (dxJoint *j, dVector3 result, dVector3 axis1) static void getAxis2 (dxJoint *j, dVector3 result, dVector3 axis2) { if (j->node[1].body) { - dMULTIPLY0_331 (result,j->node[1].body->R,axis2); + dMULTIPLY0_331 (result,j->node[1].body->posr.R,axis2); } else { result[0] = axis2[0]; @@ -535,9 +535,9 @@ int dxJointLimitMotor::addLimot (dxJoint *joint, dVector3 ltd; // Linear Torque Decoupling vector (a torque) if (!rotational && joint->node[1].body) { dVector3 c; - c[0]=REAL(0.5)*(joint->node[1].body->pos[0]-joint->node[0].body->pos[0]); - c[1]=REAL(0.5)*(joint->node[1].body->pos[1]-joint->node[0].body->pos[1]); - c[2]=REAL(0.5)*(joint->node[1].body->pos[2]-joint->node[0].body->pos[2]); + c[0]=REAL(0.5)*(joint->node[1].body->posr.pos[0]-joint->node[0].body->posr.pos[0]); + c[1]=REAL(0.5)*(joint->node[1].body->posr.pos[1]-joint->node[0].body->posr.pos[1]); + c[2]=REAL(0.5)*(joint->node[1].body->posr.pos[2]-joint->node[0].body->posr.pos[2]); dCROSS (ltd,=,c,ax1); info->J1a[srow+0] = ltd[0]; info->J1a[srow+1] = ltd[1]; @@ -784,7 +784,7 @@ static void hingeGetInfo2 (dxJointHinge *joint, dxJoint::Info2 *info) dVector3 ax1; // length 1 joint axis in global coordinates, from 1st body dVector3 p,q; // plane space vectors for ax1 - dMULTIPLY0_331 (ax1,joint->node[0].body->R,joint->axis1); + dMULTIPLY0_331 (ax1,joint->node[0].body->posr.R,joint->axis1); dPlaneSpace (ax1,p,q); int s3=3*info->rowskip; @@ -824,7 +824,7 @@ static void hingeGetInfo2 (dxJointHinge *joint, dxJoint::Info2 *info) dVector3 ax2,b; if (joint->node[1].body) { - dMULTIPLY0_331 (ax2,joint->node[1].body->R,joint->axis2); + dMULTIPLY0_331 (ax2,joint->node[1].body->posr.R,joint->axis2); } else { ax2[0] = joint->axis2[0]; @@ -984,7 +984,7 @@ extern "C" dReal dJointGetHingeAngleRate (dxJointHinge *joint) dUASSERT(joint->vtable == &__dhinge_vtable,"joint is not a Hinge"); if (joint->node[0].body) { dVector3 axis; - dMULTIPLY0_331 (axis,joint->node[0].body->R,joint->axis1); + dMULTIPLY0_331 (axis,joint->node[0].body->posr.R,joint->axis1); dReal rate = dDOT(axis,joint->node[0].body->avel); if (joint->node[1].body) rate -= dDOT(axis,joint->node[1].body->avel); if (joint->flags & dJOINT_REVERSE) rate = - rate; @@ -1046,16 +1046,16 @@ extern "C" dReal dJointGetSliderPosition (dxJointSlider *joint) // get axis1 in global coordinates dVector3 ax1,q; - dMULTIPLY0_331 (ax1,joint->node[0].body->R,joint->axis1); + dMULTIPLY0_331 (ax1,joint->node[0].body->posr.R,joint->axis1); if (joint->node[1].body) { // get body2 + offset point in global coordinates - dMULTIPLY0_331 (q,joint->node[1].body->R,joint->offset); - for (int i=0; i<3; i++) q[i] = joint->node[0].body->pos[i] - q[i] - - joint->node[1].body->pos[i]; + dMULTIPLY0_331 (q,joint->node[1].body->posr.R,joint->offset); + for (int i=0; i<3; i++) q[i] = joint->node[0].body->posr.pos[i] - q[i] - + joint->node[1].body->posr.pos[i]; } else { - for (int i=0; i<3; i++) q[i] = joint->node[0].body->pos[i] - + for (int i=0; i<3; i++) q[i] = joint->node[0].body->posr.pos[i] - joint->offset[i]; } @@ -1070,7 +1070,7 @@ extern "C" dReal dJointGetSliderPositionRate (dxJointSlider *joint) // get axis1 in global coordinates dVector3 ax1; - dMULTIPLY0_331 (ax1,joint->node[0].body->R,joint->axis1); + dMULTIPLY0_331 (ax1,joint->node[0].body->posr.R,joint->axis1); if (joint->node[1].body) { return dDOT(ax1,joint->node[0].body->lvel) - @@ -1121,11 +1121,11 @@ static void sliderGetInfo2 (dxJointSlider *joint, dxJoint::Info2 *info) dReal *pos1,*pos2,*R1,*R2; dVector3 c; - pos1 = joint->node[0].body->pos; - R1 = joint->node[0].body->R; + pos1 = joint->node[0].body->posr.pos; + R1 = joint->node[0].body->posr.R; if (joint->node[1].body) { - pos2 = joint->node[1].body->pos; - R2 = joint->node[1].body->R; + pos2 = joint->node[1].body->posr.pos; + R2 = joint->node[1].body->posr.R; for (i=0; i<3; i++) c[i] = pos2[i] - pos1[i]; } else { @@ -1195,14 +1195,14 @@ extern "C" void dJointSetSliderAxis (dxJointSlider *joint, dQMultiply1 (joint->qrel,joint->node[0].body->q,joint->node[1].body->q); dVector3 c; for (i=0; i<3; i++) - c[i] = joint->node[0].body->pos[i] - joint->node[1].body->pos[i]; - dMULTIPLY1_331 (joint->offset,joint->node[1].body->R,c); + c[i] = joint->node[0].body->posr.pos[i] - joint->node[1].body->posr.pos[i]; + dMULTIPLY1_331 (joint->offset,joint->node[1].body->posr.R,c); } else { // set joint->qrel to the transpose of the first body's q joint->qrel[0] = joint->node[0].body->q[0]; for (i=1; i<4; i++) joint->qrel[i] = -joint->node[0].body->q[i]; - for (i=0; i<3; i++) joint->offset[i] = joint->node[0].body->pos[i]; + for (i=0; i<3; i++) joint->offset[i] = joint->node[0].body->posr.pos[i]; } } @@ -1327,7 +1327,7 @@ static void contactGetInfo2 (dxJointContact *j, dxJoint::Info2 *info) // c1,c2 = contact points with respect to body PORs dVector3 c1,c2; - for (i=0; i<3; i++) c1[i] = j->contact.geom.pos[i] - j->node[0].body->pos[i]; + for (i=0; i<3; i++) c1[i] = j->contact.geom.pos[i] - j->node[0].body->posr.pos[i]; // set jacobian for normal info->J1l[0] = normal[0]; @@ -1336,7 +1336,7 @@ static void contactGetInfo2 (dxJointContact *j, dxJoint::Info2 *info) dCROSS (info->J1a,=,c1,normal); if (j->node[1].body) { for (i=0; i<3; i++) c2[i] = j->contact.geom.pos[i] - - j->node[1].body->pos[i]; + j->node[1].body->posr.pos[i]; info->J2l[0] = -normal[0]; info->J2l[1] = -normal[1]; info->J2l[2] = -normal[2]; @@ -1485,8 +1485,8 @@ dxJoint::Vtable __dcontact_special_vtable = { static dReal measureHinge2Angle (dxJointHinge2 *joint) { dVector3 a1,a2; - dMULTIPLY0_331 (a1,joint->node[1].body->R,joint->axis2); - dMULTIPLY1_331 (a2,joint->node[0].body->R,a1); + dMULTIPLY0_331 (a1,joint->node[1].body->posr.R,joint->axis2); + dMULTIPLY1_331 (a2,joint->node[0].body->posr.R,a1); dReal x = dDOT(joint->v1,a2); dReal y = dDOT(joint->v2,a2); return -dAtan2 (y,x); @@ -1547,8 +1547,8 @@ static void hinge2GetInfo1 (dxJointHinge2 *j, dxJoint::Info1 *info) #define HINGE2_GET_AXIS_INFO(axis,sin_angle,cos_angle) \ dVector3 ax1,ax2; \ - dMULTIPLY0_331 (ax1,joint->node[0].body->R,joint->axis1); \ - dMULTIPLY0_331 (ax2,joint->node[1].body->R,joint->axis2); \ + dMULTIPLY0_331 (ax1,joint->node[0].body->posr.R,joint->axis1); \ + dMULTIPLY0_331 (ax2,joint->node[1].body->posr.R,joint->axis2); \ dCROSS (axis,=,ax1,ax2); \ sin_angle = dSqrt (axis[0]*axis[0] + axis[1]*axis[1] + axis[2]*axis[2]); \ cos_angle = dDOT (ax1,ax2); @@ -1613,8 +1613,8 @@ static void makeHinge2V1andV2 (dxJointHinge2 *joint) if (joint->node[0].body) { // get axis 1 and 2 in global coords dVector3 ax1,ax2,v; - dMULTIPLY0_331 (ax1,joint->node[0].body->R,joint->axis1); - dMULTIPLY0_331 (ax2,joint->node[1].body->R,joint->axis2); + dMULTIPLY0_331 (ax1,joint->node[0].body->posr.R,joint->axis1); + dMULTIPLY0_331 (ax2,joint->node[1].body->posr.R,joint->axis2); // don't do anything if the axis1 or axis2 vectors are zero or the same if ((ax1[0]==0 && ax1[1]==0 && ax1[2]==0) || @@ -1628,8 +1628,8 @@ static void makeHinge2V1andV2 (dxJointHinge2 *joint) // make v1 = modified axis2, v2 = axis1 x (modified axis2) dCROSS (v,=,ax1,ax2); - dMULTIPLY1_331 (joint->v1,joint->node[0].body->R,ax2); - dMULTIPLY1_331 (joint->v2,joint->node[0].body->R,v); + dMULTIPLY1_331 (joint->v1,joint->node[0].body->posr.R,ax2); + dMULTIPLY1_331 (joint->v2,joint->node[0].body->posr.R,v); } } @@ -1656,7 +1656,7 @@ extern "C" void dJointSetHinge2Axis1 (dxJointHinge2 *joint, q[2] = z; q[3] = 0; dNormalize3 (q); - dMULTIPLY1_331 (joint->axis1,joint->node[0].body->R,q); + dMULTIPLY1_331 (joint->axis1,joint->node[0].body->posr.R,q); joint->axis1[3] = 0; // compute the sin and cos of the angle between axis 1 and axis 2 @@ -1679,7 +1679,7 @@ extern "C" void dJointSetHinge2Axis2 (dxJointHinge2 *joint, q[2] = z; q[3] = 0; dNormalize3 (q); - dMULTIPLY1_331 (joint->axis2,joint->node[1].body->R,q); + dMULTIPLY1_331 (joint->axis2,joint->node[1].body->posr.R,q); joint->axis1[3] = 0; // compute the sin and cos of the angle between axis 1 and axis 2 @@ -1736,7 +1736,7 @@ extern "C" void dJointGetHinge2Axis1 (dxJointHinge2 *joint, dVector3 result) dUASSERT(result,"bad result argument"); dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2"); if (joint->node[0].body) { - dMULTIPLY0_331 (result,joint->node[0].body->R,joint->axis1); + dMULTIPLY0_331 (result,joint->node[0].body->posr.R,joint->axis1); } } @@ -1747,7 +1747,7 @@ extern "C" void dJointGetHinge2Axis2 (dxJointHinge2 *joint, dVector3 result) dUASSERT(result,"bad result argument"); dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2"); if (joint->node[1].body) { - dMULTIPLY0_331 (result,joint->node[1].body->R,joint->axis2); + dMULTIPLY0_331 (result,joint->node[1].body->posr.R,joint->axis2); } } @@ -1782,7 +1782,7 @@ extern "C" dReal dJointGetHinge2Angle1Rate (dxJointHinge2 *joint) dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2"); if (joint->node[0].body) { dVector3 axis; - dMULTIPLY0_331 (axis,joint->node[0].body->R,joint->axis1); + dMULTIPLY0_331 (axis,joint->node[0].body->posr.R,joint->axis1); dReal rate = dDOT(axis,joint->node[0].body->avel); if (joint->node[1].body) rate -= dDOT(axis,joint->node[1].body->avel); return rate; @@ -1797,7 +1797,7 @@ extern "C" dReal dJointGetHinge2Angle2Rate (dxJointHinge2 *joint) dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2"); if (joint->node[0].body && joint->node[1].body) { dVector3 axis; - dMULTIPLY0_331 (axis,joint->node[1].body->R,joint->axis2); + dMULTIPLY0_331 (axis,joint->node[1].body->posr.R,joint->axis2); dReal rate = dDOT(axis,joint->node[0].body->avel); if (joint->node[1].body) rate -= dDOT(axis,joint->node[1].body->avel); return rate; @@ -1813,8 +1813,8 @@ extern "C" void dJointAddHinge2Torques (dxJointHinge2 *joint, dReal torque1, dRe dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2"); if (joint->node[0].body && joint->node[1].body) { - dMULTIPLY0_331 (axis1,joint->node[0].body->R,joint->axis1); - dMULTIPLY0_331 (axis2,joint->node[1].body->R,joint->axis2); + dMULTIPLY0_331 (axis1,joint->node[0].body->posr.R,joint->axis1); + dMULTIPLY0_331 (axis2,joint->node[1].body->posr.R,joint->axis2); axis1[0] = axis1[0] * torque1 + axis2[0] * torque2; axis1[1] = axis1[1] * torque1 + axis2[1] * torque2; axis1[2] = axis1[2] * torque1 + axis2[2] * torque2; @@ -1860,11 +1860,11 @@ static void universalInit (dxJointUniversal *j) static void getUniversalAxes(dxJointUniversal *joint, dVector3 ax1, dVector3 ax2) { - // This says "ax1 = joint->node[0].body->R * joint->axis1" - dMULTIPLY0_331 (ax1,joint->node[0].body->R,joint->axis1); + // This says "ax1 = joint->node[0].body->posr.R * joint->axis1" + dMULTIPLY0_331 (ax1,joint->node[0].body->posr.R,joint->axis1); if (joint->node[1].body) { - dMULTIPLY0_331 (ax2,joint->node[1].body->R,joint->axis2); + dMULTIPLY0_331 (ax2,joint->node[1].body->posr.R,joint->axis2); } else { ax2[0] = joint->axis2[0]; @@ -2321,9 +2321,9 @@ static void amotorComputeGlobalAxes (dxJointAMotor *joint, dVector3 ax[3]) { if (joint->mode == dAMotorEuler) { // special handling for euler mode - dMULTIPLY0_331 (ax[0],joint->node[0].body->R,joint->axis[0]); + dMULTIPLY0_331 (ax[0],joint->node[0].body->posr.R,joint->axis[0]); if (joint->node[1].body) { - dMULTIPLY0_331 (ax[2],joint->node[1].body->R,joint->axis[2]); + dMULTIPLY0_331 (ax[2],joint->node[1].body->posr.R,joint->axis[2]); } else { ax[2][0] = joint->axis[2][0]; @@ -2337,12 +2337,12 @@ static void amotorComputeGlobalAxes (dxJointAMotor *joint, dVector3 ax[3]) for (int i=0; i < joint->num; i++) { if (joint->rel[i] == 1) { // relative to b1 - dMULTIPLY0_331 (ax[i],joint->node[0].body->R,joint->axis[i]); + dMULTIPLY0_331 (ax[i],joint->node[0].body->posr.R,joint->axis[i]); } if (joint->rel[i] == 2) { // relative to b2 dIASSERT(joint->node[1].body); - dMULTIPLY0_331 (ax[i],joint->node[1].body->R,joint->axis[i]); + dMULTIPLY0_331 (ax[i],joint->node[1].body->posr.R,joint->axis[i]); } else { // global - just copy it @@ -2369,9 +2369,9 @@ static void amotorComputeEulerAngles (dxJointAMotor *joint, dVector3 ax[3]) // calculate references in global frame dVector3 ref1,ref2; - dMULTIPLY0_331 (ref1,joint->node[0].body->R,joint->reference1); + dMULTIPLY0_331 (ref1,joint->node[0].body->posr.R,joint->reference1); if (joint->node[1].body) { - dMULTIPLY0_331 (ref2,joint->node[1].body->R,joint->reference2); + dMULTIPLY0_331 (ref2,joint->node[1].body->posr.R,joint->reference2); } else { ref2[0] = joint->reference2[0]; @@ -2405,14 +2405,14 @@ static void amotorSetEulerReferenceVectors (dxJointAMotor *j) { if (j->node[0].body && j->node[1].body) { dVector3 r; // axis[2] and axis[0] in global coordinates - dMULTIPLY0_331 (r,j->node[1].body->R,j->axis[2]); - dMULTIPLY1_331 (j->reference1,j->node[0].body->R,r); - dMULTIPLY0_331 (r,j->node[0].body->R,j->axis[0]); - dMULTIPLY1_331 (j->reference2,j->node[1].body->R,r); + dMULTIPLY0_331 (r,j->node[1].body->posr.R,j->axis[2]); + dMULTIPLY1_331 (j->reference1,j->node[0].body->posr.R,r); + dMULTIPLY0_331 (r,j->node[0].body->posr.R,j->axis[0]); + dMULTIPLY1_331 (j->reference2,j->node[1].body->posr.R,r); } else if (j->node[0].body) { - dMULTIPLY1_331 (j->reference1,j->node[0].body->R,j->axis[2]); - dMULTIPLY0_331 (j->reference2,j->node[0].body->R,j->axis[0]); + dMULTIPLY1_331 (j->reference1,j->node[0].body->posr.R,j->axis[2]); + dMULTIPLY0_331 (j->reference2,j->node[0].body->posr.R,j->axis[0]); } } @@ -2522,11 +2522,11 @@ extern "C" void dJointSetAMotorAxis (dxJointAMotor *joint, int anum, int rel, r[3] = 0; if (rel > 0) { if (rel==1) { - dMULTIPLY1_331 (joint->axis[anum],joint->node[0].body->R,r); + dMULTIPLY1_331 (joint->axis[anum],joint->node[0].body->posr.R,r); } else { dIASSERT (joint->node[1].body); - dMULTIPLY1_331 (joint->axis[anum],joint->node[1].body->R,r); + dMULTIPLY1_331 (joint->axis[anum],joint->node[1].body->posr.R,r); } } else { @@ -2594,10 +2594,10 @@ extern "C" void dJointGetAMotorAxis (dxJointAMotor *joint, int anum, if (anum > 2) anum = 2; if (joint->rel[anum] > 0) { if (joint->rel[anum]==1) { - dMULTIPLY0_331 (result,joint->node[0].body->R,joint->axis[anum]); + dMULTIPLY0_331 (result,joint->node[0].body->posr.R,joint->axis[anum]); } else { - dMULTIPLY0_331 (result,joint->node[1].body->R,joint->axis[anum]); + dMULTIPLY0_331 (result,joint->node[1].body->posr.R,joint->axis[anum]); } } else { @@ -2730,7 +2730,7 @@ static void fixedGetInfo2 (dxJointFixed *joint, dxJoint::Info2 *info) info->J1l[2*s+2] = 1; dVector3 ofs; - dMULTIPLY0_331 (ofs,joint->node[0].body->R,joint->offset); + dMULTIPLY0_331 (ofs,joint->node[0].body->posr.R,joint->offset); if (joint->node[1].body) { dCROSSMAT (info->J1a,ofs,s,+,-); info->J2l[0] = -1; @@ -2742,12 +2742,12 @@ static void fixedGetInfo2 (dxJointFixed *joint, dxJoint::Info2 *info) dReal k = info->fps * info->erp; if (joint->node[1].body) { for (int j=0; j<3; j++) - info->c[j] = k * (joint->node[1].body->pos[j] - - joint->node[0].body->pos[j] + ofs[j]); + info->c[j] = k * (joint->node[1].body->posr.pos[j] - + joint->node[0].body->posr.pos[j] + ofs[j]); } else { for (int j=0; j<3; j++) - info->c[j] = k * (joint->offset[j] - joint->node[0].body->pos[j]); + info->c[j] = k * (joint->offset[j] - joint->node[0].body->posr.pos[j]); } } @@ -2765,15 +2765,15 @@ extern "C" void dJointSetFixed (dxJointFixed *joint) if (joint->node[1].body) { dQMultiply1 (joint->qrel,joint->node[0].body->q,joint->node[1].body->q); dReal ofs[4]; - for (i=0; i<4; i++) ofs[i] = joint->node[0].body->pos[i]; - for (i=0; i<4; i++) ofs[i] -= joint->node[1].body->pos[i]; - dMULTIPLY1_331 (joint->offset,joint->node[0].body->R,ofs); + for (i=0; i<4; i++) ofs[i] = joint->node[0].body->posr.pos[i]; + for (i=0; i<4; i++) ofs[i] -= joint->node[1].body->posr.pos[i]; + dMULTIPLY1_331 (joint->offset,joint->node[0].body->posr.R,ofs); } else { // set joint->qrel to the transpose of the first body's q joint->qrel[0] = joint->node[0].body->q[0]; for (i=1; i<4; i++) joint->qrel[i] = -joint->node[0].body->q[i]; - for (i=0; i<4; i++) joint->offset[i] = joint->node[0].body->pos[i]; + for (i=0; i<4; i++) joint->offset[i] = joint->node[0].body->posr.pos[i]; } } } @@ -2791,15 +2791,15 @@ extern "C" void dJointSetFixedQuaternionPos (dxJointFixed *joint,dQuaternion qua if (joint->node[1].body) { /* dQMultiply1 (joint->qrel,joint->node[0].body->q,joint->node[1].body->q); dReal ofs[4]; - for (i=0; i<4; i++) ofs[i] = joint->node[0].body->pos[i]; - for (i=0; i<4; i++) ofs[i] -= joint->node[1].body->pos[i]; - dMULTIPLY1_331 (joint->offset,joint->node[0].body->R,ofs);*/ + for (i=0; i<4; i++) ofs[i] = joint->node[0].body->posr.pos[i]; + for (i=0; i<4; i++) ofs[i] -= joint->node[1].body->posr.pos[i]; + dMULTIPLY1_331 (joint->offset,joint->node[0].body->posr.R,ofs);*/ } else { // set joint->qrel to the transpose of the first body's q joint->qrel[0] = quaternion[0];//joint->node[0].body->q[0]; for (i=1; i<4; i++) joint->qrel[i] = -quaternion[i];//-joint->node[0].body->q[i]; - for (i=0; i<4; i++) joint->offset[i] = pos[i];//joint->node[0].body->pos[i]; + for (i=0; i<4; i++) joint->offset[i] = pos[i];//joint->node[0].body->posr.pos[i]; } } } diff --git a/Externals/ode/ode/src/objects.h b/Externals/ode/ode/src/objects.h index 21c353bb74a..02c469a9a2b 100644 --- a/Externals/ode/ode/src/objects.h +++ b/Externals/ode/ode/src/objects.h @@ -87,6 +87,15 @@ struct dxContactParameters { }; + +// position vector and rotation matrix for geometry objects that are not +// connected to bodies. + +struct dxPosR { + dVector3 pos; + dMatrix3 R; +}; + struct dxBody : public dObject { dxJointNode *firstjoint; // list of attached joints int flags; // some dxBodyFlagXXX flags @@ -94,9 +103,8 @@ struct dxBody : public dObject { dMass mass; // mass parameters about POR dMatrix3 invI; // inverse of mass.I dReal invMass; // 1 / mass.mass - dVector3 pos; // position of POR (point of reference) + dxPosR posr; // position and orientation of point of reference dQuaternion q; // orientation quaternion - dMatrix3 R; // rotation matrix, always corresponds to q dVector3 lvel,avel; // linear and angular velocity of POR dVector3 facc,tacc; // force and torque accumulators dVector3 finite_rot_axis; // finite rotation axis, unit length or 0=none diff --git a/Externals/ode/ode/src/ode.cpp b/Externals/ode/ode/src/ode.cpp index 87f23246e06..0c70ad223de 100644 --- a/Externals/ode/ode/src/ode.cpp +++ b/Externals/ode/ode/src/ode.cpp @@ -262,10 +262,10 @@ dxBody *dBodyCreate (dxWorld *w) b->invI[5] = 1; b->invI[10] = 1; b->invMass = 1; - dSetZero (b->pos,4); + dSetZero (b->posr.pos,4); dSetZero (b->q,4); b->q[0] = 1; - dRSetIdentity (b->R); + dRSetIdentity (b->posr.R); dSetZero (b->lvel,4); dSetZero (b->avel,4); dSetZero (b->facc,4); @@ -328,9 +328,9 @@ void *dBodyGetData (dBodyID b) void dBodySetPosition (dBodyID b, dReal x, dReal y, dReal z) { dAASSERT (b); - b->pos[0] = x; - b->pos[1] = y; - b->pos[2] = z; + b->posr.pos[0] = x; + b->posr.pos[1] = y; + b->posr.pos[2] = z; // notify all attached geoms that this body has moved for (dxGeom *geom = b->geom; geom; geom = dGeomGetBodyNext (geom)) @@ -348,7 +348,7 @@ void dBodySetRotation (dBodyID b, const dMatrix3 R) b->q[1] = q[1]; b->q[2] = q[2]; b->q[3] = q[3]; - dQtoR (b->q,b->R); + dQtoR (b->q,b->posr.R); // notify all attached geoms that this body has moved for (dxGeom *geom = b->geom; geom; geom = dGeomGetBodyNext (geom)) @@ -364,7 +364,7 @@ void dBodySetQuaternion (dBodyID b, const dQuaternion q) b->q[2] = q[2]; b->q[3] = q[3]; dNormalize4 (b->q); - dQtoR (b->q,b->R); + dQtoR (b->q,b->posr.R); // notify all attached geoms that this body has moved for (dxGeom *geom = b->geom; geom; geom = dGeomGetBodyNext (geom)) @@ -393,14 +393,14 @@ void dBodySetAngularVel (dBodyID b, dReal x, dReal y, dReal z) const dReal * dBodyGetPosition (dBodyID b) { dAASSERT (b); - return b->pos; + return b->posr.pos; } const dReal * dBodyGetRotation (dBodyID b) { dAASSERT (b); - return b->R; + return b->posr.R; } @@ -470,7 +470,7 @@ void dBodyAddRelForce (dBodyID b, dReal fx, dReal fy, dReal fz) t1[1] = fy; t1[2] = fz; t1[3] = 0; - dMULTIPLY0_331 (t2,b->R,t1); + dMULTIPLY0_331 (t2,b->posr.R,t1); b->facc[0] += t2[0]; b->facc[1] += t2[1]; b->facc[2] += t2[2]; @@ -485,7 +485,7 @@ void dBodyAddRelTorque (dBodyID b, dReal fx, dReal fy, dReal fz) t1[1] = fy; t1[2] = fz; t1[3] = 0; - dMULTIPLY0_331 (t2,b->R,t1); + dMULTIPLY0_331 (t2,b->posr.R,t1); b->tacc[0] += t2[0]; b->tacc[1] += t2[1]; b->tacc[2] += t2[2]; @@ -503,9 +503,9 @@ void dBodyAddForceAtPos (dBodyID b, dReal fx, dReal fy, dReal fz, f[0] = fx; f[1] = fy; f[2] = fz; - q[0] = px - b->pos[0]; - q[1] = py - b->pos[1]; - q[2] = pz - b->pos[2]; + q[0] = px - b->posr.pos[0]; + q[1] = py - b->posr.pos[1]; + q[2] = pz - b->posr.pos[2]; dCROSS (b->tacc,+=,q,f); } @@ -523,7 +523,7 @@ void dBodyAddForceAtRelPos (dBodyID b, dReal fx, dReal fy, dReal fz, prel[1] = py; prel[2] = pz; prel[3] = 0; - dMULTIPLY0_331 (p,b->R,prel); + dMULTIPLY0_331 (p,b->posr.R,prel); b->facc[0] += f[0]; b->facc[1] += f[1]; b->facc[2] += f[2]; @@ -540,14 +540,14 @@ void dBodyAddRelForceAtPos (dBodyID b, dReal fx, dReal fy, dReal fz, frel[1] = fy; frel[2] = fz; frel[3] = 0; - dMULTIPLY0_331 (f,b->R,frel); + dMULTIPLY0_331 (f,b->posr.R,frel); b->facc[0] += f[0]; b->facc[1] += f[1]; b->facc[2] += f[2]; dVector3 q; - q[0] = px - b->pos[0]; - q[1] = py - b->pos[1]; - q[2] = pz - b->pos[2]; + q[0] = px - b->posr.pos[0]; + q[1] = py - b->posr.pos[1]; + q[2] = pz - b->posr.pos[2]; dCROSS (b->tacc,+=,q,f); } @@ -565,8 +565,8 @@ void dBodyAddRelForceAtRelPos (dBodyID b, dReal fx, dReal fy, dReal fz, prel[1] = py; prel[2] = pz; prel[3] = 0; - dMULTIPLY0_331 (f,b->R,frel); - dMULTIPLY0_331 (p,b->R,prel); + dMULTIPLY0_331 (f,b->posr.R,frel); + dMULTIPLY0_331 (p,b->posr.R,prel); b->facc[0] += f[0]; b->facc[1] += f[1]; b->facc[2] += f[2]; @@ -615,10 +615,10 @@ void dBodyGetRelPointPos (dBodyID b, dReal px, dReal py, dReal pz, prel[1] = py; prel[2] = pz; prel[3] = 0; - dMULTIPLY0_331 (p,b->R,prel); - result[0] = p[0] + b->pos[0]; - result[1] = p[1] + b->pos[1]; - result[2] = p[2] + b->pos[2]; + dMULTIPLY0_331 (p,b->posr.R,prel); + result[0] = p[0] + b->posr.pos[0]; + result[1] = p[1] + b->posr.pos[1]; + result[2] = p[2] + b->posr.pos[2]; } @@ -631,7 +631,7 @@ void dBodyGetRelPointVel (dBodyID b, dReal px, dReal py, dReal pz, prel[1] = py; prel[2] = pz; prel[3] = 0; - dMULTIPLY0_331 (p,b->R,prel); + dMULTIPLY0_331 (p,b->posr.R,prel); result[0] = b->lvel[0]; result[1] = b->lvel[1]; result[2] = b->lvel[2]; @@ -644,9 +644,9 @@ void dBodyGetPointVel (dBodyID b, dReal px, dReal py, dReal pz, { dAASSERT (b); dVector3 p; - p[0] = px - b->pos[0]; - p[1] = py - b->pos[1]; - p[2] = pz - b->pos[2]; + p[0] = px - b->posr.pos[0]; + p[1] = py - b->posr.pos[1]; + p[2] = pz - b->posr.pos[2]; p[3] = 0; result[0] = b->lvel[0]; result[1] = b->lvel[1]; @@ -660,11 +660,11 @@ void dBodyGetPosRelPoint (dBodyID b, dReal px, dReal py, dReal pz, { dAASSERT (b); dVector3 prel; - prel[0] = px - b->pos[0]; - prel[1] = py - b->pos[1]; - prel[2] = pz - b->pos[2]; + prel[0] = px - b->posr.pos[0]; + prel[1] = py - b->posr.pos[1]; + prel[2] = pz - b->posr.pos[2]; prel[3] = 0; - dMULTIPLY1_331 (result,b->R,prel); + dMULTIPLY1_331 (result,b->posr.R,prel); } @@ -677,7 +677,7 @@ void dBodyVectorToWorld (dBodyID b, dReal px, dReal py, dReal pz, p[1] = py; p[2] = pz; p[3] = 0; - dMULTIPLY0_331 (result,b->R,p); + dMULTIPLY0_331 (result,b->posr.R,p); } @@ -690,7 +690,7 @@ void dBodyVectorFromWorld (dBodyID b, dReal px, dReal py, dReal pz, p[1] = py; p[2] = pz; p[3] = 0; - dMULTIPLY1_331 (result,b->R,p); + dMULTIPLY1_331 (result,b->posr.R,p); } diff --git a/Externals/ode/ode/src/quickstep.cpp b/Externals/ode/ode/src/quickstep.cpp index d23daf4438d..d9fb0abb6ee 100644 --- a/Externals/ode/ode/src/quickstep.cpp +++ b/Externals/ode/ode/src/quickstep.cpp @@ -786,11 +786,11 @@ void dxQuickStepper (dxWorld *world, dxBody * const *body, int nb, dIASSERT(dValid(b->tacc[0])&&dValid(b->tacc[1])&&dValid(b->tacc[2])); #endif // compute inertia tensor in global frame - dMULTIPLY2_333 (tmp,b->mass.I,body[i]->R); - dMULTIPLY0_333 (I+i*12,b->R,tmp); + dMULTIPLY2_333 (tmp,b->mass.I,body[i]->posr.R); + dMULTIPLY0_333 (I+i*12,b->posr.R,tmp); // compute inverse inertia tensor in global frame - dMULTIPLY2_333 (tmp,b->invI,b->R); - dMULTIPLY0_333 (invI+i*12,b->R,tmp); + dMULTIPLY2_333 (tmp,b->invI,b->posr.R); + dMULTIPLY0_333 (invI+i*12,b->posr.R,tmp); // compute rotational force dMULTIPLY0_331 (tmp,I+i*12,b->avel); diff --git a/Externals/ode/ode/src/step.cpp b/Externals/ode/ode/src/step.cpp index 84ab6c69614..1bbf6e0d5e5 100644 --- a/Externals/ode/ode/src/step.cpp +++ b/Externals/ode/ode/src/step.cpp @@ -243,11 +243,11 @@ void dInternalStepIsland_x1 (dxWorld *world, dxBody * const *body, int nb, for (i=0; imass.I,body[i]->R); - dMULTIPLY0_333 (I+i*12,body[i]->R,tmp); + dMULTIPLY2_333 (tmp,body[i]->mass.I,body[i]->posr.R); + dMULTIPLY0_333 (I+i*12,body[i]->posr.R,tmp); // compute inverse inertia tensor in global frame - dMULTIPLY2_333 (tmp,body[i]->invI,body[i]->R); - dMULTIPLY0_333 (invI+i*12,body[i]->R,tmp); + dMULTIPLY2_333 (tmp,body[i]->invI,body[i]->posr.R); + dMULTIPLY0_333 (invI+i*12,body[i]->posr.R,tmp); // compute rotational force dMULTIPLY0_331 (tmp,I+i*12,body[i]->avel); dCROSS (body[i]->tacc,-=,body[i]->avel,tmp); @@ -555,11 +555,11 @@ void dInternalStepIsland_x2 (dxWorld *world, dxBody * const *body, int nb, for (i=0; imass.I,body[i]->R); - dMULTIPLY0_333 (I+i*12,body[i]->R,tmp); + dMULTIPLY2_333 (tmp,body[i]->mass.I,body[i]->posr.R); + dMULTIPLY0_333 (I+i*12,body[i]->posr.R,tmp); // compute inverse inertia tensor in global frame - dMULTIPLY2_333 (tmp,body[i]->invI,body[i]->R); - dMULTIPLY0_333 (invI+i*12,body[i]->R,tmp); + dMULTIPLY2_333 (tmp,body[i]->invI,body[i]->posr.R); + dMULTIPLY0_333 (invI+i*12,body[i]->posr.R,tmp); // compute rotational force dMULTIPLY0_331 (tmp,I+i*12,body[i]->avel); dCROSS (body[i]->tacc,-=,body[i]->avel,tmp); diff --git a/Externals/ode/ode/src/stepfast.cpp b/Externals/ode/ode/src/stepfast.cpp index 1818aa03cde..eb30dba46a6 100644 --- a/Externals/ode/ode/src/stepfast.cpp +++ b/Externals/ode/ode/src/stepfast.cpp @@ -250,7 +250,7 @@ moveAndRotateBody (dxBody * b, dReal h) // handle linear velocity for (j = 0; j < 3; j++) - b->pos[j] += h * b->lvel[j]; + b->posr.pos[j] += h * b->lvel[j]; if (b->flags & dxBodyFlagFiniteRotation) { @@ -319,7 +319,7 @@ moveAndRotateBody (dxBody * b, dReal h) // normalize the quaternion and convert it to a rotation matrix dNormalize4 (b->q); - dQtoR (b->q, b->R); + dQtoR (b->q, b->posr.R); // notify all attached geoms that this body has moved for (dxGeom * geom = b->geom; geom; geom = dGeomGetBodyNext (geom)) @@ -461,8 +461,8 @@ dInternalStepFast (dxWorld * world, dxBody * body[2], dReal * GI[2], dReal * Gin dSolveLCP (m, A, lambda, rhs, residual, nub, lo, hi, Jinfo.findex); #endif - //����� ��� LCP - solver replacement �� ������� � ���� Source control - //������ ����� �� �� ����������� + //здесь был LCP - solver replacement он остался в базе Source control + //скорее всего он не понадобится // compute the constraint force `cforce' # ifdef TIMING @@ -609,59 +609,59 @@ dInternalStepIslandFast (dxWorld * world, dxBody * const *bodies, int nb, dxJoin if (m) { - // create a constraint equation right hand side vector `c', a constraint - // force mixing vector `cfm', and LCP low and high bound vectors, and an - // 'findex' vector. + // create a constraint equation right hand side vector `c', a constraint + // force mixing vector `cfm', and LCP low and high bound vectors, and an + // 'findex' vector. c = (dReal *) ALLOCA (m * sizeof (dReal)); cfm = (dReal *) ALLOCA (m * sizeof (dReal)); lo = (dReal *) ALLOCA (m * sizeof (dReal)); hi = (dReal *) ALLOCA (m * sizeof (dReal)); findex = (int *) ALLOCA (m * sizeof (int)); - dSetZero (c, m); - dSetValue (cfm, m, world->global_cfm); - dSetValue (lo, m, -dInfinity); - dSetValue (hi, m, dInfinity); - - - // get jacobian data from constraints. a (2*m)x8 matrix will be created - // to store the two jacobian blocks from each constraint. it has this - // format: - // - // l l l 0 a a a 0 \ . - // l l l 0 a a a 0 }-- jacobian body 1 block for joint 0 (3 rows) - // l l l 0 a a a 0 / - // l l l 0 a a a 0 \ . - // l l l 0 a a a 0 }-- jacobian body 2 block for joint 0 (3 rows) - // l l l 0 a a a 0 / - // l l l 0 a a a 0 }--- jacobian body 1 block for joint 1 (1 row) - // l l l 0 a a a 0 }--- jacobian body 2 block for joint 1 (1 row) - // etc... - // - // (lll) = linear jacobian data - // (aaa) = angular jacobian data - // + dSetZero (c, m); + dSetValue (cfm, m, world->global_cfm); + dSetValue (lo, m, -dInfinity); + dSetValue (hi, m, dInfinity); + + + // get jacobian data from constraints. a (2*m)x8 matrix will be created + // to store the two jacobian blocks from each constraint. it has this + // format: + // + // l l l 0 a a a 0 \ . + // l l l 0 a a a 0 }-- jacobian body 1 block for joint 0 (3 rows) + // l l l 0 a a a 0 / + // l l l 0 a a a 0 \ . + // l l l 0 a a a 0 }-- jacobian body 2 block for joint 0 (3 rows) + // l l l 0 a a a 0 / + // l l l 0 a a a 0 }--- jacobian body 1 block for joint 1 (1 row) + // l l l 0 a a a 0 }--- jacobian body 2 block for joint 1 (1 row) + // etc... + // + // (lll) = linear jacobian data + // (aaa) = angular jacobian data + // # ifdef TIMING - dTimerNow ("create J"); + dTimerNow ("create J"); # endif J = (dReal *) ALLOCA (2 * m * 8 * sizeof (dReal)); dSetZero (J, 2 * m * 8); Jinfo = (dxJoint::Info2 *) ALLOCA (nj * sizeof (dxJoint::Info2)); - for (i = 0; i < nj; i++) - { - Jinfo[i].rowskip = 8; - Jinfo[i].fps = dRecip (stepsize); - Jinfo[i].erp = world->global_erp; - Jinfo[i].J1l = J + 2 * 8 * ofs[i]; - Jinfo[i].J1a = Jinfo[i].J1l + 4; - Jinfo[i].J2l = Jinfo[i].J1l + 8 * info[i].m; - Jinfo[i].J2a = Jinfo[i].J2l + 4; - Jinfo[i].c = c + ofs[i]; - Jinfo[i].cfm = cfm + ofs[i]; - Jinfo[i].lo = lo + ofs[i]; - Jinfo[i].hi = hi + ofs[i]; - Jinfo[i].findex = findex + ofs[i]; - //joints[i]->vtable->getInfo2 (joints[i], Jinfo+i); - } + for (i = 0; i < nj; i++) + { + Jinfo[i].rowskip = 8; + Jinfo[i].fps = dRecip (stepsize); + Jinfo[i].erp = world->global_erp; + Jinfo[i].J1l = J + 2 * 8 * ofs[i]; + Jinfo[i].J1a = Jinfo[i].J1l + 4; + Jinfo[i].J2l = Jinfo[i].J1l + 8 * info[i].m; + Jinfo[i].J2a = Jinfo[i].J2l + 4; + Jinfo[i].c = c + ofs[i]; + Jinfo[i].cfm = cfm + ofs[i]; + Jinfo[i].lo = lo + ofs[i]; + Jinfo[i].hi = hi + ofs[i]; + Jinfo[i].findex = findex + ofs[i]; + //joints[i]->vtable->getInfo2 (joints[i], Jinfo+i); + } } @@ -675,8 +675,8 @@ dInternalStepIslandFast (dxWorld * world, dxBody * const *bodies, int nb, dxJoin { saveFacc[b * 4 + i] = bodies[b]->facc[i]; saveTacc[b * 4 + i] = bodies[b]->tacc[i]; - bodies[b]->tag = b; } + bodies[b]->tag = b; } for (iter = 0; iter < maxiterations; iter++) @@ -698,11 +698,11 @@ dInternalStepIslandFast (dxWorld * world, dxBody * const *bodies, int nb, dxJoin // @@@ check computation of rotational force. // compute inertia tensor in global frame - dMULTIPLY2_333 (tmp, body->mass.I, body->R); - dMULTIPLY0_333 (globalI + b * 12, body->R, tmp); + dMULTIPLY2_333 (tmp, body->mass.I, body->posr.R); + dMULTIPLY0_333 (globalI + b * 12, body->posr.R, tmp); // compute inverse inertia tensor in global frame - dMULTIPLY2_333 (tmp, body->invI, body->R); - dMULTIPLY0_333 (globalInvI + b * 12, body->R, tmp); + dMULTIPLY2_333 (tmp, body->invI, body->posr.R); + dMULTIPLY0_333 (globalInvI + b * 12, body->posr.R, tmp); for (i = 0; i < 4; i++) body->tacc[i] = saveTacc[b * 4 + i]; @@ -747,7 +747,6 @@ dInternalStepIslandFast (dxWorld * world, dxBody * const *bodies, int nb, dxJoin //now iterate through the random ordered joint array we created. for (j = 0; j < nj; j++) { - #ifdef TIMING dTimerNow ("setting up joint"); #endif @@ -776,7 +775,7 @@ dInternalStepIslandFast (dxWorld * world, dxBody * const *bodies, int nb, dxJoin } joints[j]->vtable->getInfo2 (joints[j], Jinfo + j); - + //dInternalStepIslandFast is an exact copy of the old routine with one //modification: the calculated forces are added back to the facc and tacc //vectors instead of applying them to the bodies and moving them. @@ -807,23 +806,8 @@ dInternalStepIslandFast (dxWorld * world, dxBody * const *bodies, int nb, dxJoin for (i = 0; i < 4; i++) { -#ifdef NOISING - const dReal RAND_MAX_2=16383; - const dReal NOISE_EPSILON=0.000000831f; -#define NOISE() (1.f+(RAND_MAX_2-rand()%RAND_MAX)*NOISE_EPSILON) - dReal noise=NOISE(); - dReal mul=ministep*noise; - body->facc[i] *= mul; - noise=NOISE(); - mul=ministep*noise; - body->tacc[i] *= mul; -#else - body->facc[i] *= ministep; body->tacc[i] *= ministep; - -#endif - } //apply torque @@ -959,8 +943,7 @@ processIslandsFast (dxWorld * world, dReal stepsize, int maxiterations) b = stack[--stacksize]; // pop body off stack autoDepth = autostack[stacksize]; body[bcount++] = b; // put body on body list - -quickstart: + quickstart: // traverse and tag all body's joints, add untagged connected bodies // to stack diff --git a/Externals/ode/ode/src/testing.cpp b/Externals/ode/ode/src/testing.cpp index fb13084d2f1..a22f8657680 100644 --- a/Externals/ode/ode/src/testing.cpp +++ b/Externals/ode/ode/src/testing.cpp @@ -31,7 +31,6 @@ static const dReal tol = 1.0e-9; static const dReal tol = 1.0e-5f; #endif -#ifdef _DEBUG // matrix header on the stack @@ -156,7 +155,7 @@ static void myDebug (int num, const char *msg, va_list ap) } -extern "C" void dTestMatrixComparison() +extern "C" ODE_API void dTestMatrixComparison() { volatile int i; printf ("dTestMatrixComparison()\n"); @@ -242,5 +241,3 @@ extern "C" void dTestMatrixComparison() } dSetDebugHandler (orig_debug); } - -#endif diff --git a/Externals/ode/ode/src/timer.cpp b/Externals/ode/ode/src/timer.cpp index 0d2ef252158..b6110ab5b05 100644 --- a/Externals/ode/ode/src/timer.cpp +++ b/Externals/ode/ode/src/timer.cpp @@ -103,31 +103,54 @@ double dTimerTicksPerSecond() #define PENTIUM_HZ (500e6) - static inline void getClockCount (unsigned long cc[2]) { +#ifndef X86_64_SYSTEM asm volatile ( "rdtsc\n" "movl %%eax,(%%esi)\n" "movl %%edx,4(%%esi)\n" : : "S" (cc) : "%eax","%edx","cc","memory"); +#else + asm volatile ( + "rdtsc\n" + "movl %%eax,(%%rsi)\n" + "movl %%edx,4(%%rsi)\n" + : : "S" (cc) : "%eax","%edx","cc","memory"); +#endif } static inline void serialize() { +#ifndef X86_64_SYSTEM asm volatile ( "mov $0,%%eax\n" + "push %%ebx\n" + "cpuid\n" + "pop %%ebx\n" + : : : "%eax","%ecx","%edx","cc","memory"); +#else + asm volatile ( + "mov $0,%%rax\n" + "push %%rbx\n" "cpuid\n" - : : : "%eax","%ebx","%ecx","%edx","cc","memory"); + "pop %%rbx\n" + : : : "%rax","%rcx","%rdx","cc","memory"); +#endif } static inline double loadClockCount (unsigned long a[2]) { double ret; +#ifndef X86_64_SYSTEM asm volatile ("fildll %1; fstpl %0" : "=m" (ret) : "m" (a[0]) : "cc","memory"); +#else + asm volatile ("fildll %1; fstpl %0" : "=m" (ret) : "m" (a[0]) : + "cc","memory"); +#endif return ret; } @@ -339,7 +362,6 @@ static void fprintDoubleWithPrefix (FILE *f, double a, char *fmt) fprintf (f,"n"); } -#ifdef _DEBUG void dTimerReport (FILE *fout, int average) { int i; @@ -396,4 +418,3 @@ void dTimerReport (FILE *fout, int average) } fprintf (fout,"\n"); } -#endif diff --git a/Externals/ode/ode/src/util.cpp b/Externals/ode/ode/src/util.cpp index 6519f3e6eca..a08bf2ac723 100644 --- a/Externals/ode/ode/src/util.cpp +++ b/Externals/ode/ode/src/util.cpp @@ -98,7 +98,7 @@ void dxStepBody (dxBody *b, dReal h) dIASSERT(dValid(b->avel[0])&&dValid(b->avel[1])&&dValid(b->avel[2])); #endif // handle linear velocity - for (j=0; j<3; j++) b->pos[j] += h * b->lvel[j]; + for (j=0; j<3; j++) b->posr.pos[j] += h * b->lvel[j]; if (b->flags & dxBodyFlagFiniteRotation) { dVector3 irv; // infitesimal rotation vector @@ -160,7 +160,7 @@ dIASSERT(dValid(b->avel[0])&&dValid(b->avel[1])&&dValid(b->avel[2])); // normalize the quaternion and convert it to a rotation matrix dNormalize4 (b->q); - dQtoR (b->q,b->R); + dQtoR (b->q,b->posr.R); // notify all attached geoms that this body has moved for (dxGeom *geom = b->geom; geom; geom = dGeomGetBodyNext (geom)) diff --git a/src/xrPhysics/PHElement.cpp b/src/xrPhysics/PHElement.cpp index dda207e2e8b..d9877c6928f 100644 --- a/src/xrPhysics/PHElement.cpp +++ b/src/xrPhysics/PHElement.cpp @@ -1562,9 +1562,9 @@ static void dBodyGetPointForce(dBodyID b, dReal px, dReal py, dReal pz, dVector3 { VERIFY(b); dVector3 p; - p[0] = px - b->pos[0]; - p[1] = py - b->pos[1]; - p[2] = pz - b->pos[2]; + p[0] = px - b->posr.pos[0]; + p[1] = py - b->posr.pos[1]; + p[2] = pz - b->posr.pos[2]; p[3] = 0; result[0] = b->facc[0]; result[1] = b->facc[1]; diff --git a/src/xrPhysics/PHFracture.cpp b/src/xrPhysics/PHFracture.cpp index bf8834f0665..0e3ffdce638 100644 --- a/src/xrPhysics/PHFracture.cpp +++ b/src/xrPhysics/PHFracture.cpp @@ -461,15 +461,15 @@ bool CPHFracture::Update(CPHElement* element) dMatrix3 glI1, glI2, glInvI, tmp; // compute inertia tensors in global frame - dMULTIPLY2_333(tmp, body->invI, body->R); - dMULTIPLY0_333(glInvI, body->R, tmp); + dMULTIPLY2_333(tmp, body->invI, body->posr.R); + dMULTIPLY0_333(glInvI, body->posr.R, tmp); - dMULTIPLY2_333(tmp, m_firstM.I, body->R); - dMULTIPLY0_333(glI1, body->R, tmp); + dMULTIPLY2_333(tmp, m_firstM.I, body->posr.R); + dMULTIPLY0_333(glI1, body->posr.R, tmp); - dMULTIPLY2_333(tmp, m_secondM.I, body->R); - dMULTIPLY0_333(glI2, body->R, tmp); - // both parts have eqiual start angular vel same as have body so we ignore it + dMULTIPLY2_333(tmp, m_secondM.I, body->posr.R); + dMULTIPLY0_333(glI2, body->posr.R, tmp); + // both parts have equal start angular vel same as have body so we ignore it // compute breaking torque /// break_torque=glI2*glInvI*first_part_torque-glI1*glInvI*second_part_torque+crossproduct(second_in_bone,second_part_force)-crossproduct(first_in_bone,first_part_force) diff --git a/src/xrPhysics/Physics.cpp b/src/xrPhysics/Physics.cpp index feb73243cc8..b06d20bbc34 100644 --- a/src/xrPhysics/Physics.cpp +++ b/src/xrPhysics/Physics.cpp @@ -419,12 +419,12 @@ void BodyCutForce(dBodyID body, float l_limit, float w_limit) dMatrix3 tmp, invI, I; // compute inertia tensor in global frame - dMULTIPLY2_333(tmp, m.I, body->R); - dMULTIPLY0_333(I, body->R, tmp); + dMULTIPLY2_333(tmp, m.I, body->posr.R); + dMULTIPLY0_333(I, body->posr.R, tmp); // compute inverse inertia tensor in global frame - dMULTIPLY2_333(tmp, body->invI, body->R); - dMULTIPLY0_333(invI, body->R, tmp); + dMULTIPLY2_333(tmp, body->invI, body->posr.R); + dMULTIPLY0_333(invI, body->posr.R, tmp); // angular accel dVector3 wa;