void setBall( dxJoint *joint, dxJoint::Info2 *info, dVector3 anchor1, dVector3 anchor2 ) { // anchor points in global coordinates with respect to body PORs. dVector3 a1, a2; int s = info->rowskip; // set jacobian info->J1l[0] = 1; info->J1l[s+1] = 1; info->J1l[2*s+2] = 1; dMultiply0_331( a1, joint->node[0].body->posr.R, anchor1 ); dSetCrossMatrixMinus( info->J1a, a1, s ); if ( joint->node[1].body ) { info->J2l[0] = -1; info->J2l[s+1] = -1; info->J2l[2*s+2] = -1; dMultiply0_331( a2, joint->node[1].body->posr.R, anchor2 ); dSetCrossMatrixPlus( info->J2a, a2, s ); } // set right hand side dReal k = info->fps * info->erp; if ( joint->node[1].body ) { for ( int j = 0; j < 3; j++ ) { info->c[j] = k * ( a2[j] + joint->node[1].body->posr.pos[j] - a1[j] - joint->node[0].body->posr.pos[j] ); } } else { for ( int j = 0; j < 3; j++ ) { info->c[j] = k * ( anchor2[j] - a1[j] - joint->node[0].body->posr.pos[j] ); } } }
void dxJointDBall::getInfo2( dxJoint::Info2 *info ) { info->erp = erp; info->cfm[0] = cfm; dVector3 globalA1, globalA2; dBodyGetRelPointPos(node[0].body, anchor1[0], anchor1[1], anchor1[2], globalA1); if (node[1].body) dBodyGetRelPointPos(node[1].body, anchor2[0], anchor2[1], anchor2[2], globalA2); else dCopyVector3(globalA2, anchor2); dVector3 q; dSubtractVectors3(q, globalA1, globalA2); #ifdef dSINGLE const dReal MIN_LENGTH = REAL(1e-7); #else const dReal MIN_LENGTH = REAL(1e-12); #endif if (dCalcVectorLength3(q) < MIN_LENGTH) { // too small, let's choose an arbitrary direction // heuristic: difference in velocities at anchors dVector3 v1, v2; dBodyGetPointVel(node[0].body, globalA1[0], globalA1[1], globalA1[2], v1); if (node[1].body) dBodyGetPointVel(node[1].body, globalA2[0], globalA2[1], globalA2[2], v2); else dSetZero(v2, 3); dSubtractVectors3(q, v1, v2); if (dCalcVectorLength3(q) < MIN_LENGTH) { // this direction is as good as any q[0] = 1; q[1] = 0; q[2] = 0; } } dNormalize3(q); info->J1l[0] = q[0]; info->J1l[1] = q[1]; info->J1l[2] = q[2]; dVector3 relA1; dBodyVectorToWorld(node[0].body, anchor1[0], anchor1[1], anchor1[2], relA1); dMatrix3 a1m; dSetZero(a1m, 12); dSetCrossMatrixMinus(a1m, relA1, 4); dMultiply1_331(info->J1a, a1m, q); if (node[1].body) { info->J2l[0] = -q[0]; info->J2l[1] = -q[1]; info->J2l[2] = -q[2]; dVector3 relA2; dBodyVectorToWorld(node[1].body, anchor2[0], anchor2[1], anchor2[2], relA2); dMatrix3 a2m; dSetZero(a2m, 12); dSetCrossMatrixPlus(a2m, relA2, 4); dMultiply1_331(info->J2a, a2m, q); } const dReal k = info->fps * info->erp; info->c[0] = k * (targetDistance - dCalcPointsDistance3(globalA1, globalA2)); }
btVector3 btRigidBody::computeGyroscopicImpulseImplicit_Cooper(btScalar step) const { #if 0 dReal h = callContext->m_stepperCallContext->m_stepSize; // Step size dVector3 L; // Compute angular momentum dMultiply0_331(L, I, b->avel); #endif btVector3 inertiaLocal = getLocalInertia(); btMatrix3x3 inertiaTensorWorld = getWorldTransform().getBasis().scaled(inertiaLocal) * getWorldTransform().getBasis().transpose(); btVector3 L = inertiaTensorWorld*getAngularVelocity(); btMatrix3x3 Itild(0, 0, 0, 0, 0, 0, 0, 0, 0); #if 0 for (int ii = 0; ii<12; ++ii) { Itild[ii] = Itild[ii] * h + I[ii]; } #endif btSetCrossMatrixMinus(Itild, L*step); Itild += inertiaTensorWorld; #if 0 // Compute a new effective 'inertia tensor' // for the implicit step: the cross-product // matrix of the angular momentum plus the // old tensor scaled by the timestep. // Itild may not be symmetric pos-definite, // but we can still use it to compute implicit // gyroscopic torques. dMatrix3 Itild = { 0 }; dSetCrossMatrixMinus(Itild, L, 4); for (int ii = 0; ii<12; ++ii) { Itild[ii] = Itild[ii] * h + I[ii]; } #endif L *= step; //Itild may not be symmetric pos-definite btMatrix3x3 itInv = Itild.inverse(); Itild = inertiaTensorWorld * itInv; btMatrix3x3 ident(1,0,0,0,1,0,0,0,1); Itild -= ident; #if 0 // Scale momentum by inverse time to get // a sort of "torque" dScaleVector3(L, dRecip(h)); // Invert the pseudo-tensor dMatrix3 itInv; // This is a closed-form inversion. // It's probably not numerically stable // when dealing with small masses with // a large asymmetry. // An LU decomposition might be better. if (dInvertMatrix3(itInv, Itild) != 0) { // "Divide" the original tensor // by the pseudo-tensor (on the right) dMultiply0_333(Itild, I, itInv); // Subtract an identity matrix Itild[0] -= 1; Itild[5] -= 1; Itild[10] -= 1; // This new inertia matrix rotates the // momentum to get a new set of torques // that will work correctly when applied // to the old inertia matrix as explicit // torques with a semi-implicit update // step. dVector3 tau0; dMultiply0_331(tau0, Itild, L); // Add the gyro torques to the torque // accumulator for (int ii = 0; ii<3; ++ii) { b->tacc[ii] += tau0[ii]; } #endif btVector3 tau0 = Itild * L; // printf("tau0 = %f,%f,%f\n",tau0.x(),tau0.y(),tau0.z()); return tau0; } btVector3 btRigidBody::computeGyroscopicImpulseImplicit_Ewert(btScalar step) const { // use full newton-euler equations. common practice to drop the wxIw term. want it for better tumbling behavior. // calculate using implicit euler step so it's stable. const btVector3 inertiaLocal = getLocalInertia(); const btVector3 w0 = getAngularVelocity(); btMatrix3x3 I; I = m_worldTransform.getBasis().scaled(inertiaLocal) * m_worldTransform.getBasis().transpose(); // use newtons method to find implicit solution for new angular velocity (w') // f(w') = -(T*step + Iw) + Iw' + w' + w'xIw'*step = 0 // df/dw' = I + 1xIw'*step + w'xI*step btVector3 w1 = w0; // one step of newton's method { const btVector3 fw = evalEulerEqn(w1, w0, btVector3(0, 0, 0), step, I); const btMatrix3x3 dfw = evalEulerEqnDeriv(w1, w0, step, I); const btMatrix3x3 dfw_inv = dfw.inverse(); btVector3 dw; dw = dfw_inv*fw; w1 -= dw; } btVector3 gf = (w1 - w0); return gf; } void btRigidBody::integrateVelocities(btScalar step) { if (isStaticOrKinematicObject()) return; m_linearVelocity += m_totalForce * (m_inverseMass * step); m_angularVelocity += m_invInertiaTensorWorld * m_totalTorque * step; #define MAX_ANGVEL SIMD_HALF_PI /// clamp angular velocity. collision calculations will fail on higher angular velocities btScalar angvel = m_angularVelocity.length(); if (angvel*step > MAX_ANGVEL) { m_angularVelocity *= (MAX_ANGVEL/step) /angvel; } } btQuaternion btRigidBody::getOrientation() const { btQuaternion orn; m_worldTransform.getBasis().getRotation(orn); return orn; }