dQuaternion dQuaternion::Slerp (const dQuaternion &QB, dFloat t) const { dFloat dot; dFloat ang; dFloat Sclp; dFloat Sclq; dFloat den; dFloat sinAng; dQuaternion Q; dot = DotProduct (QB); _ASSERTE (dot >= 0.0f); if ((dot + dFloat(1.0f)) > dFloat(1.0e-5f)) { if (dot < dFloat(0.995f)) { ang = dAcos (dot); sinAng = dSin (ang); den = dFloat(1.0f) / sinAng; Sclp = dSin ((dFloat(1.0f) - t ) * ang) * den; Sclq = dSin (t * ang) * den; } else { Sclp = dFloat(1.0f) - t; Sclq = t; } Q.m_q0 = m_q0 * Sclp + QB.m_q0 * Sclq; Q.m_q1 = m_q1 * Sclp + QB.m_q1 * Sclq; Q.m_q2 = m_q2 * Sclp + QB.m_q2 * Sclq; Q.m_q3 = m_q3 * Sclp + QB.m_q3 * Sclq; } else { Q.m_q0 = m_q3; Q.m_q1 = -m_q2; Q.m_q2 = m_q1; Q.m_q3 = m_q0; Sclp = dSin ((dFloat(1.0f) - t) * dFloat (3.141592f *0.5f)); Sclq = dSin (t * dFloat (3.141592f * 0.5f)); Q.m_q0 = m_q0 * Sclp + Q.m_q0 * Sclq; Q.m_q1 = m_q1 * Sclp + Q.m_q1 * Sclq; Q.m_q2 = m_q2 * Sclp + Q.m_q2 * Sclq; Q.m_q3 = m_q3 * Sclp + Q.m_q3 * Sclq; } dot = Q.DotProduct (Q); if ((dot) < (1.0f - 1.0e-4f) ) { dot = dFloat(1.0f) / dSqrt (dot); //dot = dgRsqrt (dot); Q.m_q0 *= dot; Q.m_q1 *= dot; Q.m_q2 *= dot; Q.m_q3 *= dot; } return Q; }
void MSP::BallAndSocket::submit_constraints(const NewtonJoint* joint, dFloat timestep, int thread_index) { MSP::Joint::JointData* joint_data = reinterpret_cast<MSP::Joint::JointData*>(NewtonJointGetUserData(joint)); BallAndSocketData* cj_data = reinterpret_cast<BallAndSocketData*>(joint_data->m_cj_data); dFloat inv_timestep = 1.0f / timestep; // Calculate the position of the pivot point and the Jacobian direction vectors, in global space. dMatrix matrix0, matrix1; MSP::Joint::c_calculate_global_matrix(joint_data, matrix0, matrix1); dFloat last_cone_angle = cj_data->m_cur_cone_angle; // Calculate current cone angle dFloat cur_cone_angle_cos = matrix0.m_right.DotProduct3(matrix1.m_right); cj_data->m_cur_cone_angle = dAcos(Util::clamp_float(cur_cone_angle_cos, -1.0f, 1.0f)); // Calculate current twist angle, omega, and acceleration. if (cur_cone_angle_cos < -0.999999f) { cj_data->m_cur_twist_omega = 0.0f; cj_data->m_cur_twist_alpha = 0.0f; } else { dFloat last_twist_angle = cj_data->m_twist_ai->get_angle(); dFloat last_twist_omega = cj_data->m_cur_twist_omega; dMatrix rot_matrix0; Util::rotate_matrix_to_dir(matrix0, matrix1.m_right, rot_matrix0); dFloat sin_angle; dFloat cos_angle; MSP::Joint::c_calculate_angle(matrix1.m_front, rot_matrix0.m_front, matrix1.m_right, sin_angle, cos_angle); cj_data->m_twist_ai->update(cos_angle, sin_angle); cj_data->m_cur_twist_omega = (cj_data->m_twist_ai->get_angle() - last_twist_angle) * inv_timestep; cj_data->m_cur_twist_alpha = (cj_data->m_cur_twist_omega - last_twist_omega) * inv_timestep; } // Get the current lateral and tangent dir dVector lateral_dir; dVector front_dir; if (dAbs(cur_cone_angle_cos) > 0.999999f) { lateral_dir = matrix1.m_front; front_dir = matrix1.m_up; } else { lateral_dir = matrix1.m_right.CrossProduct(matrix0.m_right); front_dir = matrix1.m_right.CrossProduct(lateral_dir); } // Restrict the movement on the pivot point along all tree orthonormal directions. NewtonUserJointAddLinearRow(joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix1.m_front[0]); NewtonUserJointSetRowStiffness(joint, joint_data->m_stiffness); NewtonUserJointAddLinearRow(joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix1.m_up[0]); NewtonUserJointSetRowStiffness(joint, joint_data->m_stiffness); NewtonUserJointAddLinearRow(joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix1.m_right[0]); NewtonUserJointSetRowStiffness(joint, joint_data->m_stiffness); // Calculate friction dFloat power = cj_data->m_friction * dAbs(cj_data->m_controller); // Handle cone angle if (cj_data->m_cone_limits_enabled && cj_data->m_max_cone_angle < Joint::ANGULAR_LIMIT_EPSILON2) { // Handle in case joint being a hinge; max cone angle is near zero. NewtonUserJointAddAngularRow(joint, MSP::Joint::c_calculate_angle2(matrix0.m_right, matrix1.m_right, matrix1.m_front), &matrix1.m_front[0]); NewtonUserJointSetRowStiffness(joint, joint_data->m_stiffness); NewtonUserJointAddAngularRow(joint, MSP::Joint::c_calculate_angle2(matrix0.m_right, matrix1.m_right, matrix1.m_up), &matrix1.m_up[0]); NewtonUserJointSetRowStiffness(joint, joint_data->m_stiffness); } else if (cj_data->m_cone_limits_enabled && cj_data->m_cur_cone_angle > cj_data->m_max_cone_angle) { // Handle in case current cone angle is greater than max cone angle dFloat dangle = cj_data->m_cur_cone_angle - cj_data->m_max_cone_angle; NewtonUserJointAddAngularRow(joint, dangle, &lateral_dir[0]); NewtonUserJointSetRowMaximumFriction(joint, 0.0f); NewtonUserJointSetRowStiffness(joint, joint_data->m_stiffness); NewtonUserJointAddAngularRow(joint, 0.0f, &front_dir[0]); NewtonUserJointSetRowMinimumFriction(joint, -power); NewtonUserJointSetRowMaximumFriction(joint, power); NewtonUserJointSetRowStiffness(joint, joint_data->m_stiffness); } else { // Handle in case limits are not necessary dFloat cur_cone_omega = (cj_data->m_cur_cone_angle - last_cone_angle) * inv_timestep; dFloat des_cone_accel = -cur_cone_omega * inv_timestep; NewtonUserJointAddAngularRow(joint, 0.0f, &lateral_dir[0]); NewtonUserJointSetRowAcceleration(joint, des_cone_accel); NewtonUserJointSetRowMinimumFriction(joint, -power); NewtonUserJointSetRowMaximumFriction(joint, power); NewtonUserJointSetRowStiffness(joint, joint_data->m_stiffness); NewtonUserJointAddAngularRow(joint, 0.0f, &front_dir[0]); NewtonUserJointSetRowMinimumFriction(joint, -power); NewtonUserJointSetRowMaximumFriction(joint, power); NewtonUserJointSetRowStiffness(joint, joint_data->m_stiffness); } // Handle twist angle bool bcontinue = false; if (cj_data->m_twist_limits_enabled) { if (cj_data->m_min_twist_angle > cj_data->m_max_twist_angle) { // Handle in case min angle is greater than max NewtonUserJointAddAngularRow(joint, (cj_data->m_min_twist_angle + cj_data->m_max_twist_angle) * 0.5f - cj_data->m_twist_ai->get_angle(), &matrix0.m_right[0]); NewtonUserJointSetRowStiffness(joint, joint_data->m_stiffness); } else if (cj_data->m_max_twist_angle - cj_data->m_min_twist_angle < Joint::ANGULAR_LIMIT_EPSILON2) { // Handle in case min angle is almost equal to max NewtonUserJointAddAngularRow(joint, cj_data->m_max_twist_angle - cj_data->m_twist_ai->get_angle(), &matrix0.m_right[0]); NewtonUserJointSetRowStiffness(joint, joint_data->m_stiffness); } else if (cj_data->m_twist_ai->get_angle() < cj_data->m_min_twist_angle) { // Handle in case current twist angle is less than min NewtonUserJointAddAngularRow(joint, cj_data->m_min_twist_angle - cj_data->m_twist_ai->get_angle() + Joint::ANGULAR_LIMIT_EPSILON, &matrix0.m_right[0]); NewtonUserJointSetRowMinimumFriction(joint, 0.0f); NewtonUserJointSetRowStiffness(joint, joint_data->m_stiffness); } else if (cj_data->m_twist_ai->get_angle() > cj_data->m_max_twist_angle) { // Handle in case current twist angle is greater than max NewtonUserJointAddAngularRow(joint, cj_data->m_max_twist_angle - cj_data->m_twist_ai->get_angle() - Joint::ANGULAR_LIMIT_EPSILON, &matrix0.m_right[0]); NewtonUserJointSetRowMaximumFriction(joint, 0.0f); NewtonUserJointSetRowStiffness(joint, joint_data->m_stiffness); } else bcontinue = true; } else bcontinue = true; if (bcontinue) { // Handle in case limits are not necessary NewtonUserJointAddAngularRow(joint, 0.0f, &matrix0.m_right[0]); NewtonUserJointSetRowAcceleration(joint, -cj_data->m_cur_twist_omega * inv_timestep); NewtonUserJointSetRowMinimumFriction(joint, -power); NewtonUserJointSetRowMaximumFriction(joint, power); NewtonUserJointSetRowStiffness(joint, joint_data->m_stiffness); } }
void CustomControlledBallAndSocket::SubmitConstraints (dFloat timestep, int threadIndex) { dMatrix matrix0; dMatrix matrix1; // calculate the position of the pivot point and the Jacobian direction vectors, in global space. CalculateGlobalMatrix (matrix0, matrix1); // Restrict the movement on the pivot point along all tree orthonormal direction NewtonUserJointAddLinearRow (m_joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix1.m_front[0]); NewtonUserJointAddLinearRow (m_joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix1.m_up[0]); NewtonUserJointAddLinearRow (m_joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix1.m_right[0]); #if 0 dVector euler0; dVector euler1; dMatrix localMatrix (matrix0 * matrix1.Inverse()); localMatrix.GetEulerAngles(euler0, euler1); AngularIntegration pitchStep0 (AngularIntegration (euler0.m_x) - m_pitch); AngularIntegration pitchStep1 (AngularIntegration (euler1.m_x) - m_pitch); if (dAbs (pitchStep0.GetAngle()) > dAbs (pitchStep1.GetAngle())) { euler0 = euler1; } dVector euler (m_pitch.Update (euler0.m_x), m_yaw.Update (euler0.m_y), m_roll.Update (euler0.m_z), 0.0f); for (int i = 0; i < 3; i ++) { dFloat error = m_targetAngles[i] - euler[i]; if (dAbs (error) > (0.125f * 3.14159213f / 180.0f) ) { dFloat angularStep = dSign(error) * m_angulaSpeed * timestep; if (angularStep > 0.0f) { if (angularStep > error) { angularStep = error * 0.5f; } } else { if (angularStep < error) { angularStep = error * 0.5f; } } euler[i] = euler[i] + angularStep; } } dMatrix p0y0r0 (dPitchMatrix(euler[0]) * dYawMatrix(euler[1]) * dRollMatrix(euler[2])); dMatrix baseMatrix (p0y0r0 * matrix1); dMatrix rotation (matrix0.Inverse() * baseMatrix); dQuaternion quat (rotation); if (quat.m_q0 > dFloat (0.99995f)) { dVector euler0; dVector euler1; rotation.GetEulerAngles(euler0, euler1); NewtonUserJointAddAngularRow(m_joint, euler0[0], &rotation[0][0]); NewtonUserJointAddAngularRow(m_joint, euler0[1], &rotation[1][0]); NewtonUserJointAddAngularRow(m_joint, euler0[2], &rotation[2][0]); } else { dMatrix basis (dGrammSchmidt (dVector (quat.m_q1, quat.m_q2, quat.m_q3, 0.0f))); NewtonUserJointAddAngularRow (m_joint, 2.0f * dAcos (quat.m_q0), &basis[0][0]); NewtonUserJointAddAngularRow (m_joint, 0.0f, &basis[1][0]); NewtonUserJointAddAngularRow (m_joint, 0.0f, &basis[2][0]); } #else matrix1 = m_targetRotation * matrix1; dQuaternion localRotation(matrix1 * matrix0.Inverse()); if (localRotation.DotProduct(m_targetRotation) < 0.0f) { localRotation.Scale(-1.0f); } dFloat angle = 2.0f * dAcos(localRotation.m_q0); dFloat angleStep = m_angulaSpeed * timestep; if (angleStep < angle) { dVector axis(dVector(localRotation.m_q1, localRotation.m_q2, localRotation.m_q3, 0.0f)); axis = axis.Scale(1.0f / dSqrt(axis % axis)); // localRotation = dQuaternion(axis, angleStep); } dVector axis (matrix1.m_front * matrix1.m_front); dVector axis1 (matrix1.m_front * matrix1.m_front); //dFloat sinAngle; //dFloat cosAngle; //CalculatePitchAngle (matrix0, matrix1, sinAngle, cosAngle); //float xxxx = dAtan2(sinAngle, cosAngle); //float xxxx1 = dAtan2(sinAngle, cosAngle); dQuaternion quat(localRotation); if (quat.m_q0 > dFloat(0.99995f)) { // dAssert (0); /* dVector euler0; dVector euler1; rotation.GetEulerAngles(euler0, euler1); NewtonUserJointAddAngularRow(m_joint, euler0[0], &rotation[0][0]); NewtonUserJointAddAngularRow(m_joint, euler0[1], &rotation[1][0]); NewtonUserJointAddAngularRow(m_joint, euler0[2], &rotation[2][0]); */ } else { dMatrix basis(dGrammSchmidt(dVector(quat.m_q1, quat.m_q2, quat.m_q3, 0.0f))); NewtonUserJointAddAngularRow(m_joint, -2.0f * dAcos(quat.m_q0), &basis[0][0]); NewtonUserJointAddAngularRow(m_joint, 0.0f, &basis[1][0]); NewtonUserJointAddAngularRow(m_joint, 0.0f, &basis[2][0]); } #endif }
void dCustomBallAndSocket::SubmitConstraints(dFloat timestep, int threadIndex) { dMatrix matrix0; dMatrix matrix1; // calculate the position of the pivot point and the Jacobian direction vectors, in global space. CalculateGlobalMatrix(matrix0, matrix1); SubmitLinearRows(0x07, matrix0, matrix1); const dVector& coneDir0 = matrix0.m_front; const dVector& coneDir1 = matrix1.m_front; dFloat cosAngleCos = coneDir1.DotProduct3(coneDir0); dMatrix coneRotation(dGetIdentityMatrix()); dVector lateralDir(matrix0.m_up); if (cosAngleCos < 0.9999f) { lateralDir = coneDir1.CrossProduct(coneDir0); dFloat mag2 = lateralDir.DotProduct3(lateralDir); if (mag2 > 1.0e-4f) { lateralDir = lateralDir.Scale(1.0f / dSqrt(mag2)); coneRotation = dMatrix(dQuaternion(lateralDir, dAcos(dClamp(cosAngleCos, dFloat(-1.0f), dFloat(1.0f)))), matrix1.m_posit); } else { lateralDir = matrix0.m_up.Scale (-1.0f); coneRotation = dMatrix(dQuaternion(matrix0.m_up, 180 * dDegreeToRad), matrix1.m_posit); } } dVector omega0(0.0f); dVector omega1(0.0f); NewtonBodyGetOmega(m_body0, &omega0[0]); if (m_body1) { NewtonBodyGetOmega(m_body1, &omega1[0]); } dVector relOmega(omega0 - omega1); // do twist angle calculations dMatrix twistMatrix(matrix0 * (matrix1 * coneRotation).Inverse()); dFloat twistAngle = m_twistAngle.Update(dAtan2(twistMatrix[1][2], twistMatrix[1][1])); if (m_options.m_option0) { if ((m_minTwistAngle == 0.0f) && (m_minTwistAngle == 0.0f)) { NewtonUserJointAddAngularRow(m_joint, -twistAngle, &matrix0.m_front[0]); NewtonUserJointSetRowStiffness(m_joint, m_stiffness); } else { if (m_options.m_option1) { // TODO spring option dAssert (0); } else { SubmitConstraintTwistLimits(matrix0, matrix1, relOmega, timestep); } } } else if (m_options.m_option1) { // TODO spring option dAssert (0); } else if (m_twistFriction > 0.0f) { NewtonUserJointAddAngularRow(m_joint, 0, &matrix0.m_front[0]); NewtonUserJointSetRowStiffness(m_joint, m_stiffness); NewtonUserJointSetRowAcceleration(m_joint, NewtonUserJointCalculateRowZeroAccelaration(m_joint)); NewtonUserJointSetRowMinimumFriction(m_joint, -m_twistFriction); NewtonUserJointSetRowMaximumFriction(m_joint, m_twistFriction); } // do twist cone angle calculations if (m_options.m_option2) { if ((m_maxConeAngle == 0.0f)) { dMatrix localMatrix(matrix0 * matrix1.Inverse()); dVector euler0; dVector euler1; localMatrix.GetEulerAngles(euler0, euler1, m_pitchRollYaw); NewtonUserJointAddAngularRow(m_joint, -euler0[1], &matrix1[1][0]); NewtonUserJointSetRowStiffness(m_joint, m_stiffness); NewtonUserJointAddAngularRow(m_joint, -euler0[2], &matrix1[2][0]); NewtonUserJointSetRowStiffness(m_joint, m_stiffness); } else { if (m_options.m_option3) { // TODO spring option dAssert(0); } else { dFloat jointOmega = relOmega.DotProduct3(lateralDir); dFloat currentAngle = dAcos(dClamp(cosAngleCos, dFloat(-1.0f), dFloat(1.0f))); dFloat coneAngle = currentAngle + jointOmega * timestep; if (coneAngle >= m_maxConeAngle) { //dQuaternion rot(lateralDir, coneAngle); //dVector frontDir(rot.RotateVector(coneDir1)); //dVector upDir(lateralDir.CrossProduct(frontDir)); dVector upDir(lateralDir.CrossProduct(coneDir0)); NewtonUserJointAddAngularRow(m_joint, 0.0f, &upDir[0]); NewtonUserJointSetRowAcceleration(m_joint, NewtonUserJointCalculateRowZeroAccelaration(m_joint)); NewtonUserJointSetRowStiffness(m_joint, m_stiffness); NewtonUserJointAddAngularRow(m_joint, 0.0f, &lateralDir[0]); NewtonUserJointSetRowStiffness(m_joint, m_stiffness); NewtonUserJointSetRowMaximumFriction(m_joint, m_coneFriction); const dFloat invtimestep = 1.0f / timestep; const dFloat speed = 0.5f * (m_maxConeAngle - currentAngle) * invtimestep; const dFloat stopAccel = NewtonUserJointCalculateRowZeroAccelaration(m_joint) + speed * invtimestep; NewtonUserJointSetRowAcceleration(m_joint, stopAccel); } else if (m_coneFriction != 0) { NewtonUserJointAddAngularRow(m_joint, 0.0f, &lateralDir[0]); NewtonUserJointSetRowAcceleration(m_joint, NewtonUserJointCalculateRowZeroAccelaration(m_joint)); NewtonUserJointSetRowMinimumFriction(m_joint, -m_coneFriction); NewtonUserJointSetRowMaximumFriction(m_joint, m_coneFriction); dVector upDir(lateralDir.CrossProduct(coneDir0)); NewtonUserJointAddAngularRow(m_joint, 0.0f, &upDir[0]); NewtonUserJointSetRowAcceleration(m_joint, NewtonUserJointCalculateRowZeroAccelaration(m_joint)); NewtonUserJointSetRowMinimumFriction(m_joint, -m_coneFriction); NewtonUserJointSetRowMaximumFriction(m_joint, m_coneFriction); } } } } else if (m_options.m_option3) { // TODO spring option dAssert(0); } else if (m_coneFriction > 0.0f) { NewtonUserJointAddAngularRow(m_joint, 0.0f, &lateralDir[0]); NewtonUserJointSetRowAcceleration(m_joint, NewtonUserJointCalculateRowZeroAccelaration(m_joint)); NewtonUserJointSetRowMinimumFriction(m_joint, -m_coneFriction); NewtonUserJointSetRowMaximumFriction(m_joint, m_coneFriction); dVector upDir(lateralDir.CrossProduct(coneDir0)); NewtonUserJointAddAngularRow(m_joint, 0.0f, &upDir[0]); NewtonUserJointSetRowAcceleration(m_joint, NewtonUserJointCalculateRowZeroAccelaration(m_joint)); NewtonUserJointSetRowMinimumFriction(m_joint, -m_coneFriction); NewtonUserJointSetRowMaximumFriction(m_joint, m_coneFriction); } }
void dCustomBallAndSocket::Debug(dDebugDisplay* const debugDisplay) const { dMatrix matrix0; dMatrix matrix1; dCustomJoint::Debug(debugDisplay); CalculateGlobalMatrix(matrix0, matrix1); const dVector& coneDir0 = matrix0.m_front; const dVector& coneDir1 = matrix1.m_front; dFloat cosAngleCos = coneDir0.DotProduct3(coneDir1); dMatrix coneRotation(dGetIdentityMatrix()); if (cosAngleCos < 0.9999f) { dVector lateralDir(coneDir1.CrossProduct(coneDir0)); dFloat mag2 = lateralDir.DotProduct3(lateralDir); //dAssert(mag2 > 1.0e-4f); if (mag2 > 1.0e-4f) { lateralDir = lateralDir.Scale(1.0f / dSqrt(mag2)); coneRotation = dMatrix(dQuaternion(lateralDir, dAcos(dClamp(cosAngleCos, dFloat(-1.0f), dFloat(1.0f)))), matrix1.m_posit); } else { lateralDir = matrix0.m_up.Scale(-1.0f); coneRotation = dMatrix(dQuaternion(matrix0.m_up, 180 * dDegreeToRad), matrix1.m_posit); } } else if (cosAngleCos < -0.9999f) { coneRotation[0][0] = -1.0f; coneRotation[1][1] = -1.0f; } const int subdiv = 18; const dFloat radius = debugDisplay->m_debugScale; dVector arch[subdiv + 1]; // show twist angle limits if (m_options.m_option0 && ((m_maxTwistAngle - m_minTwistAngle) > dFloat(1.0e-3f))) { dMatrix pitchMatrix(matrix1 * coneRotation); pitchMatrix.m_posit = matrix1.m_posit; dVector point(dFloat(0.0f), dFloat(radius), dFloat(0.0f), dFloat(0.0f)); dFloat angleStep = dMin (m_maxTwistAngle - m_minTwistAngle, dFloat (2.0f * dPi)) / subdiv; dFloat angle0 = m_minTwistAngle; debugDisplay->SetColor(dVector(0.6f, 0.2f, 0.0f, 0.0f)); for (int i = 0; i <= subdiv; i++) { arch[i] = pitchMatrix.TransformVector(dPitchMatrix(angle0).RotateVector(point)); debugDisplay->DrawLine(pitchMatrix.m_posit, arch[i]); angle0 += angleStep; } for (int i = 0; i < subdiv; i++) { debugDisplay->DrawLine(arch[i], arch[i + 1]); } } // show cone angle limits if (m_options.m_option2) { dVector point(radius * dCos(m_maxConeAngle), radius * dSin(m_maxConeAngle), 0.0f, 0.0f); dFloat angleStep = dPi * 2.0f / subdiv; dFloat angle0 = 0.0f; debugDisplay->SetColor(dVector(0.3f, 0.8f, 0.0f, 0.0f)); for (int i = 0; i <= subdiv; i++) { dVector conePoint(dPitchMatrix(angle0).RotateVector(point)); dVector p(matrix1.TransformVector(conePoint)); arch[i] = p; debugDisplay->DrawLine(matrix1.m_posit, p); angle0 += angleStep; } for (int i = 0; i < subdiv; i++) { debugDisplay->DrawLine(arch[i], arch[i + 1]); } } }
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); } } }