void dCustomUpVector::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);
// if the body ha rotated by some amount, the there will be a plane of rotation
dVector lateralDir (matrix0.m_front.CrossProduct(matrix1.m_front));
dFloat mag = lateralDir.DotProduct3(lateralDir);
if (mag > 1.0e-6f) {
// if the side vector is not zero, it means the body has rotated
mag = dSqrt (mag);
lateralDir = lateralDir.Scale (1.0f / mag);
dFloat angle = dAsin (mag);
// add an angular constraint to correct the error angle
NewtonUserJointAddAngularRow (m_joint, angle, &lateralDir[0]);
// in theory only one correction is needed, but this produces instability as the body may move sideway.
// a lateral correction prevent this from happening.
dVector frontDir (lateralDir.CrossProduct(matrix1.m_front));
NewtonUserJointAddAngularRow (m_joint, 0.0f, &frontDir[0]);
} else {
// if the angle error is very small then two angular correction along the plane axis do the trick
NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_up[0]);
NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_right[0]);
}
}
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);
}
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();
}
void CustomConeLimitedBallAndSocket::SubmitConstrainst ()
{
dFloat coneCos;
dMatrix matrix0;
dMatrix matrix1;
// add the tree rows to keep the pivot in place
// calculate the position of the pivot point and the Jacobian direction vectors, in global space.
CalculateGlobalMatrix (m_localMatrix0, m_localMatrix1, 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], &matrix0.m_front[0]);
NewtonUserJointAddLinearRow (m_joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix0.m_up[0]);
NewtonUserJointAddLinearRow (m_joint, &matrix0.m_posit[0], &matrix1.m_posit[0], &matrix0.m_right[0]);
// ///////////////////////////////////////////////////////////////////
//
// add a row to keep the child body inside the cone limit
//
// The child is inside the cone if the dCos of the angle between the pin and
coneCos = matrix0.m_front % matrix1.m_front;
if (coneCos < m_cosConeAngle) {
// the child body has violated the cone limit we need to stop it from keep moving
// for that we are going to pick a point along the the child body front vector
dVector p0 (matrix0.m_posit + matrix0.m_front.Scale(MIN_JOINT_PIN_LENGTH));
dVector p1 (matrix1.m_posit + matrix1.m_front.Scale(MIN_JOINT_PIN_LENGTH));
// get a vectors perpendicular to the plane of motion
dVector lateralDir (matrix0.m_front * matrix1.m_front);
// note this could fail if the angle between matrix0.m_front and matrix1.m_front is 90 degree
dVector unitLateralDir = lateralDir.Scale (1.0f / dSqrt (lateralDir % lateralDir));
// now we will add a constraint row along the lateral direction,
// this will add stability as it will prevent the child body from moving sideways
NewtonUserJointAddLinearRow (m_joint, &p0[0], &p0[0], &unitLateralDir[0]);
// calculate the unit vector tangent to the trajectory
dVector tangentDir (unitLateralDir * matrix0.m_front);
p1 = p0 + (p1 - p0).Scale (0.3f);
NewtonUserJointAddLinearRow (m_joint, &p0[0], &p1[0], &tangentDir[0]);
//we need to allow the body to mo in opposite direction to the penetration
//that can be done by setting the min friction to zero
NewtonUserJointSetRowMinimumFriction (m_joint, 0.0f);
}
}
void CustomLimitBallAndSocket::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);
const dVector& p0 = matrix0.m_posit;
const dVector& p1 = matrix1.m_posit;
// Restrict the movement on the pivot point along all tree orthonormal direction
NewtonUserJointAddLinearRow(m_joint, &p0[0], &p1[0], &matrix1.m_front[0]);
NewtonUserJointAddLinearRow(m_joint, &p0[0], &p1[0], &matrix1.m_up[0]);
NewtonUserJointAddLinearRow(m_joint, &p0[0], &p1[0], &matrix1.m_right[0]);
matrix1 = m_rotationOffset * matrix1;
// handle special case of the joint being a hinge
if (m_coneAngleCos > 0.9999f) {
NewtonUserJointAddAngularRow(m_joint, CalculateAngle (matrix0.m_front, matrix1.m_front, matrix1.m_up), &matrix1.m_up[0]);
NewtonUserJointAddAngularRow(m_joint, CalculateAngle(matrix0.m_front, matrix1.m_front, matrix1.m_right), &matrix1.m_right[0]);
// the joint angle can be determined by getting the angle between any two non parallel vectors
dFloat pitchAngle = CalculateAngle (matrix0.m_up, matrix1.m_up, matrix1.m_front);
if ((m_maxTwistAngle - m_minTwistAngle) < 1.0e-4f) {
NewtonUserJointAddAngularRow(m_joint, pitchAngle, &matrix1.m_front[0]);
} else {
if (pitchAngle > m_maxTwistAngle) {
pitchAngle -= m_maxTwistAngle;
NewtonUserJointAddAngularRow(m_joint, pitchAngle, &matrix0.m_front[0]);
NewtonUserJointSetRowMinimumFriction(m_joint, -0.0f);
} else if (pitchAngle < m_minTwistAngle) {
pitchAngle -= m_minTwistAngle;
NewtonUserJointAddAngularRow(m_joint, pitchAngle, &matrix0.m_front[0]);
NewtonUserJointSetRowMaximumFriction(m_joint, 0.0f);
}
}
} else {
const dVector& coneDir0 = matrix0.m_front;
const dVector& coneDir1 = matrix1.m_front;
dFloat cosAngle = coneDir0 % coneDir1;
if (cosAngle <= m_coneAngleCos) {
dVector lateralDir(coneDir0 * coneDir1);
dFloat mag2 = lateralDir % lateralDir;
dAssert(mag2 > 1.0e-4f);
lateralDir = lateralDir.Scale(1.0f / dSqrt(mag2));
dQuaternion rot(m_coneAngleHalfCos, lateralDir.m_x * m_coneAngleHalfSin, lateralDir.m_y * m_coneAngleHalfSin, lateralDir.m_z * m_coneAngleHalfSin);
dVector frontDir(rot.UnrotateVector(coneDir1));
dVector upDir(lateralDir * frontDir);
NewtonUserJointAddAngularRow(m_joint, 0.0f, &upDir[0]);
NewtonUserJointAddAngularRow(m_joint, CalculateAngle(coneDir0, frontDir, lateralDir), &lateralDir[0]);
NewtonUserJointSetRowMinimumFriction(m_joint, 0.0f);
}
//handle twist angle
dFloat pitchAngle = CalculateAngle (matrix0.m_up, matrix1.m_up, matrix1.m_front);
if ((m_maxTwistAngle - m_minTwistAngle) < 1.0e-4f) {
NewtonUserJointAddAngularRow(m_joint, pitchAngle, &matrix1.m_front[0]);
} else {
if (pitchAngle > m_maxTwistAngle) {
pitchAngle -= m_maxTwistAngle;
NewtonUserJointAddAngularRow(m_joint, pitchAngle, &matrix0.m_front[0]);
NewtonUserJointSetRowMinimumFriction(m_joint, -0.0f);
} else if (pitchAngle < m_minTwistAngle) {
pitchAngle -= m_minTwistAngle;
NewtonUserJointAddAngularRow(m_joint, pitchAngle, &matrix0.m_front[0]);
NewtonUserJointSetRowMaximumFriction(m_joint, 0.0f);
}
}
}
}
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]);
}
}
}
