void dxStepBody (dxBody *b, dReal h) { int j; #ifdef DEBUG_VALID 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]; 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 = dDOT (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 (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 (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 (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->R); // notify all attached geoms that this body has moved for (dxGeom *geom = b->geom; geom; geom = dGeomGetBodyNext (geom)) dGeomMoved (geom); #ifdef DEBUG_VALID dIASSERT(dValid(b->avel[0])&&dValid(b->avel[1])&&dValid(b->avel[2])); #endif }
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); } } }
static inline void moveAndRotateBody (dxBody * b, dReal h) { int j; // handle linear velocity for (j = 0; j < 3; j++) b->posr.pos[j] += dMUL(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 = dDOT (b->finite_rot_axis, b->avel); frv[0] = dMUL(b->finite_rot_axis[0],k); frv[1] = dMUL(b->finite_rot_axis[1],k); frv[2] = dMUL(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 = dMUL(h,REAL (0.5)); dReal theta = dMUL(k,h); q[0] = dCos (theta); dReal s = dMUL(sinc (theta),h); q[1] = dMUL(frv[0],s); q[2] = dMUL(frv[1],s); q[3] = dMUL(frv[2],s); } else { // make a rotation quaternion q that corresponds to w * h dReal wlen = dSqrt (dMUL(b->avel[0],b->avel[0]) + dMUL(b->avel[1],b->avel[1]) + dMUL(b->avel[2],b->avel[2])); h = dMUL(h,REAL (0.5)); dReal theta = dMUL(wlen,h); q[0] = dCos (theta); dReal s = dMUL(sinc (theta),h); q[1] = dMUL(b->avel[0],s); q[2] = dMUL(b->avel[1],s); q[3] = dMUL(b->avel[2],s); } // do the finite rotation dQuaternion q2; dQMultiply0 (q2, q, b->q); for (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 (j = 0; j < 4; j++) b->q[j] += dMUL(h,dq[j]); } } else { // the normal way - do an infitesimal rotation dReal dq[4]; dWtoDQ (b->avel, b->q, dq); for (j = 0; j < 4; j++) b->q[j] += dMUL(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 for (dxGeom * geom = b->geom; geom; geom = dGeomGetBodyNext (geom)) dGeomMoved (geom); }