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; }