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);
}
}
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);
}
