void CalculatePickForceAndTorque (const NewtonBody* const body, const dVector& pointOnBodyInGlobalSpace, const dVector& targetPositionInGlobalSpace, dFloat timestep) { dMatrix matrix; dVector com(0.0f); dVector omega0(0.0f); dVector veloc0(0.0f); dVector omega1(0.0f); dVector veloc1(0.0f); dVector pointVeloc(0.0f); const dFloat stiffness = 0.3f; const dFloat angularDamp = 0.95f; dFloat invTimeStep = 1.0f / timestep; NewtonWorld* const world = NewtonBodyGetWorld (body); NewtonWorldCriticalSectionLock (world, 0); // calculate the desired impulse NewtonBodyGetMatrix(body, &matrix[0][0]); NewtonBodyGetOmega (body, &omega0[0]); NewtonBodyGetVelocity (body, &veloc0[0]); NewtonBodyGetPointVelocity (body, &pointOnBodyInGlobalSpace[0], &pointVeloc[0]); dVector deltaVeloc (targetPositionInGlobalSpace - pointOnBodyInGlobalSpace); deltaVeloc = deltaVeloc.Scale (stiffness * invTimeStep) - pointVeloc; for (int i = 0; i < 3; i ++) { dVector veloc (0.0f); veloc[i] = deltaVeloc[i]; NewtonBodyAddImpulse (body, &veloc[0], &pointOnBodyInGlobalSpace[0]); } // damp angular velocity NewtonBodyGetOmega (body, &omega1[0]); NewtonBodyGetVelocity (body, &veloc1[0]); omega1 = omega1.Scale (angularDamp); // restore body velocity and angular velocity NewtonBodySetOmega (body, &omega0[0]); NewtonBodySetVelocity(body, &veloc0[0]); // convert the delta velocity change to a external force and torque dFloat Ixx; dFloat Iyy; dFloat Izz; dFloat mass; NewtonBodyGetMassMatrix (body, &mass, &Ixx, &Iyy, &Izz); dVector angularMomentum (Ixx, Iyy, Izz); angularMomentum = matrix.RotateVector (angularMomentum.CompProduct(matrix.UnrotateVector(omega1 - omega0))); dVector force ((veloc1 - veloc0).Scale (mass * invTimeStep)); dVector torque (angularMomentum.Scale(invTimeStep)); NewtonBodyAddForce(body, &force[0]); NewtonBodyAddTorque(body, &torque[0]); // make sure the body is unfrozen, if it is picked NewtonBodySetSleepState (body, 0); NewtonWorldCriticalSectionUnlock (world); }
void CustomKinematicController::SubmitConstraints (dFloat timestep, int threadIndex) { // check if this is an impulsive time step if (timestep > 0.0f) { dMatrix matrix0; dVector v(0.0f); dVector w(0.0f); dVector cg(0.0f); dFloat invTimestep = 1.0f / timestep; // calculate the position of the pivot point and the Jacobian direction vectors, in global space. NewtonBodyGetOmega (m_body0, &w[0]); NewtonBodyGetVelocity (m_body0, &v[0]); NewtonBodyGetCentreOfMass (m_body0, &cg[0]); NewtonBodyGetMatrix (m_body0, &matrix0[0][0]); dVector p0 (matrix0.TransformVector (m_localHandle)); dVector pointVeloc (v + w * matrix0.RotateVector (m_localHandle - cg)); dVector relPosit (m_targetPosit - p0); dVector relVeloc (relPosit.Scale (invTimestep) - pointVeloc); dVector relAccel (relVeloc.Scale (invTimestep * 0.3f)); // Restrict the movement on the pivot point along all tree orthonormal direction NewtonUserJointAddLinearRow (m_joint, &p0[0], &m_targetPosit[0], &matrix0.m_front[0]); NewtonUserJointSetRowAcceleration (m_joint, relAccel % matrix0.m_front); NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxLinearFriction); NewtonUserJointSetRowMaximumFriction (m_joint, m_maxLinearFriction); NewtonUserJointAddLinearRow (m_joint, &p0[0], &m_targetPosit[0], &matrix0.m_up[0]); NewtonUserJointSetRowAcceleration (m_joint, relAccel % matrix0.m_up); NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxLinearFriction); NewtonUserJointSetRowMaximumFriction (m_joint, m_maxLinearFriction); NewtonUserJointAddLinearRow (m_joint, &p0[0], &m_targetPosit[0], &matrix0.m_right[0]); NewtonUserJointSetRowAcceleration (m_joint, relAccel % matrix0.m_right); NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxLinearFriction); NewtonUserJointSetRowMaximumFriction (m_joint, m_maxLinearFriction); if (m_pickMode) { dQuaternion rotation; NewtonBodyGetRotation (m_body0, &rotation.m_q0); if (m_targetRot.DotProduct (rotation) < 0.0f) { rotation.m_q0 *= -1.0f; rotation.m_q1 *= -1.0f; rotation.m_q2 *= -1.0f; rotation.m_q3 *= -1.0f; } dVector relOmega (rotation.CalcAverageOmega (m_targetRot, invTimestep) - w); dFloat mag = relOmega % relOmega; if (mag > 1.0e-6f) { dVector pin (relOmega.Scale (1.0f / mag)); dMatrix basis (dGrammSchmidt (pin)); dFloat relSpeed = dSqrt (relOmega % relOmega); dFloat relAlpha = relSpeed * invTimestep; NewtonUserJointAddAngularRow (m_joint, 0.0f, &basis.m_front[0]); NewtonUserJointSetRowAcceleration (m_joint, relAlpha); NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction); NewtonUserJointSetRowMaximumFriction (m_joint, m_maxAngularFriction); NewtonUserJointAddAngularRow (m_joint, 0.0f, &basis.m_up[0]); NewtonUserJointSetRowAcceleration (m_joint, 0.0f); NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction); NewtonUserJointSetRowMaximumFriction (m_joint, m_maxAngularFriction); NewtonUserJointAddAngularRow (m_joint, 0.0f, &basis.m_right[0]); NewtonUserJointSetRowAcceleration (m_joint, 0.0f); NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction); NewtonUserJointSetRowMaximumFriction (m_joint, m_maxAngularFriction); } else { dVector relAlpha (w.Scale (-invTimestep)); NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_front[0]); NewtonUserJointSetRowAcceleration (m_joint, relAlpha % matrix0.m_front); NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction); NewtonUserJointSetRowMaximumFriction (m_joint, m_maxAngularFriction); NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_up[0]); NewtonUserJointSetRowAcceleration (m_joint, relAlpha % matrix0.m_up); NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction); NewtonUserJointSetRowMaximumFriction (m_joint, m_maxAngularFriction); NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_right[0]); NewtonUserJointSetRowAcceleration (m_joint, relAlpha % matrix0.m_right); NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction); NewtonUserJointSetRowMaximumFriction (m_joint, m_maxAngularFriction); } } else { // this is the single handle pick mode, add some angular friction dVector relAlpha = w.Scale (-invTimestep); NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_front[0]); NewtonUserJointSetRowAcceleration (m_joint, relAlpha % matrix0.m_front); NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction * 0.025f); NewtonUserJointSetRowMaximumFriction (m_joint, m_maxAngularFriction * 0.025f); NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_up[0]); NewtonUserJointSetRowAcceleration (m_joint, relAlpha % matrix0.m_up); NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction * 0.025f); NewtonUserJointSetRowMaximumFriction (m_joint, m_maxAngularFriction * 0.025f); NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_right[0]); NewtonUserJointSetRowAcceleration (m_joint, relAlpha % matrix0.m_right); NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction * 0.025f); NewtonUserJointSetRowMaximumFriction (m_joint, m_maxAngularFriction * 0.025f); } } }
void dCustomKinematicController::SubmitConstraints (dFloat timestep, int threadIndex) { // check if this is an impulsive time step dMatrix matrix0(GetBodyMatrix()); dVector omega(0.0f); dVector com(0.0f); dVector pointVeloc(0.0f); const dFloat damp = 0.3f; dAssert (timestep > 0.0f); const dFloat invTimestep = 1.0f / timestep; // we not longer cap excessive angular velocities, it is left to the client application. NewtonBodyGetOmega(m_body0, &omega[0]); //cap excessive angular velocities dFloat mag2 = omega.DotProduct3(omega); if (mag2 > (m_omegaCap * m_omegaCap)) { omega = omega.Normalize().Scale(m_omegaCap); NewtonBodySetOmega(m_body0, &omega[0]); } // calculate the position of the pivot point and the Jacobian direction vectors, in global space. dVector relPosit(m_targetMatrix.m_posit - matrix0.m_posit); NewtonBodyGetPointVelocity(m_body0, &m_targetMatrix.m_posit[0], &pointVeloc[0]); for (int i = 0; i < 3; i ++) { // Restrict the movement on the pivot point along all tree orthonormal direction dFloat speed = pointVeloc.DotProduct3(m_targetMatrix[i]); dFloat dist = relPosit.DotProduct3(m_targetMatrix[i]) * damp; dFloat relSpeed = dist * invTimestep - speed; dFloat relAccel = relSpeed * invTimestep; NewtonUserJointAddLinearRow(m_joint, &matrix0.m_posit[0], &matrix0.m_posit[0], &m_targetMatrix[i][0]); NewtonUserJointSetRowAcceleration(m_joint, relAccel); NewtonUserJointSetRowMinimumFriction(m_joint, -m_maxLinearFriction); NewtonUserJointSetRowMaximumFriction(m_joint, m_maxLinearFriction); } if (m_isSixdof) { dQuaternion rotation (matrix0.Inverse() * m_targetMatrix); if (dAbs (rotation.m_q0) < 0.99998f) { dMatrix rot (dGrammSchmidt(dVector (rotation.m_q1, rotation.m_q2, rotation.m_q3))); dFloat angle = 2.0f * dAcos(dClamp(rotation.m_q0, dFloat(-1.0f), dFloat(1.0f))); NewtonUserJointAddAngularRow (m_joint, angle, &rot.m_front[0]); NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction); NewtonUserJointSetRowMaximumFriction (m_joint, m_maxAngularFriction); NewtonUserJointAddAngularRow (m_joint, 0.0f, &rot.m_up[0]); NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction); NewtonUserJointSetRowMaximumFriction (m_joint, m_maxAngularFriction); NewtonUserJointAddAngularRow (m_joint, 0.0f, &rot.m_right[0]); NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction); NewtonUserJointSetRowMaximumFriction (m_joint, m_maxAngularFriction); } else { NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_front[0]); NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction); NewtonUserJointSetRowMaximumFriction (m_joint, m_maxAngularFriction); NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_up[0]); NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction); NewtonUserJointSetRowMaximumFriction (m_joint, m_maxAngularFriction); NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_right[0]); NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction); NewtonUserJointSetRowMaximumFriction (m_joint, m_maxAngularFriction); } } }