dgUnsigned32 dgUpVectorConstraint::JacobianDerivative (dgContraintDescritor& params) { dgMatrix matrix0; dgMatrix matrix1; CalculateGlobalMatrixAndAngle (matrix0, matrix1); dgVector lateralDir (matrix0.m_front * matrix1.m_front); dgInt32 ret = 0; dgFloat32 mag = lateralDir % lateralDir; if (mag > dgFloat32 (1.0e-6f)) { mag = dgSqrt (mag); lateralDir = lateralDir.Scale3 (dgFloat32 (1.0f) / mag); dgFloat32 angle = dgAsin (mag); CalculateAngularDerivative (0, params, lateralDir, m_stiffness, angle, &m_jointForce[0]); dgVector frontDir (lateralDir * matrix1.m_front); CalculateAngularDerivative (1, params, frontDir, m_stiffness, dgFloat32 (0.0f), &m_jointForce[1]); ret = 2; } else { CalculateAngularDerivative (0, params, matrix0.m_up, m_stiffness, 0.0, &m_jointForce[0]); CalculateAngularDerivative (1, params, matrix0.m_right, m_stiffness, dgFloat32 (0.0f), &m_jointForce[1]); ret = 2; } return dgUnsigned32 (ret); }
dgVector dgBallConstraint::GetJointForce () const { dgMatrix matrix0; dgMatrix matrix1; CalculateGlobalMatrixAndAngle (matrix0, matrix1); return dgVector (matrix0.m_front.Scale3 (m_jointForce[0].m_force) + matrix0.m_up.Scale3 (m_jointForce[1].m_force) + matrix0.m_right.Scale3 (m_jointForce[2].m_force)); }
dgUnsigned32 dgHingeConstraint::JacobianDerivative (dgContraintDescritor& params) { dgMatrix matrix0; dgMatrix matrix1; dgVector angle (CalculateGlobalMatrixAndAngle (matrix0, matrix1)); m_angle = -angle.m_x; dgAssert (dgAbsf (1.0f - (matrix0.m_front % matrix0.m_front)) < dgFloat32 (1.0e-5f)); dgAssert (dgAbsf (1.0f - (matrix0.m_up % matrix0.m_up)) < dgFloat32 (1.0e-5f)); dgAssert (dgAbsf (1.0f - (matrix0.m_right % matrix0.m_right)) < dgFloat32 (1.0e-5f)); const dgVector& dir0 = matrix0.m_front; const dgVector& dir1 = matrix0.m_up; const dgVector& dir2 = matrix0.m_right; const dgVector& p0 = matrix0.m_posit; const dgVector& p1 = matrix1.m_posit; dgVector q0 (p0 + matrix0.m_front.Scale3(MIN_JOINT_PIN_LENGTH)); dgVector q1 (p1 + matrix1.m_front.Scale3(MIN_JOINT_PIN_LENGTH)); // dgAssert (((p1 - p0) % (p1 - p0)) < 1.0e-2f); dgPointParam pointDataP; dgPointParam pointDataQ; InitPointParam (pointDataP, m_stiffness, p0, p1); InitPointParam (pointDataQ, m_stiffness, q0, q1); CalculatePointDerivative (0, params, dir0, pointDataP, &m_jointForce[0]); CalculatePointDerivative (1, params, dir1, pointDataP, &m_jointForce[1]); CalculatePointDerivative (2, params, dir2, pointDataP, &m_jointForce[2]); CalculatePointDerivative (3, params, dir1, pointDataQ, &m_jointForce[3]); CalculatePointDerivative (4, params, dir2, pointDataQ, &m_jointForce[4]); dgInt32 ret = 5; if (m_jointAccelFnt) { dgJointCallbackParam axisParam; axisParam.m_accel = dgFloat32 (0.0f); axisParam.m_timestep = params.m_timestep; axisParam.m_minFriction = DG_MIN_BOUND; axisParam.m_maxFriction = DG_MAX_BOUND; if (m_jointAccelFnt (*this, &axisParam)) { if ((axisParam.m_minFriction > DG_MIN_BOUND) || (axisParam.m_maxFriction < DG_MAX_BOUND)) { params.m_forceBounds[5].m_low = axisParam.m_minFriction; params.m_forceBounds[5].m_upper = axisParam.m_maxFriction; params.m_forceBounds[5].m_normalIndex = DG_BILATERAL_FRICTION_CONSTRAINT; } CalculateAngularDerivative (5, params, dir0, m_stiffness, dgFloat32 (0.0f), &m_jointForce[5]); // params.m_jointAccel[5] = axisParam.m_accel; SetMotorAcceleration (5, axisParam.m_accel, params); ret = 6; } } return dgUnsigned32 (ret); }
void dgBallConstraint::SetLimits(const dgVector& coneDir, dgFloat32 minConeAngle, dgFloat32 maxConeAngle, dgFloat32 maxTwistAngle, const dgVector& bilateralDir, dgFloat32 negativeBilateralConeAngle__, dgFloat32 positiveBilateralConeAngle__) { dgMatrix matrix0; dgMatrix matrix1; CalculateGlobalMatrixAndAngle(matrix0, matrix1); _ASSERTE(m_body0); _ASSERTE(m_body1); const dgMatrix& body0_Matrix = m_body0->GetMatrix(); dgVector lateralDir(bilateralDir * coneDir); if ((lateralDir % lateralDir) < dgFloat32(1.0e-3f)) { dgMatrix tmp(coneDir); lateralDir = tmp.m_up; } m_localMatrix0.m_front = body0_Matrix.UnrotateVector(coneDir); m_localMatrix0.m_up = body0_Matrix.UnrotateVector(lateralDir); m_localMatrix0.m_posit = body0_Matrix.UntransformVector(matrix1.m_posit); m_localMatrix0.m_front = m_localMatrix0.m_front.Scale( dgFloat32( 1.0f) / dgSqrt (m_localMatrix0.m_front % m_localMatrix0.m_front)); m_localMatrix0.m_up = m_localMatrix0.m_up.Scale( dgFloat32(1.0f) / dgSqrt (m_localMatrix0.m_up % m_localMatrix0.m_up)); m_localMatrix0.m_right = m_localMatrix0.m_front * m_localMatrix0.m_up; m_localMatrix0.m_front.m_w = dgFloat32(0.0f); m_localMatrix0.m_up.m_w = dgFloat32(0.0f); m_localMatrix0.m_right.m_w = dgFloat32(0.0f); m_localMatrix0.m_posit.m_w = dgFloat32(1.0f); // dgMatrix body1_Matrix (dgGetIdentityMatrix()); // if (m_body1) { // body1_Matrix = m_body1->GetMatrix(); // } const dgMatrix& body1_Matrix = m_body1->GetMatrix(); m_twistAngle = ClampValue(maxTwistAngle, dgFloat32(5.0f) * dgDEG2RAD, dgFloat32(90.0f) * dgDEG2RAD); m_coneAngle = ClampValue((maxConeAngle - minConeAngle) * dgFloat32(0.5f), dgFloat32(5.0f) * dgDEG2RAD, 175.0f * dgDEG2RAD); m_coneAngleCos = dgCos (m_coneAngle); dgMatrix coneMatrix( dgPitchMatrix((maxConeAngle + minConeAngle) * dgFloat32(0.5f))); m_localMatrix0 = coneMatrix * m_localMatrix0; m_localMatrix1 = m_localMatrix0 * body0_Matrix * body1_Matrix.Inverse(); }
dgVector dgSlidingConstraint::GetJointForce () const { dgMatrix matrix0; dgMatrix matrix1; CalculateGlobalMatrixAndAngle (matrix0, matrix1); return dgVector (matrix0.m_up.Scale (m_jointForce[0]) + matrix0.m_right.Scale (m_jointForce[1]) + matrix0.m_up.Scale (m_jointForce[2]) + matrix0.m_right.Scale (m_jointForce[3]) + matrix0.m_right.Scale (m_jointForce[4])); }
void dgBallConstraint::SetPivotPoint(const dgVector &pivot) { dgAssert (m_body0); dgAssert (m_body1); const dgMatrix& matrix = m_body0->GetMatrix(); dgVector pin (pivot - matrix.m_posit); if ((pin % pin) < dgFloat32 (1.0e-3f)) { pin = matrix.m_front; } SetPivotAndPinDir (pivot, pin); dgMatrix matrix0; dgMatrix matrix1; CalculateGlobalMatrixAndAngle (matrix0, matrix1); SetLimits (matrix0.m_front, -dgPI * dgFloat32 (0.5f), dgPI * dgFloat32 (0.5f), dgPI * dgFloat32 (0.5f), matrix0.m_right, dgFloat32 (0.0f), dgFloat32 (0.0f)); }
dgUnsigned32 dgBallConstraint::JacobianDerivative(dgContraintDescritor& params) { dgInt32 ret; dgFloat32 relVelocErr; dgFloat32 penetrationErr; dgMatrix matrix0; dgMatrix matrix1; if (m_jointUserCallback) { m_jointUserCallback(*this, params.m_timestep); } dgVector angle(CalculateGlobalMatrixAndAngle(matrix0, matrix1)); m_angles = angle.Scale(-dgFloat32(1.0f)); const dgVector& dir0 = matrix0.m_front; const dgVector& dir1 = matrix0.m_up; const dgVector& dir2 = matrix0.m_right; const dgVector& p0 = matrix0.m_posit; const dgVector& p1 = matrix1.m_posit; dgPointParam pointData; InitPointParam(pointData, m_stiffness, p0, p1); CalculatePointDerivative(0, params, dir0, pointData, &m_jointForce[0]); CalculatePointDerivative(1, params, dir1, pointData, &m_jointForce[1]); CalculatePointDerivative(2, params, dir2, pointData, &m_jointForce[2]); ret = 3; if (m_twistLimit) { if (angle.m_x > m_twistAngle) { dgVector p0(matrix0.m_posit + matrix0.m_up.Scale(MIN_JOINT_PIN_LENGTH)); InitPointParam(pointData, m_stiffness, p0, p0); const dgVector& dir = matrix0.m_right; CalculatePointDerivative(ret, params, dir, pointData, &m_jointForce[ret]); dgVector velocError(pointData.m_veloc1 - pointData.m_veloc0); relVelocErr = velocError % dir; if (relVelocErr > dgFloat32(1.0e-3f)) { relVelocErr *= dgFloat32(1.1f); } penetrationErr = MIN_JOINT_PIN_LENGTH * (angle.m_x - m_twistAngle); _ASSERTE(penetrationErr >= dgFloat32 (0.0f)); params.m_forceBounds[ret].m_low = dgFloat32(0.0f); params.m_forceBounds[ret].m_normalIndex = DG_NORMAL_CONSTRAINT; params.m_forceBounds[ret].m_jointForce = &m_jointForce[ret]; // params.m_jointAccel[ret] = (relVelocErr + penetrationErr) * params.m_invTimestep; SetMotorAcceleration(ret, (relVelocErr + penetrationErr) * params.m_invTimestep, params); ret++; } else if (angle.m_x < -m_twistAngle) { dgVector p0(matrix0.m_posit + matrix0.m_up.Scale(MIN_JOINT_PIN_LENGTH)); InitPointParam(pointData, m_stiffness, p0, p0); dgVector dir(matrix0.m_right.Scale(-dgFloat32(1.0f))); CalculatePointDerivative(ret, params, dir, pointData, &m_jointForce[ret]); dgVector velocError(pointData.m_veloc1 - pointData.m_veloc0); relVelocErr = velocError % dir; if (relVelocErr > dgFloat32(1.0e-3f)) { relVelocErr *= dgFloat32(1.1f); } penetrationErr = MIN_JOINT_PIN_LENGTH * (-m_twistAngle - angle.m_x); _ASSERTE(penetrationErr >= dgFloat32 (0.0f)); params.m_forceBounds[ret].m_low = dgFloat32(0.0f); params.m_forceBounds[ret].m_normalIndex = DG_NORMAL_CONSTRAINT; params.m_forceBounds[ret].m_jointForce = &m_jointForce[ret]; // params.m_jointAccel[ret] = (relVelocErr + penetrationErr) * params.m_invTimestep; SetMotorAcceleration(ret, (relVelocErr + penetrationErr) * params.m_invTimestep, params); ret++; } } if (m_coneLimit) { dgFloat32 coneCos; coneCos = matrix0.m_front % matrix1.m_front; if (coneCos < m_coneAngleCos) { dgVector p0( matrix0.m_posit + matrix0.m_front.Scale(MIN_JOINT_PIN_LENGTH)); InitPointParam(pointData, m_stiffness, p0, p0); dgVector tangentDir(matrix0.m_front * matrix1.m_front); tangentDir = tangentDir.Scale( dgRsqrt ((tangentDir % tangentDir) + 1.0e-8f)); CalculatePointDerivative(ret, params, tangentDir, pointData, &m_jointForce[ret]); ret++; dgVector normalDir(tangentDir * matrix0.m_front); dgVector velocError(pointData.m_veloc1 - pointData.m_veloc0); //restitution = contact.m_restitution; relVelocErr = velocError % normalDir; if (relVelocErr > dgFloat32(1.0e-3f)) { relVelocErr *= dgFloat32(1.1f); } penetrationErr = MIN_JOINT_PIN_LENGTH * (dgAcos (GetMax (coneCos, dgFloat32(-0.9999f))) - m_coneAngle); _ASSERTE(penetrationErr >= dgFloat32 (0.0f)); CalculatePointDerivative(ret, params, normalDir, pointData, &m_jointForce[ret]); params.m_forceBounds[ret].m_low = dgFloat32(0.0f); params.m_forceBounds[ret].m_normalIndex = DG_NORMAL_CONSTRAINT; params.m_forceBounds[ret].m_jointForce = &m_jointForce[ret]; // params.m_jointAccel[ret] = (relVelocErr + penetrationErr) * params.m_invTimestep; SetMotorAcceleration(ret, (relVelocErr + penetrationErr) * params.m_invTimestep, params); ret++; } } return dgUnsigned32(ret); }
dgUnsigned32 dgUniversalConstraint::JacobianDerivative (dgContraintDescritor& params) { dgInt32 ret; dgFloat32 sinAngle; dgFloat32 cosAngle; dgMatrix matrix0; dgMatrix matrix1; CalculateGlobalMatrixAndAngle (matrix0, matrix1); const dgVector& dir0 = matrix0.m_front; const dgVector& dir1 = matrix1.m_up; dgVector dir2 (dir0 * dir1); dgVector dir3 (dir2 * dir0); dir3 = dir3.Scale3 (dgRsqrt (dir3 % dir3)); const dgVector& p0 = matrix0.m_posit; const dgVector& p1 = matrix1.m_posit; dgVector q0 (p0 + dir3.Scale3(MIN_JOINT_PIN_LENGTH)); dgVector q1 (p1 + dir1.Scale3(MIN_JOINT_PIN_LENGTH)); dgPointParam pointDataP; dgPointParam pointDataQ; InitPointParam (pointDataP, m_stiffness, p0, p1); InitPointParam (pointDataQ, m_stiffness, q0, q1); CalculatePointDerivative (0, params, dir0, pointDataP, &m_jointForce[0]); CalculatePointDerivative (1, params, dir1, pointDataP, &m_jointForce[1]); CalculatePointDerivative (2, params, dir2, pointDataP, &m_jointForce[2]); CalculatePointDerivative (3, params, dir0, pointDataQ, &m_jointForce[3]); ret = 4; // dgVector sinAngle0 (matrix1.m_up * matrix0.m_up); // m_angle0 = dgAsin (ClampValue (sinAngle0 % dir0, -0.9999999f, 0.9999999f)); // if ((matrix0.m_up % matrix1.m_up) < dgFloat32 (0.0f)) { // m_angle0 = (m_angle0 >= dgFloat32 (0.0f)) ? dgPI - m_angle0 : dgPI + m_angle0; // } sinAngle = (matrix1.m_up * matrix0.m_up) % matrix0.m_front; cosAngle = matrix0.m_up % matrix1.m_up; // dgAssert (dgAbsf (m_angle0 - dgAtan2 (sinAngle, cosAngle)) < 1.0e-1f); m_angle0 = dgAtan2 (sinAngle, cosAngle); // dgVector sinAngle1 (matrix0.m_front * matrix1.m_front); // m_angle1 = dgAsin (ClampValue (sinAngle1 % dir1, -0.9999999f, 0.9999999f)); // if ((matrix0.m_front % matrix1.m_front) < dgFloat32 (0.0f)) { // m_angle1 = (m_angle1 >= dgFloat32 (0.0f)) ? dgPI - m_angle1 : dgPI + m_angle1; // } sinAngle = (matrix0.m_front * matrix1.m_front) % matrix1.m_up; cosAngle = matrix0.m_front % matrix1.m_front; // dgAssert (dgAbsf (m_angle1 - dgAtan2 (sinAngle, cosAngle)) < 1.0e-1f); m_angle1 = dgAtan2 (sinAngle, cosAngle); if (m_jointAccelFnt) { dgUnsigned32 code; dgJointCallbackParam axisParam[2]; // linear acceleration axisParam[0].m_accel = dgFloat32 (0.0f); axisParam[0].m_timestep = params.m_timestep; axisParam[0].m_minFriction = DG_MIN_BOUND; axisParam[0].m_maxFriction = DG_MAX_BOUND; // angular acceleration axisParam[1].m_accel = dgFloat32 (0.0f); axisParam[1].m_timestep = params.m_timestep; axisParam[1].m_minFriction = DG_MIN_BOUND; axisParam[1].m_maxFriction = DG_MAX_BOUND; code = m_jointAccelFnt (*this, axisParam); if (code & 1) { if ((axisParam[0].m_minFriction > DG_MIN_BOUND) || (axisParam[0].m_maxFriction < DG_MAX_BOUND)) { params.m_forceBounds[ret].m_low = axisParam[0].m_minFriction; params.m_forceBounds[ret].m_upper = axisParam[0].m_maxFriction; params.m_forceBounds[ret].m_normalIndex = DG_BILATERAL_FRICTION_CONSTRAINT; } // CalculatePointDerivative (ret, params, dir0, pointDataP, &m_jointForce[ret]); CalculateAngularDerivative (ret, params, dir0, m_stiffness, dgFloat32 (0.0f), &m_jointForce[ret]); //params.m_jointAccel[ret] = axisParam[0].m_accel; SetMotorAcceleration (ret, axisParam[0].m_accel, params); ret ++; } if (code & 2) { if ((axisParam[1].m_minFriction > DG_MIN_BOUND) || (axisParam[1].m_maxFriction < DG_MAX_BOUND)) { params.m_forceBounds[ret].m_low = axisParam[1].m_minFriction; params.m_forceBounds[ret].m_upper = axisParam[1].m_maxFriction; params.m_forceBounds[ret].m_normalIndex = DG_BILATERAL_FRICTION_CONSTRAINT; } CalculateAngularDerivative (ret, params, dir1, m_stiffness, dgFloat32 (0.0f), &m_jointForce[ret]); //params.m_jointAccel[ret] = axisParam[1].m_accel; SetMotorAcceleration (ret, axisParam[1].m_accel, params); ret ++; } } return dgUnsigned32 (ret); }
dgUnsigned32 dgCorkscrewConstraint::JacobianDerivative (dgContraintDescritor& params) { dgMatrix matrix0; dgMatrix matrix1; dgVector angle (CalculateGlobalMatrixAndAngle (matrix0, matrix1)); m_angle = -angle.m_x; m_posit = (matrix0.m_posit - matrix1.m_posit) % matrix0.m_front; matrix1.m_posit += matrix1.m_front.Scale3 (m_posit); dgAssert (dgAbsf (dgFloat32 (1.0f) - (matrix0.m_front % matrix0.m_front)) < dgFloat32 (1.0e-5f)); dgAssert (dgAbsf (dgFloat32 (1.0f) - (matrix0.m_up % matrix0.m_up)) < dgFloat32 (1.0e-5f)); dgAssert (dgAbsf (dgFloat32 (1.0f) - (matrix0.m_right % matrix0.m_right)) < dgFloat32 (1.0e-5f)); const dgVector& dir1 = matrix0.m_up; const dgVector& dir2 = matrix0.m_right; // const dgVector& p0 = matrix0.m_posit; // const dgVector& p1 = matrix1.m_posit; dgVector p0 (matrix0.m_posit); dgVector p1 (matrix1.m_posit + matrix1.m_front.Scale3 ((p0 - matrix1.m_posit) % matrix1.m_front)); dgVector q0 (p0 + matrix0.m_front.Scale3(MIN_JOINT_PIN_LENGTH)); dgVector q1 (p1 + matrix1.m_front.Scale3(MIN_JOINT_PIN_LENGTH)); dgPointParam pointDataP; dgPointParam pointDataQ; InitPointParam (pointDataP, m_stiffness, p0, p1); InitPointParam (pointDataQ, m_stiffness, q0, q1); CalculatePointDerivative (0, params, dir1, pointDataP, &m_jointForce[0]); CalculatePointDerivative (1, params, dir2, pointDataP, &m_jointForce[1]); CalculatePointDerivative (2, params, dir1, pointDataQ, &m_jointForce[2]); CalculatePointDerivative (3, params, dir2, pointDataQ, &m_jointForce[3]); dgInt32 ret = 4; if (m_jointAccelFnt) { dgUnsigned32 code; dgJointCallbackParam axisParam[2]; // linear acceleration axisParam[0].m_accel = dgFloat32 (0.0f); axisParam[0].m_timestep = params.m_timestep; axisParam[0].m_minFriction = DG_MIN_BOUND; axisParam[0].m_maxFriction = DG_MAX_BOUND; // angular acceleration axisParam[1].m_accel = dgFloat32 (0.0f); axisParam[1].m_timestep = params.m_timestep; axisParam[1].m_minFriction = DG_MIN_BOUND; axisParam[1].m_maxFriction = DG_MAX_BOUND; code = m_jointAccelFnt (*this, axisParam); if (code & 1) { if ((axisParam[0].m_minFriction > DG_MIN_BOUND) || (axisParam[0].m_maxFriction < DG_MAX_BOUND)) { params.m_forceBounds[ret].m_low = axisParam[0].m_minFriction; params.m_forceBounds[ret].m_upper = axisParam[0].m_maxFriction; params.m_forceBounds[ret].m_normalIndex = DG_BILATERAL_FRICTION_CONSTRAINT; } CalculatePointDerivative (ret, params, matrix0.m_front, pointDataP, &m_jointForce[ret]); //params.m_jointAccel[ret] = axisParam[0].m_accel; SetMotorAcceleration (ret, axisParam[0].m_accel, params); ret ++; } if (code & 2) { if ((axisParam[1].m_minFriction > DG_MIN_BOUND) || (axisParam[1].m_maxFriction < DG_MAX_BOUND)) { params.m_forceBounds[ret].m_low = axisParam[1].m_minFriction; params.m_forceBounds[ret].m_upper = axisParam[1].m_maxFriction; params.m_forceBounds[ret].m_normalIndex = DG_BILATERAL_FRICTION_CONSTRAINT; } // dgVector p (p0 + dir1); // dgPointParam pointData; // InitPointParam (pointData, m_stiffness, p, p); // CalculatePointDerivative (ret, params, dir2, pointData, &m_jointForce[ret]); CalculateAngularDerivative (ret, params, matrix0.m_front, m_stiffness, dgFloat32 (0.0f), &m_jointForce[ret]); //params.m_jointAccel[ret] = axisParam[1].m_accel; SetMotorAcceleration (ret, axisParam[1].m_accel, params); ret ++; } } return dgUnsigned32 (ret); }