int dCollideCapsulePlane (dxGeom *o1, dxGeom *o2, int flags,
dContactGeom *contact, int skip)
{
dIASSERT (skip >= (int)sizeof(dContactGeom));
dIASSERT (o1->type == dCapsuleClass);
dIASSERT (o2->type == dPlaneClass);
dIASSERT ((flags & NUMC_MASK) >= 1);
dxCapsule *ccyl = (dxCapsule*) o1;
dxPlane *plane = (dxPlane*) o2;
// collide the deepest capping sphere with the plane
dReal sign = (dCalcVectorDot3_14 (plane->p,o1->final_posr->R+2) > 0) ? REAL(-1.0) : REAL(1.0);
dVector3 p;
p[0] = o1->final_posr->pos[0] + o1->final_posr->R[2] * ccyl->lz * REAL(0.5) * sign;
p[1] = o1->final_posr->pos[1] + o1->final_posr->R[6] * ccyl->lz * REAL(0.5) * sign;
p[2] = o1->final_posr->pos[2] + o1->final_posr->R[10] * ccyl->lz * REAL(0.5) * sign;
dReal k = dCalcVectorDot3 (p,plane->p);
dReal depth = plane->p[3] - k + ccyl->radius;
if (depth < 0) return 0;
contact->normal[0] = plane->p[0];
contact->normal[1] = plane->p[1];
contact->normal[2] = plane->p[2];
contact->pos[0] = p[0] - plane->p[0] * ccyl->radius;
contact->pos[1] = p[1] - plane->p[1] * ccyl->radius;
contact->pos[2] = p[2] - plane->p[2] * ccyl->radius;
contact->depth = depth;
int ncontacts = 1;
if ((flags & NUMC_MASK) >= 2) {
// collide the other capping sphere with the plane
p[0] = o1->final_posr->pos[0] - o1->final_posr->R[2] * ccyl->lz * REAL(0.5) * sign;
p[1] = o1->final_posr->pos[1] - o1->final_posr->R[6] * ccyl->lz * REAL(0.5) * sign;
p[2] = o1->final_posr->pos[2] - o1->final_posr->R[10] * ccyl->lz * REAL(0.5) * sign;
k = dCalcVectorDot3 (p,plane->p);
depth = plane->p[3] - k + ccyl->radius;
if (depth >= 0) {
dContactGeom *c2 = CONTACT(contact,skip);
c2->normal[0] = plane->p[0];
c2->normal[1] = plane->p[1];
c2->normal[2] = plane->p[2];
c2->pos[0] = p[0] - plane->p[0] * ccyl->radius;
c2->pos[1] = p[1] - plane->p[1] * ccyl->radius;
c2->pos[2] = p[2] - plane->p[2] * ccyl->radius;
c2->depth = depth;
ncontacts = 2;
}
}
for (int i=0; i < ncontacts; i++) {
dContactGeom *currContact = CONTACT(contact,i*skip);
currContact->g1 = o1;
currContact->g2 = o2;
currContact->side1 = -1;
currContact->side2 = -1;
}
return ncontacts;
}
dReal
dxJointHinge2::measureAngle() const
{
dVector3 a1, a2;
dMultiply0_331( a1, node[1].body->posr.R, axis2 );
dMultiply1_331( a2, node[0].body->posr.R, a1 );
dReal x = dCalcVectorDot3( v1, a2 );
dReal y = dCalcVectorDot3( v2, a2 );
return -dAtan2( y, x );
}
int dCollideRayPlane (dxGeom *o1, dxGeom *o2, int flags,
dContactGeom *contact, int skip)
{
dIASSERT (skip >= (int)sizeof(dContactGeom));
dIASSERT (o1->type == dRayClass);
dIASSERT (o2->type == dPlaneClass);
dIASSERT ((flags & NUMC_MASK) >= 1);
dxRay *ray = (dxRay*) o1;
dxPlane *plane = (dxPlane*) o2;
dReal alpha = plane->p[3] - dCalcVectorDot3 (plane->p,ray->final_posr->pos);
// note: if alpha > 0 the starting point is below the plane
dReal nsign = (alpha > 0) ? REAL(-1.0) : REAL(1.0);
dReal k = dCalcVectorDot3_14(plane->p,ray->final_posr->R+2);
if (k==0) return 0; // ray parallel to plane
alpha /= k;
if (alpha < 0 || alpha > ray->length) return 0;
contact->pos[0] = ray->final_posr->pos[0] + alpha*ray->final_posr->R[0*4+2];
contact->pos[1] = ray->final_posr->pos[1] + alpha*ray->final_posr->R[1*4+2];
contact->pos[2] = ray->final_posr->pos[2] + alpha*ray->final_posr->R[2*4+2];
contact->normal[0] = nsign*plane->p[0];
contact->normal[1] = nsign*plane->p[1];
contact->normal[2] = nsign*plane->p[2];
contact->depth = alpha;
contact->g1 = ray;
contact->g2 = plane;
contact->side1 = -1;
contact->side2 = -1;
return 1;
}
void
dxJointHinge2::makeW1andW2()
{
if ( node[1].body )
{
// get axis 1 and 2 in global coords
dVector3 ax1, ax2, w;
dMultiply0_331( ax1, node[0].body->posr.R, axis1 );
dMultiply0_331( ax2, node[1].body->posr.R, 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 ) ||
( ax2[0] == 0 && ax2[1] == 0 && ax2[2] == 0 ) ||
( ax1[0] == ax2[0] && ax1[1] == ax2[1] && ax1[2] == ax2[2] ) ) return;
// modify axis 1 so it's perpendicular to axis 2
dReal k = dCalcVectorDot3( ax2, ax1 );
for ( int i = 0; i < 3; i++ ) ax1[i] -= k * ax2[i];
dNormalize3( ax1 );
// make w1 = modified axis1, w2 = axis2 x (modified axis1)
dCalcVectorCross3( w, ax2, ax1 );
dMultiply1_331( w1, node[1].body->posr.R, ax1 );
dMultiply1_331( w2, node[1].body->posr.R, w );
}
}
dReal dJointGetHinge2Angle2Rate( dJointID j )
{
dxJointHinge2* joint = ( dxJointHinge2* )j;
dUASSERT( joint, "bad joint argument" );
checktype( joint, Hinge2 );
if ( joint->node[0].body && joint->node[1].body )
{
dVector3 axis;
dMultiply0_331( axis, joint->node[1].body->posr.R, joint->axis2 );
dReal rate = dCalcVectorDot3( axis, joint->node[0].body->avel );
if ( joint->node[1].body )
rate -= dCalcVectorDot3( axis, joint->node[1].body->avel );
return rate;
}
else return 0;
}
int dCollideSpherePlane (dxGeom *o1, dxGeom *o2, int flags,
dContactGeom *contact, int skip)
{
dIASSERT (skip >= (int)sizeof(dContactGeom));
dIASSERT (o1->type == dSphereClass);
dIASSERT (o2->type == dPlaneClass);
dIASSERT ((flags & NUMC_MASK) >= 1);
dxSphere *sphere = (dxSphere*) o1;
dxPlane *plane = (dxPlane*) o2;
contact->g1 = o1;
contact->g2 = o2;
contact->side1 = -1;
contact->side2 = -1;
dReal k = dCalcVectorDot3 (o1->final_posr->pos,plane->p);
dReal depth = plane->p[3] - k + sphere->radius;
if (depth >= 0) {
contact->normal[0] = plane->p[0];
contact->normal[1] = plane->p[1];
contact->normal[2] = plane->p[2];
contact->pos[0] = o1->final_posr->pos[0] - plane->p[0] * sphere->radius;
contact->pos[1] = o1->final_posr->pos[1] - plane->p[1] * sphere->radius;
contact->pos[2] = o1->final_posr->pos[2] - plane->p[2] * sphere->radius;
contact->depth = depth;
return 1;
}
else return 0;
}
dReal dJointGetHingeAngleRate( dJointID j )
{
dxJointHinge* joint = ( dxJointHinge* )j;
dAASSERT( joint );
checktype( joint, Hinge );
if ( joint->node[0].body )
{
dVector3 axis;
dMultiply0_331( axis, joint->node[0].body->posr.R, joint->axis1 );
dReal rate = dCalcVectorDot3( axis, joint->node[0].body->avel );
if ( joint->node[1].body ) rate -= dCalcVectorDot3( axis, joint->node[1].body->avel );
if ( joint->flags & dJOINT_REVERSE ) rate = - rate;
return rate;
}
else return 0;
}
void
dxJointHinge2::makeV1andV2()
{
if ( node[0].body )
{
// get axis 1 and 2 in global coords
dVector3 ax1, ax2, v;
dMultiply0_331( ax1, node[0].body->posr.R, axis1 );
dMultiply0_331( ax2, node[1].body->posr.R, axis2 );
// don't do anything if the axis1 or axis2 vectors are zero or the same
if ((_dequal(ax1[0], 0.0) && _dequal(ax1[1], 0.0) && _dequal(ax1[2], 0.0)) ||
(_dequal(ax2[0], 0.0) && _dequal(ax2[1], 0.0) && _dequal(ax2[2], 0.0)) ||
(_dequal(ax1[0], ax2[0]) && _dequal(ax1[1], ax2[1]) && _dequal(ax1[2], ax2[2])))
return;
// modify axis 2 so it's perpendicular to axis 1
dReal k = dCalcVectorDot3( ax1, ax2 );
for ( int i = 0; i < 3; i++ ) ax2[i] -= k * ax1[i];
dNormalize3( ax2 );
// make v1 = modified axis2, v2 = axis1 x (modified axis2)
dCalcVectorCross3( v, ax1, ax2 );
dMultiply1_331( v1, node[0].body->posr.R, ax2 );
dMultiply1_331( v2, node[0].body->posr.R, v );
}
}
void testPlaneSpace()
{
HEADER;
dVector3 n,p,q;
int bad = 0;
for (int i=0; i<1000; i++) {
dMakeRandomVector (n,3,1.0);
dNormalize3 (n);
dPlaneSpace (n,p,q);
if (fabs(dCalcVectorDot3(n,p)) > tol) bad = 1;
if (fabs(dCalcVectorDot3(n,q)) > tol) bad = 1;
if (fabs(dCalcVectorDot3(p,q)) > tol) bad = 1;
if (fabs(dCalcVectorDot3(p,p)-1) > tol) bad = 1;
if (fabs(dCalcVectorDot3(q,q)-1) > tol) bad = 1;
}
printf ("\t%s\n", bad ? "FAILED" : "passed");
}
////////////////////////////////////////////////////////////////////////////////
/// Function that computes ax1,ax2 = axis 1 and 2 in global coordinates (they are
/// relative to body 1 and 2 initially) and then computes the constrained
/// rotational axis as the cross product of ax1 and ax2.
/// the sin and cos of the angle between axis 1 and 2 is computed, this comes
/// from dot and cross product rules.
///
/// @param ax1 Will contain the joint axis1 in world frame
/// @param ax2 Will contain the joint axis2 in world frame
/// @param axis Will contain the cross product of ax1 x ax2
/// @param sin_angle
/// @param cos_angle
////////////////////////////////////////////////////////////////////////////////
void
dxJointHinge2::getAxisInfo(dVector3 ax1, dVector3 ax2, dVector3 axCross,
dReal &sin_angle, dReal &cos_angle) const
{
dMultiply0_331 (ax1, node[0].body->posr.R, axis1);
dMultiply0_331 (ax2, node[1].body->posr.R, axis2);
dCalcVectorCross3(axCross,ax1,ax2);
sin_angle = dSqrt (axCross[0]*axCross[0] + axCross[1]*axCross[1] + axCross[2]*axCross[2]);
cos_angle = dCalcVectorDot3 (ax1,ax2);
}
dReal
dxJointHinge2::measureAngle2() const
{
// bring axis 1 into second body's reference frame
dVector3 p, q;
if (node[0].body)
dMultiply0_331( p, node[0].body->posr.R, axis1 );
else
dCopyVector3r4(p, axis1);
if (node[1].body)
dMultiply1_331( q, node[1].body->posr.R, p );
else
dCopyVector3r4(q, p);
dReal x = dCalcVectorDot3( w1, q );
dReal y = dCalcVectorDot3( w2, q );
return -dAtan2( y, x );
}
dReal
dxJointHinge2::measureAngle1() const
{
// bring axis 2 into first body's reference frame
dVector3 p, q;
if (node[1].body)
dMultiply0_331( p, node[1].body->posr.R, axis2 );
else
dCopyVector3(p, axis2);
if (node[0].body)
dMultiply1_331( q, node[0].body->posr.R, p );
else
dCopyVector3(q, p);
dReal x = dCalcVectorDot3( v1, q );
dReal y = dCalcVectorDot3( v2, q );
return -dAtan2( y, x );
}
dReal dJointGetPRPositionRate( dJointID j )
{
dxJointPR* joint = ( dxJointPR* ) j;
dUASSERT( joint, "bad joint argument" );
checktype( joint, PR );
// get axis1 in global coordinates
dVector3 ax1;
dMultiply0_331( ax1, joint->node[0].body->posr.R, joint->axisP1 );
if ( joint->node[1].body )
{
dVector3 lv2;
dBodyGetRelPointVel( joint->node[1].body, joint->anchor2[0], joint->anchor2[1], joint->anchor2[2], lv2 );
return dCalcVectorDot3( ax1, joint->node[0].body->lvel ) - dCalcVectorDot3( ax1, lv2 );
}
else
{
dReal rate = dCalcVectorDot3( ax1, joint->node[0].body->lvel );
return ( (joint->flags & dJOINT_REVERSE) ? -rate : rate);
}
}
void testNormalize3()
{
HEADER;
int i,j,bad=0;
dVector3 n1,n2;
for (i=0; i<1000; i++) {
dMakeRandomVector (n1,3,1.0);
for (j=0; j<3; j++) n2[j]=n1[j];
dNormalize3 (n2);
if (dFabs(dCalcVectorDot3(n2,n2) - 1.0) > tol) bad |= 1;
if (dFabs(n2[0]/n1[0] - n2[1]/n1[1]) > tol) bad |= 2;
if (dFabs(n2[0]/n1[0] - n2[2]/n1[2]) > tol) bad |= 4;
if (dFabs(n2[1]/n1[1] - n2[2]/n1[2]) > tol) bad |= 8;
if (dFabs(dCalcVectorDot3(n2,n1) - dSqrt(dCalcVectorDot3(n1,n1))) > tol) bad |= 16;
if (bad) {
printf ("\tFAILED (code=%x)\n",bad);
return;
}
}
printf ("\tpassed\n");
}
dReal dJointGetPUAngle2Rate( dJointID j )
{
dxJointPU* joint = ( dxJointPU* ) j;
dUASSERT( joint, "bad joint argument" );
checktype( joint, PU );
if ( joint->node[0].body )
{
dVector3 axis;
if ( joint->flags & dJOINT_REVERSE )
getAxis( joint, axis, joint->axis1 );
else
getAxis2( joint, axis, joint->axis2 );
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;
}
void makeRandomRotation (dMatrix3 R)
{
dReal *u1 = R, *u2=R+4, *u3=R+8;
dMakeRandomVector (u1,3,1.0);
dNormalize3 (u1);
dMakeRandomVector (u2,3,1.0);
dReal d = dCalcVectorDot3(u1,u2);
u2[0] -= d*u1[0];
u2[1] -= d*u1[1];
u2[2] -= d*u1[2];
dNormalize3(u2);
dCalcVectorCross3(u3,u1,u2);
}
dReal dJointGetScrewPositionRate ( dJointID j )
{
dxJointScrew* joint = ( dxJointScrew* ) j;
dUASSERT ( joint, "bad joint argument" );
checktype ( joint, Screw );
// get axis1 in global coordinates
dVector3 ax1;
dMultiply0_331 ( ax1, joint->node[0].body->posr.R, joint->axis1 );
if ( joint->node[1].body )
{
return dCalcVectorDot3 ( ax1, joint->node[0].body->lvel ) -
dCalcVectorDot3 ( ax1, joint->node[1].body->lvel );
}
else
{
dReal rate = dCalcVectorDot3 ( ax1, joint->node[0].body->lvel );
if ( joint->flags & dJOINT_REVERSE ) rate = - rate;
return rate;
}
}
dReal dJointGetPRPosition( dJointID j )
{
dxJointPR* joint = ( dxJointPR* ) j;
dUASSERT( joint, "bad joint argument" );
checktype( joint, PR );
dVector3 q;
// get the offset in global coordinates
dMultiply0_331( q, joint->node[0].body->posr.R, joint->offset );
if ( joint->node[1].body )
{
dVector3 anchor2;
// get the anchor2 in global coordinates
dMultiply0_331( anchor2, joint->node[1].body->posr.R, joint->anchor2 );
q[0] = (( joint->node[0].body->posr.pos[0] + q[0] ) -
( joint->node[1].body->posr.pos[0] + anchor2[0] ) );
q[1] = (( joint->node[0].body->posr.pos[1] + q[1] ) -
( joint->node[1].body->posr.pos[1] + anchor2[1] ) );
q[2] = (( joint->node[0].body->posr.pos[2] + q[2] ) -
( joint->node[1].body->posr.pos[2] + anchor2[2] ) );
}
else
{
//N.B. When there is no body 2 the joint->anchor2 is already in
// global coordinates
q[0] = (( joint->node[0].body->posr.pos[0] + q[0] ) -
( joint->anchor2[0] ) );
q[1] = (( joint->node[0].body->posr.pos[1] + q[1] ) -
( joint->anchor2[1] ) );
q[2] = (( joint->node[0].body->posr.pos[2] + q[2] ) -
( joint->anchor2[2] ) );
if ( joint->flags & dJOINT_REVERSE )
{
q[0] = -q[0];
q[1] = -q[1];
q[2] = -q[2];
}
}
dVector3 axP;
// get prismatic axis in global coordinates
dMultiply0_331( axP, joint->node[0].body->posr.R, joint->axisP1 );
return dCalcVectorDot3( axP, q );
}
// Generate contact info for movement 1
static void contactplat_2(dContact &contact)
{
/*
For arbitrary contact directions we need to project the moving
geom's velocity against the contact normal and fdir1, fdir2
(obtained with dPlaneSpace()). Assuming moving geom=g2
(so the contact joint is in the moving geom's reference frame):
motion1 = dCalcVectorDot3(fdir1, vel);
motion2 = dCalcVectorDot3(fdir2, vel);
motionN = dCalcVectorDot3(normal, vel);
For geom=g1 just negate motionN and motion2. fdir1 is an arbitrary
vector, so there's no need to negate motion1.
*/
contact.surface.mode |=
dContactMotionN | // velocity along normal
dContactMotion1 | dContactMotion2 | // and along the contact plane
dContactFDir1; // don't forget to set the direction 1
// This is a convenience function: given a vector, it finds other 2 perpendicular vectors
dVector3 motiondir1, motiondir2;
dPlaneSpace(contact.geom.normal, motiondir1, motiondir2);
for (int i=0; i<3; ++i)
contact.fdir1[i] = motiondir1[i];
dReal inv = 1;
if (contact.geom.g1 == platform)
inv = -1;
contact.surface.motion1 = dCalcVectorDot3(mov2_vel, motiondir1);
contact.surface.motion2 = inv * dCalcVectorDot3(mov2_vel, motiondir2);
contact.surface.motionN = inv * dCalcVectorDot3(mov2_vel, contact.geom.normal);
}
dReal dJointGetScrewPosition ( dJointID j )
{
dxJointScrew* joint = ( dxJointScrew* ) j;
dUASSERT ( joint, "bad joint argument" );
checktype ( joint, Screw );
// get axis1 in global coordinates
dVector3 ax1, q;
if (!joint->node[0].body)
return 0;
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->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
{
q[0] = joint->node[0].body->posr.pos[0] - joint->offset[0];
q[1] = joint->node[0].body->posr.pos[1] - joint->offset[1];
q[2] = joint->node[0].body->posr.pos[2] - joint->offset[2];
if ( joint->flags & dJOINT_REVERSE )
{
// N.B. it could have been simplier to only inverse the sign of
// the dCalcVectorDot3 result but this case is exceptional and
// doing the check for all case can decrease the performance.
ax1[0] = -ax1[0];
ax1[1] = -ax1[1];
ax1[2] = -ax1[2];
}
}
return dCalcVectorDot3 ( ax1, q );
}
dReal getHingeAngleFromRelativeQuat( dQuaternion qrel, dVector3 axis )
{
// the angle between the two bodies is extracted from the quaternion that
// represents the relative rotation between them. recall that a quaternion
// q is:
// [s,v] = [ cos(theta/2) , sin(theta/2) * u ]
// where s is a scalar and v is a 3-vector. u is a unit length axis and
// theta is a rotation along that axis. we can get theta/2 by:
// theta/2 = atan2 ( sin(theta/2) , cos(theta/2) )
// but we can't get sin(theta/2) directly, only its absolute value, i.e.:
// |v| = |sin(theta/2)| * |u|
// = |sin(theta/2)|
// using this value will have a strange effect. recall that there are two
// quaternion representations of a given rotation, q and -q. typically as
// a body rotates along the axis it will go through a complete cycle using
// one representation and then the next cycle will use the other
// representation. this corresponds to u pointing in the direction of the
// hinge axis and then in the opposite direction. the result is that theta
// will appear to go "backwards" every other cycle. here is a fix: if u
// points "away" from the direction of the hinge (motor) axis (i.e. more
// than 90 degrees) then use -q instead of q. this represents the same
// rotation, but results in the cos(theta/2) value being sign inverted.
// extract the angle from the quaternion. cost2 = cos(theta/2),
// sint2 = |sin(theta/2)|
dReal cost2 = qrel[0];
dReal sint2 = dSqrt( qrel[1] * qrel[1] + qrel[2] * qrel[2] + qrel[3] * qrel[3] );
dReal theta = ( dCalcVectorDot3( qrel + 1, axis ) >= 0 ) ? // @@@ padding assumptions
( 2 * dAtan2( sint2, cost2 ) ) : // if u points in direction of axis
( 2 * dAtan2( sint2, -cost2 ) ); // if u points in opposite direction
// the angle we get will be between 0..2*pi, but we want to return angles
// between -pi..pi
if ( theta > M_PI ) theta -= ( dReal )( 2 * M_PI );
// the angle we've just extracted has the wrong sign
theta = -theta;
return theta;
}
int _dSafeNormalize4 (dVector4 a)
{
dAASSERT (a);
dReal l = dCalcVectorDot3(a,a)+a[3]*a[3];
if (l > 0) {
l = dRecipSqrt(l);
a[0] *= l;
a[1] *= l;
a[2] *= l;
a[3] *= l;
return 1;
}
else {
a[0] = 1;
a[1] = 0;
a[2] = 0;
a[3] = 0;
return 0;
}
}
/*
* This takes what is supposed to be a rotation matrix,
* and make sure it is correct.
* Note: this operates on rows, not columns, because for rotations
* both ways give equivalent results.
*/
void dOrthogonalizeR(dMatrix3 m)
{
dReal n0 = dCalcVectorLengthSquare3(m);
if (n0 != 1)
dSafeNormalize3(m);
// project row[0] on row[1], should be zero
dReal proj = dCalcVectorDot3(m, m+4);
if (proj != 0) {
// Gram-Schmidt step on row[1]
m[4] -= proj * m[0];
m[5] -= proj * m[1];
m[6] -= proj * m[2];
}
dReal n1 = dCalcVectorLengthSquare3(m+4);
if (n1 != 1)
dSafeNormalize3(m+4);
/* just overwrite row[2], this makes sure the matrix is not
a reflection */
dCalcVectorCross3(m+8, m, m+4);
m[3] = m[4+3] = m[8+3] = 0;
}
static int ray_sphere_helper (dxRay *ray, dVector3 sphere_pos, dReal radius,
dContactGeom *contact, int mode)
{
dVector3 q;
q[0] = ray->final_posr->pos[0] - sphere_pos[0];
q[1] = ray->final_posr->pos[1] - sphere_pos[1];
q[2] = ray->final_posr->pos[2] - sphere_pos[2];
dReal B = dCalcVectorDot3_14(q,ray->final_posr->R+2);
dReal C = dCalcVectorDot3(q,q) - radius*radius;
// note: if C <= 0 then the start of the ray is inside the sphere
dReal k = B*B - C;
if (k < 0) return 0;
k = dSqrt(k);
dReal alpha;
if (mode && C >= 0) {
alpha = -B + k;
if (alpha < 0) return 0;
}
else {
alpha = -B - k;
if (alpha < 0) {
alpha = -B + k;
if (alpha < 0) return 0;
}
}
if (alpha > ray->length) return 0;
contact->pos[0] = ray->final_posr->pos[0] + alpha*ray->final_posr->R[0*4+2];
contact->pos[1] = ray->final_posr->pos[1] + alpha*ray->final_posr->R[1*4+2];
contact->pos[2] = ray->final_posr->pos[2] + alpha*ray->final_posr->R[2*4+2];
dReal nsign = (C < 0 || mode) ? REAL(-1.0) : REAL(1.0);
contact->normal[0] = nsign*(contact->pos[0] - sphere_pos[0]);
contact->normal[1] = nsign*(contact->pos[1] - sphere_pos[1]);
contact->normal[2] = nsign*(contact->pos[2] - sphere_pos[2]);
dNormalize3 (contact->normal);
contact->depth = alpha;
return 1;
}
void
dxJointAMotor::computeEulerAngles( dVector3 ax[3] )
{
// assumptions:
// global axes already calculated --> ax
// axis[0] is relative to body 1 --> global ax[0]
// axis[2] is relative to body 2 --> global ax[2]
// ax[1] = ax[2] x ax[0]
// original ax[0] and ax[2] are perpendicular
// reference1 is perpendicular to ax[0] (in body 1 frame)
// reference2 is perpendicular to ax[2] (in body 2 frame)
// all ax[] and reference vectors are unit length
// calculate references in global frame
dVector3 ref1, ref2;
dMultiply0_331( ref1, node[0].body->posr.R, reference1 );
if ( node[1].body )
{
dMultiply0_331( ref2, node[1].body->posr.R, reference2 );
}
else
{
ref2[0] = reference2[0];
ref2[1] = reference2[1];
ref2[2] = reference2[2];
}
// get q perpendicular to both ax[0] and ref1, get first euler angle
dVector3 q;
dCalcVectorCross3( q, ax[0], ref1 );
angle[0] = -dAtan2( dCalcVectorDot3( ax[2], q ), dCalcVectorDot3( ax[2], ref1 ) );
// get q perpendicular to both ax[0] and ax[1], get second euler angle
dCalcVectorCross3( q, ax[0], ax[1] );
angle[1] = -dAtan2( dCalcVectorDot3( ax[2], ax[0] ), dCalcVectorDot3( ax[2], q ) );
// get q perpendicular to both ax[1] and ax[2], get third euler angle
dCalcVectorCross3( q, ax[1], ax[2] );
angle[2] = -dAtan2( dCalcVectorDot3( ref2, ax[1] ), dCalcVectorDot3( ref2, q ) );
}
void
dxJointScrew::getInfo2( dxJoint::Info2 *info )
{
// Added by OSRF
//
// Screw Constraint Overview
//
// make 5 constraint rows.
// given screw axis, first create two orthogonal axis p and q.
// row 1: linear constraint along p
// row 2: linear constraint along q
// row 3: screw constraint about user specified axis
// row 4: rotational constraint about p
// row 5: rotational constraint about q
// Added by OSRF
// If joint values of erp and cfm are negative, then ignore them.
// info->erp, info->cfm already have the global values from quickstep
if (this->erp >= 0)
info->erp = erp;
if (this->cfm >= 0)
{
info->cfm[0] = cfm;
info->cfm[1] = cfm;
info->cfm[2] = cfm;
info->cfm[3] = cfm;
info->cfm[4] = cfm;
}
// constraint rows 1 to 3
{
// pull out pos and R for both bodies. also get the `connection'
// vector pos2-pos1.
dReal *pos1, *pos2, *R1, *R2;
dVector3 cgdiff;
cgdiff[0] = cgdiff[1] = cgdiff[2] = 0;
pos1 = node[0].body->posr.pos;
R1 = node[0].body->posr.R;
if ( node[1].body )
{
pos2 = node[1].body->posr.pos;
R2 = node[1].body->posr.R;
for (int i = 0; i < 3; ++i )
{
// store distance between cg's in cgdiff
cgdiff[i] = pos2[i] - pos1[i];
}
}
else
{
pos2 = 0;
R2 = 0;
}
// compute error for screw due to drift
dReal lin_disp; // linear displacement
dReal lin_err; // linear displacement
{
// get linear disp for screw
// get axis1 in global coordinates
dVector3 ax1, q;
dMultiply0_331 ( ax1, node[0].body->posr.R, axis1 );
if ( node[1].body )
{
// get body2 + offset point in global coordinates
dMultiply0_331 ( q, node[1].body->posr.R, offset );
//printf("debug offset q[%f %f %f] p0[%f %f %f] p1[%f %f %f] \t",
// q[0],q[1],q[2],
// node[0].body->posr.pos[0], node[0].body->posr.pos[1],
// node[0].body->posr.pos[2],
// node[1].body->posr.pos[0], node[1].body->posr.pos[1],
// node[1].body->posr.pos[2]);
for ( int ii = 0; ii < 3; ++ii )
q[ii] = node[0].body->posr.pos[ii] - q[ii] -
node[1].body->posr.pos[ii];
}
else
{
q[0] = node[0].body->posr.pos[0] - offset[0];
q[1] = node[0].body->posr.pos[1] - offset[1];
q[2] = node[0].body->posr.pos[2] - offset[2];
}
lin_disp = dCalcVectorDot3 ( ax1, q );
// linear error should be length scaled, BUT
if (dFabs(thread_pitch) > 1.0)
{
// constraint is written in length scale, so
// linear error is length scaled.
lin_err = -(lin_disp-cumulative_angle/thread_pitch);
}
else
{
// here the entire constraint equation, including lin_err
// is multiplied by thread_pitch for |thread_pitch| less than 1.0
// for added numerical stability,
lin_err = -(thread_pitch*lin_disp-cumulative_angle);
}
// printf("lin disp: %f lin err: %f\n", lin_disp, lin_err);
}
int s0 = 0 * info->rowskip;
int s1 = 1 * info->rowskip;
int s2 = 2 * info->rowskip;
// remaining two rows. we want: vel2 = vel1 + w1 x cgdiff ... but this would
// result in three equations, so we project along the planespace vectors
// so that sliding along the slider axis is disregarded. for symmetry we
// also substitute (w1+w2)/2 for w1, as w1 is supposed to equal w2.
// ax1 is axis1 converted to body1 frame
dVector3 ax1;
dMultiply0_331 ( ax1, R1, axis1 );
// p and q are vectors perpendicular to ax1 in body1 frame
dVector3 p, q;
dPlaneSpace ( ax1, p, q );
// linear constraints for the hinge joint
// perpendicular to the sliding axis direction.
for (int i = 0; i < 3; ++i ) info->J1l[s0+i] = p[i];
for (int i = 0; i < 3; ++i ) info->J1l[s1+i] = q[i];
// if p and q do not pass through body CG's,
// we need to add angular constraints to balance out the forces
// from these linear constraints. See below:
// a1 and a2 are axis vectors in the body frame
// (whereas anchor1 and anchor2 are in world frame).
// anchor1 is the vector from CG to joint anchor in world frame.
dVector3 a1, a2;
dMultiply0_331( a1, R1, anchor1 );
// tmpp is a vector perpendicular to a1 and p in body frame,
// it is the direction of the angular constraint that will
// cancel out moment generated by linear constraint p
// if p does not pass through CG.
{
dVector3 tmpp;
dCalcVectorCross3(tmpp, p, a1);
for (int i = 0; i < 3; ++i ) info->J1a[s0+i] = -tmpp[i];
}
// tmpq is similar to tmpp, but for q.
{
dVector3 tmpq;
dCalcVectorCross3(tmpq, q, a1);
for (int i = 0; i < 3; ++i ) info->J1a[s1+i] = -tmpq[i];
}
// screw constraint:
// now constrain the sliding axis by rotation of the other body
if (dFabs(thread_pitch) > 1.0)
{
for (int i = 0; i < 3; ++i ) info->J1l[s2+i] = ax1[i];
for (int i = 0; i < 3; ++i ) info->J1a[s2+i] = -ax1[i]/thread_pitch;
}
else
{
// here the entire constraint equation, including lin_err
// is multiplied by thread_pitch for |thread_pitch| less than 1.0
// for added numerical stability,
for (int i = 0; i < 3; ++i ) info->J1l[s2+i] = ax1[i]*thread_pitch;
for (int i = 0; i < 3; ++i ) info->J1a[s2+i] = -ax1[i];
}
// repeat above for child body if one exists
if ( node[1].body )
{
// linear constraints for s0 and s1
for (int i = 0; i < 3; ++i ) info->J2l[s0+i] = -p[i];
for (int i = 0; i < 3; ++i ) info->J2l[s1+i] = -q[i];
// angular compensation if p and q do not pass through CG
dMultiply0_331( a2, R2, anchor2 );
dVector3 tmpp;
dCalcVectorCross3(tmpp, p, a2);
for (int i = 0; i < 3; ++i ) info->J2a[s0+i] = tmpp[i];
dVector3 tmpq;
dCalcVectorCross3(tmpq, q, a2);
for (int i = 0; i < 3; ++i ) info->J2a[s1+i] = tmpq[i];
// screw constraint:
// constrain the sliding axis by rotation of the other body
if (dFabs(thread_pitch) > 1.0)
{
for (int i = 0; i < 3; ++i ) info->J2a[s2+i] = ax1[i]/thread_pitch;
for (int i = 0; i < 3; ++i ) info->J2l[s2+i] = -ax1[i];
}
else
{
// here the entire constraint equation, including lin_err
// is multiplied by thread_pitch for |thread_pitch| less than 1.0
// for added numerical stability,
for (int i = 0; i < 3; ++i ) info->J2a[s2+i] = ax1[i];
for (int i = 0; i < 3; ++i ) info->J2l[s2+i] = -ax1[i]*thread_pitch;
}
}
// debug
// printf ("anchor1 %f %f %f\n", anchor1[0], anchor1[1], anchor1[2]);
// printf ("a1 %f %f %f\n", a1[0], a1[1], a1[2]);
// printf ("ax1 %f %f %f\n", ax1[0], ax1[1], ax1[2]);
// printf ("tmpp %f %f %f\n", tmpp[0], tmpp[1], tmpp[2]);
// printf ("p %f %f %f\n", p[0], p[1], p[2]);
// printf ("q %f %f %f\n", q[0], q[1], q[2]);
// printf ("J1a[s0] %f %f %f\n", info->J1a[s0+0], info->J1a[s0+1],
// info->J1a[s0+2]);
// printf ("J1a[s1] %f %f %f\n", info->J1a[s1+0], info->J1a[s1+1],
// info->J1a[s1+2]);
// info->J1a[s0+0] = 1;
// info->J1a[s0+1] = 0;
// info->J1a[s0+2] = 0;
// info->J1a[s1+0] = -1;
// info->J1a[s1+1] = 0;
// info->J1a[s1+2] = 0;
// printf("screw err lin[%f], ang[%f], diff[%f] tp[%f]\n",
// thread_pitch*lin_disp, cumulative_angle, lin_err, thread_pitch);
// compute last two elements of right hand side. we want to align the offset
// point (in body 2's frame) with the center of body 1.
dReal k = info->fps * info->erp;
if ( node[1].body )
{
// dVector3 ofs; // offset point in global coordinates
// dMultiply0_331 ( ofs, R2, offset );
// for (int i = 0; i < 3; ++i ) cgdiff[i] += ofs[i];
// error between body anchors
dVector3 error12;
for (int i = 0; i < 3; ++i)
error12[i] = a2[i] + node[1].body->posr.pos[i] - a1[i] -
node[0].body->posr.pos[i];
// error in the p direction is error12 dot p
info->c[0] = k * (dCalcVectorDot3(error12, p));
// error in the q direction is error12 dot p
info->c[1] = k * (dCalcVectorDot3(error12, q));
// interpenetration error for screw constraint
info->c[2] = k * lin_err;
}
else
{
// debug
// printf ("anchor1 %f %f %f\n", anchor1[0], anchor1[1], anchor1[2]);
// printf ("anchor2 %f %f %f\n", anchor2[0], anchor2[1], anchor2[2]);
// printf ("a1 %f %f %f\n", a1[0], a1[1], a1[2]);
// printf ("p1 %f %f %f\n",
// node[0].body->posr.pos[0],
// node[0].body->posr.pos[1],
// node[0].body->posr.pos[2]);
// error of body's anchor
dVector3 error1;
for (int i = 0; i < 3; ++i) error1[i] = anchor2[i] - a1[i] -
node[0].body->posr.pos[i];
// printf ("error1 %f %f %f\n", error1[0], error1[1], error1[2]);
// error in the p direction is error1 dot p
info->c[0] = k * (dCalcVectorDot3(error1, p));
// error in the q direction
info->c[1] = k * (dCalcVectorDot3(error1, q));
// interpenetration error for screw constraint
info->c[2] = k * lin_err;
if ( flags & dJOINT_REVERSE )
for (int i = 0; i < 3; ++i ) ax1[i] = -ax1[i];
}
// uncommnet to enforce slider joint limit
// limot.addLimot ( this, info, 5, ax1, 0 );
}
// constraint rows 4 and 5
{
// set the two hinge rows. the screw axis should be the only unconstrained
// rotational axis, the angular velocity of the two bodies perpendicular to
// the hinge axis should be equal. thus the constraint equations are
// p*w1 - p*w2 = 0
// q*w1 - q*w2 = 0
// where p and q are unit vectors normal to the hinge axis, and w1 and w2
// are the angular velocity vectors of the two bodies.
dVector3 ax1; // length 1 joint axis in global coordinates, from 1st body
dVector3 p, q; // plane space vectors for ax1
dMultiply0_331( ax1, node[0].body->posr.R, axis1 );
dPlaneSpace( ax1, p, q );
int s3 = 3 * info->rowskip;
int s4 = 4 * info->rowskip;
info->J1a[s3+0] = p[0];
info->J1a[s3+1] = p[1];
info->J1a[s3+2] = p[2];
info->J1a[s4+0] = q[0];
info->J1a[s4+1] = q[1];
info->J1a[s4+2] = q[2];
if ( node[1].body )
{
info->J2a[s3+0] = -p[0];
info->J2a[s3+1] = -p[1];
info->J2a[s3+2] = -p[2];
info->J2a[s4+0] = -q[0];
info->J2a[s4+1] = -q[1];
info->J2a[s4+2] = -q[2];
}
// compute the right hand side of the constraint equation. set relative
// body velocities along p and q to bring the screw back into alignment.
// if ax1,ax2 are the unit length screw axes as computed from body1 and
// body2, we need to rotate both bodies along the axis u = (ax1 x ax2).
// if `theta' is the angle between ax1 and ax2, we need an angular velocity
// along u to cover angle erp*theta in one step :
// |angular_velocity| = angle/time = erp*theta / stepsize
// = (erp*fps) * theta
// angular_velocity = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2|
// = (erp*fps) * theta * (ax1 x ax2) / sin(theta)
// ...as ax1 and ax2 are unit length. if theta is smallish,
// theta ~= sin(theta), so
// angular_velocity = (erp*fps) * (ax1 x ax2)
// ax1 x ax2 is in the plane space of ax1, so we project the angular
// velocity to p and q to find the right hand side.
dVector3 ax2, b;
if ( node[1].body )
{
dMultiply0_331( ax2, node[1].body->posr.R, axis2 );
}
else
{
ax2[0] = axis2[0];
ax2[1] = axis2[1];
ax2[2] = axis2[2];
}
dCalcVectorCross3( b, ax1, ax2 );
dReal k = info->fps * info->erp;
info->c[3] = k * dCalcVectorDot3( b, p );
info->c[4] = k * dCalcVectorDot3( b, q );
// enforcing rotation joint limit
limot.addLimot( this, info, 5, ax1, 1 );
}
}
void
dxJointHinge::getInfo2( dxJoint::Info2 *info )
{
// Added by OSRF
// If joint values of erp and cfm are negative, then ignore them.
// info->erp, info->cfm already have the global values from quickstep
if (this->erp >= 0)
info->erp = erp;
if (this->cfm >= 0)
{
info->cfm[0] = cfm;
info->cfm[1] = cfm;
info->cfm[2] = cfm;
info->cfm[3] = cfm;
info->cfm[4] = cfm;
info->cfm[5] = cfm;
}
// set the three ball-and-socket rows
setBall( this, info, anchor1, anchor2 );
// set the two hinge rows. the hinge axis should be the only unconstrained
// rotational axis, the angular velocity of the two bodies perpendicular to
// the hinge axis should be equal. thus the constraint equations are
// p*w1 - p*w2 = 0
// q*w1 - q*w2 = 0
// where p and q are unit vectors normal to the hinge axis, and w1 and w2
// are the angular velocity vectors of the two bodies.
dVector3 ax1; // length 1 joint axis in global coordinates, from 1st body
dVector3 p, q; // plane space vectors for ax1
dMultiply0_331( ax1, node[0].body->posr.R, axis1 );
dPlaneSpace( ax1, p, q );
// strange the rotation matrix is not really a rotation matrix (non-orthogonal vectors)
// normals of columns and rows are not exactly 1 when velocity is large.
// printf("posr.R\n[%f %f %f %f]\n[%f %f %f %f]\n[%f %f %f %f]\n",
// node[0].body->posr.R[0*4+0],node[0].body->posr.R[0*4+1],node[0].body->posr.R[0*4+2],node[0].body->posr.R[0*4+3],
// node[0].body->posr.R[1*4+0],node[0].body->posr.R[1*4+1],node[0].body->posr.R[1*4+2],node[0].body->posr.R[1*4+3],
// node[0].body->posr.R[2*4+0],node[0].body->posr.R[2*4+1],node[0].body->posr.R[2*4+2],node[0].body->posr.R[2*4+3]);
// printf("axis1 [%f %f %f] ax1 [%f %f %f]\n",
// axis1[0], axis1[1], axis1[2],
// ax1[0], ax1[1], ax1[2]);
int s3 = 3 * info->rowskip;
int s4 = 4 * info->rowskip;
info->J1a[s3+0] = p[0];
info->J1a[s3+1] = p[1];
info->J1a[s3+2] = p[2];
info->J1a[s4+0] = q[0];
info->J1a[s4+1] = q[1];
info->J1a[s4+2] = q[2];
if ( node[1].body )
{
info->J2a[s3+0] = -p[0];
info->J2a[s3+1] = -p[1];
info->J2a[s3+2] = -p[2];
info->J2a[s4+0] = -q[0];
info->J2a[s4+1] = -q[1];
info->J2a[s4+2] = -q[2];
}
// compute the right hand side of the constraint equation. set relative
// body velocities along p and q to bring the hinge back into alignment.
// if ax1,ax2 are the unit length hinge axes as computed from body1 and
// body2, we need to rotate both bodies along the axis u = (ax1 x ax2).
// if `theta' is the angle between ax1 and ax2, we need an angular velocity
// along u to cover angle erp*theta in one step :
// |angular_velocity| = angle/time = erp*theta / stepsize
// = (erp*fps) * theta
// angular_velocity = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2|
// = (erp*fps) * theta * (ax1 x ax2) / sin(theta)
// ...as ax1 and ax2 are unit length. if theta is smallish,
// theta ~= sin(theta), so
// angular_velocity = (erp*fps) * (ax1 x ax2)
// ax1 x ax2 is in the plane space of ax1, so we project the angular
// velocity to p and q to find the right hand side.
dVector3 ax2, b;
if ( node[1].body )
{
dMultiply0_331( ax2, node[1].body->posr.R, axis2 );
}
else
{
ax2[0] = axis2[0];
ax2[1] = axis2[1];
ax2[2] = axis2[2];
}
dCalcVectorCross3( b, ax1, ax2 );
dReal k = info->fps * info->erp;
info->c[3] = k * dCalcVectorDot3( b, p );
info->c[4] = k * dCalcVectorDot3( b, q );
// if the hinge is powered, or has joint limits, add in the stuff
limot.addLimot( this, info, 5, ax1, 1 );
// joint damping
if (this->use_damping)
{
// added J1ad and J2ad for damping, only 1 row
info->J1ad[0] = ax1[0];
info->J1ad[1] = ax1[1];
info->J1ad[2] = ax1[2];
if ( this->node[1].body )
{
info->J2ad[0] = -ax1[0];
info->J2ad[1] = -ax1[1];
info->J2ad[2] = -ax1[2];
}
// there's no rhs for damping setup, all we want to use is the jacobian information above
}
}
void dxStepBody (dxBody *b, dReal h)
{
// cap the angular velocity
if (b->flags & dxBodyMaxAngularSpeed) {
const dReal max_ang_speed = b->max_angular_speed;
const dReal aspeed = dCalcVectorDot3( b->avel, b->avel );
if (aspeed > max_ang_speed*max_ang_speed) {
const dReal coef = max_ang_speed/dSqrt(aspeed);
dScaleVector3(b->avel, coef);
}
}
// end of angular velocity cap
// handle linear velocity
for (unsigned int j=0; j<3; j++) b->posr.pos[j] += h * b->lvel[j];
if (b->flags & dxBodyFlagFiniteRotation) {
dVector3 irv; // infitesimal rotation vector
dQuaternion q; // quaternion for finite rotation
if (b->flags & dxBodyFlagFiniteRotationAxis) {
// split the angular velocity vector into a component along the finite
// rotation axis, and a component orthogonal to it.
dVector3 frv; // finite rotation vector
dReal k = dCalcVectorDot3 (b->finite_rot_axis,b->avel);
frv[0] = b->finite_rot_axis[0] * k;
frv[1] = b->finite_rot_axis[1] * k;
frv[2] = b->finite_rot_axis[2] * k;
irv[0] = b->avel[0] - frv[0];
irv[1] = b->avel[1] - frv[1];
irv[2] = b->avel[2] - frv[2];
// make a rotation quaternion q that corresponds to frv * h.
// compare this with the full-finite-rotation case below.
h *= REAL(0.5);
dReal theta = k * h;
q[0] = dCos(theta);
dReal s = sinc(theta) * h;
q[1] = frv[0] * s;
q[2] = frv[1] * s;
q[3] = frv[2] * s;
}
else {
// make a rotation quaternion q that corresponds to w * h
dReal wlen = dSqrt (b->avel[0]*b->avel[0] + b->avel[1]*b->avel[1] +
b->avel[2]*b->avel[2]);
h *= REAL(0.5);
dReal theta = wlen * h;
q[0] = dCos(theta);
dReal s = sinc(theta) * h;
q[1] = b->avel[0] * s;
q[2] = b->avel[1] * s;
q[3] = b->avel[2] * s;
}
// do the finite rotation
dQuaternion q2;
dQMultiply0 (q2,q,b->q);
for (unsigned int j=0; j<4; j++) b->q[j] = q2[j];
// do the infitesimal rotation if required
if (b->flags & dxBodyFlagFiniteRotationAxis) {
dReal dq[4];
dWtoDQ (irv,b->q,dq);
for (unsigned int j=0; j<4; j++) b->q[j] += h * dq[j];
}
}
else {
// the normal way - do an infitesimal rotation
dReal dq[4];
dWtoDQ (b->avel,b->q,dq);
for (unsigned int j=0; j<4; j++) b->q[j] += h * dq[j];
}
// normalize the quaternion and convert it to a rotation matrix
dNormalize4 (b->q);
dQtoR (b->q,b->posr.R);
// notify all attached geoms that this body has moved
dxWorldProcessContext *world_process_context = b->world->UnsafeGetWorldProcessingContext();
for (dxGeom *geom = b->geom; geom; geom = dGeomGetBodyNext (geom)) {
world_process_context->LockForStepbodySerialization();
dGeomMoved (geom);
world_process_context->UnlockForStepbodySerialization();
}
// notify the user
if (b->moved_callback != NULL) {
b->moved_callback(b);
}
// damping
if (b->flags & dxBodyLinearDamping) {
const dReal lin_threshold = b->dampingp.linear_threshold;
const dReal lin_speed = dCalcVectorDot3( b->lvel, b->lvel );
if ( lin_speed > lin_threshold) {
const dReal k = 1 - b->dampingp.linear_scale;
dScaleVector3(b->lvel, k);
}
}
if (b->flags & dxBodyAngularDamping) {
const dReal ang_threshold = b->dampingp.angular_threshold;
const dReal ang_speed = dCalcVectorDot3( b->avel, b->avel );
if ( ang_speed > ang_threshold) {
const dReal k = 1 - b->dampingp.angular_scale;
dScaleVector3(b->avel, k);
}
}
}
void dInternalHandleAutoDisabling (dxWorld *world, dReal stepsize)
{
dxBody *bb;
for ( bb=world->firstbody; bb; bb=(dxBody*)bb->next )
{
// don't freeze objects mid-air (patch 1586738)
if ( bb->firstjoint == NULL ) continue;
// nothing to do unless this body is currently enabled and has
// the auto-disable flag set
if ( (bb->flags & (dxBodyAutoDisable|dxBodyDisabled)) != dxBodyAutoDisable ) continue;
// if sampling / threshold testing is disabled, we can never sleep.
if ( bb->adis.average_samples == 0 ) continue;
//
// see if the body is idle
//
#ifndef dNODEBUG
// sanity check
if ( bb->average_counter >= bb->adis.average_samples )
{
dUASSERT( bb->average_counter < bb->adis.average_samples, "buffer overflow" );
// something is going wrong, reset the average-calculations
bb->average_ready = 0; // not ready for average calculation
bb->average_counter = 0; // reset the buffer index
}
#endif // dNODEBUG
// sample the linear and angular velocity
bb->average_lvel_buffer[bb->average_counter][0] = bb->lvel[0];
bb->average_lvel_buffer[bb->average_counter][1] = bb->lvel[1];
bb->average_lvel_buffer[bb->average_counter][2] = bb->lvel[2];
bb->average_avel_buffer[bb->average_counter][0] = bb->avel[0];
bb->average_avel_buffer[bb->average_counter][1] = bb->avel[1];
bb->average_avel_buffer[bb->average_counter][2] = bb->avel[2];
bb->average_counter++;
// buffer ready test
if ( bb->average_counter >= bb->adis.average_samples )
{
bb->average_counter = 0; // fill the buffer from the beginning
bb->average_ready = 1; // this body is ready now for average calculation
}
int idle = 0; // Assume it's in motion unless we have samples to disprove it.
// enough samples?
if ( bb->average_ready )
{
idle = 1; // Initial assumption: IDLE
// the sample buffers are filled and ready for calculation
dVector3 average_lvel, average_avel;
// Store first velocity samples
average_lvel[0] = bb->average_lvel_buffer[0][0];
average_avel[0] = bb->average_avel_buffer[0][0];
average_lvel[1] = bb->average_lvel_buffer[0][1];
average_avel[1] = bb->average_avel_buffer[0][1];
average_lvel[2] = bb->average_lvel_buffer[0][2];
average_avel[2] = bb->average_avel_buffer[0][2];
// If we're not in "instantaneous mode"
if ( bb->adis.average_samples > 1 )
{
// add remaining velocities together
for ( unsigned int i = 1; i < bb->adis.average_samples; ++i )
{
average_lvel[0] += bb->average_lvel_buffer[i][0];
average_avel[0] += bb->average_avel_buffer[i][0];
average_lvel[1] += bb->average_lvel_buffer[i][1];
average_avel[1] += bb->average_avel_buffer[i][1];
average_lvel[2] += bb->average_lvel_buffer[i][2];
average_avel[2] += bb->average_avel_buffer[i][2];
}
// make average
dReal r1 = dReal( 1.0 ) / dReal( bb->adis.average_samples );
average_lvel[0] *= r1;
average_avel[0] *= r1;
average_lvel[1] *= r1;
average_avel[1] *= r1;
average_lvel[2] *= r1;
average_avel[2] *= r1;
}
// threshold test
dReal av_lspeed, av_aspeed;
av_lspeed = dCalcVectorDot3( average_lvel, average_lvel );
if ( av_lspeed > bb->adis.linear_average_threshold )
{
idle = 0; // average linear velocity is too high for idle
}
else
{
av_aspeed = dCalcVectorDot3( average_avel, average_avel );
if ( av_aspeed > bb->adis.angular_average_threshold )
{
idle = 0; // average angular velocity is too high for idle
}
}
}
// if it's idle, accumulate steps and time.
// these counters won't overflow because this code doesn't run for disabled bodies.
if (idle) {
bb->adis_stepsleft--;
bb->adis_timeleft -= stepsize;
}
else {
// Reset countdowns
bb->adis_stepsleft = bb->adis.idle_steps;
bb->adis_timeleft = bb->adis.idle_time;
}
// disable the body if it's idle for a long enough time
if ( bb->adis_stepsleft <= 0 && bb->adis_timeleft <= 0 )
{
bb->flags |= dxBodyDisabled; // set the disable flag
// disabling bodies should also include resetting the velocity
// should prevent jittering in big "islands"
bb->lvel[0] = 0;
bb->lvel[1] = 0;
bb->lvel[2] = 0;
bb->avel[0] = 0;
bb->avel[1] = 0;
bb->avel[2] = 0;
}
}
}
int dCollideSphereBox (dxGeom *o1, dxGeom *o2, int flags,
dContactGeom *contact, int skip)
{
dIASSERT (skip >= (int)sizeof(dContactGeom));
dIASSERT (o1->type == dSphereClass);
dIASSERT (o2->type == dBoxClass);
dIASSERT ((flags & NUMC_MASK) >= 1);
// this is easy. get the sphere center `p' relative to the box, and then clip
// that to the boundary of the box (call that point `q'). if q is on the
// boundary of the box and |p-q| is <= sphere radius, they touch.
// if q is inside the box, the sphere is inside the box, so set a contact
// normal to push the sphere to the closest box face.
dVector3 l,t,p,q,r;
dReal depth;
int onborder = 0;
dxSphere *sphere = (dxSphere*) o1;
dxBox *box = (dxBox*) o2;
contact->g1 = o1;
contact->g2 = o2;
contact->side1 = -1;
contact->side2 = -1;
p[0] = o1->final_posr->pos[0] - o2->final_posr->pos[0];
p[1] = o1->final_posr->pos[1] - o2->final_posr->pos[1];
p[2] = o1->final_posr->pos[2] - o2->final_posr->pos[2];
l[0] = box->side[0]*REAL(0.5);
t[0] = dCalcVectorDot3_14(p,o2->final_posr->R);
if (t[0] < -l[0]) { t[0] = -l[0]; onborder = 1; }
if (t[0] > l[0]) { t[0] = l[0]; onborder = 1; }
l[1] = box->side[1]*REAL(0.5);
t[1] = dCalcVectorDot3_14(p,o2->final_posr->R+1);
if (t[1] < -l[1]) { t[1] = -l[1]; onborder = 1; }
if (t[1] > l[1]) { t[1] = l[1]; onborder = 1; }
t[2] = dCalcVectorDot3_14(p,o2->final_posr->R+2);
l[2] = box->side[2]*REAL(0.5);
if (t[2] < -l[2]) { t[2] = -l[2]; onborder = 1; }
if (t[2] > l[2]) { t[2] = l[2]; onborder = 1; }
if (!onborder) {
// sphere center inside box. find closest face to `t'
dReal min_distance = l[0] - dFabs(t[0]);
int mini = 0;
for (int i=1; i<3; i++) {
dReal face_distance = l[i] - dFabs(t[i]);
if (face_distance < min_distance) {
min_distance = face_distance;
mini = i;
}
}
// contact position = sphere center
contact->pos[0] = o1->final_posr->pos[0];
contact->pos[1] = o1->final_posr->pos[1];
contact->pos[2] = o1->final_posr->pos[2];
// contact normal points to closest face
dVector3 tmp;
tmp[0] = 0;
tmp[1] = 0;
tmp[2] = 0;
tmp[mini] = (t[mini] > 0) ? REAL(1.0) : REAL(-1.0);
dMultiply0_331 (contact->normal,o2->final_posr->R,tmp);
// contact depth = distance to wall along normal plus radius
contact->depth = min_distance + sphere->radius;
return 1;
}
t[3] = 0; //@@@ hmmm
dMultiply0_331 (q,o2->final_posr->R,t);
r[0] = p[0] - q[0];
r[1] = p[1] - q[1];
r[2] = p[2] - q[2];
depth = sphere->radius - dSqrt(dCalcVectorDot3(r,r));
if (depth < 0) return 0;
contact->pos[0] = q[0] + o2->final_posr->pos[0];
contact->pos[1] = q[1] + o2->final_posr->pos[1];
contact->pos[2] = q[2] + o2->final_posr->pos[2];
contact->normal[0] = r[0];
contact->normal[1] = r[1];
contact->normal[2] = r[2];
dNormalize3 (contact->normal);
contact->depth = depth;
return 1;
}
