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 NewtonUserJoint::AddLinearRowJacobian (const dgVector& pivot0, const dgVector& pivot1, const dgVector& dir)
{
dgPointParam pointData;
InitPointParam (pointData, m_stiffness, pivot0, pivot1);
CalculatePointDerivative (m_rows, *m_param, dir, pointData, &m_forceArray[m_rows]);
m_rows ++;
dgAssert (m_rows <= dgInt32 (m_maxDOF));
}
void NewtonUserJoint::AddLinearRowJacobian (const dgVector& pivot0, const dgVector& pivot1, const dgVector& dir)
{
dgPointParam pointData;
InitPointParam (pointData, m_stiffness, pivot0, pivot1);
m_lastPosit0 = pivot0;
m_lastPosit1 = pivot1;
m_lastJointAngle = dgFloat32 (0.0f);
CalculatePointDerivative (m_rows, *m_param, dir, pointData, &m_forceArray[m_rows]);
m_rows ++;
_ASSERTE (m_rows <= dgInt32 (m_maxDOF));
}
void dComplemtaritySolver::dFrictionLessContactJoint::JacobianDerivative (dParamInfo * const constraintParams)
{
for (int i = 0; i < m_count; i ++)
{
dPointDerivativeParam pointData;
InitPointParam (pointData, m_contacts[i].m_point);
CalculatePointDerivative (constraintParams, m_contacts[i].m_normal, pointData);
dVector velocError (pointData.m_veloc1 - pointData.m_veloc0);
//dFloat restitution = 0.05f;
dFloat relVelocErr = velocError % m_contacts[i].m_normal;
dFloat penetration = 0.0f;
dFloat penetrationStiffness = 0.0f;
dFloat penetrationVeloc = penetration * penetrationStiffness;
if (relVelocErr > dFloat (1.0e-3f))
relVelocErr *= (m_restitution + dFloat (1.0f));
constraintParams->m_jointLowFriction[i] = dFloat (0.0f);
constraintParams->m_jointAccel[i] = dMax (dFloat (-4.0f), relVelocErr + penetrationVeloc) * constraintParams->m_timestepInv;
}
}
void dComplentaritySolver::dBilateralJoint::AddLinearRowJacobian (dParamInfo* const constraintParams, const dVector& pivot, const dVector& dir)
{
dPointDerivativeParam pointData;
InitPointParam (pointData, pivot);
CalculatePointDerivative (constraintParams, dir, pointData);
}
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);
}
void dgContact::JacobianContactDerivative (dgContraintDescritor& params, const dgContactMaterial& contact, dgInt32 normalIndex, dgInt32& frictionIndex)
{
dgPointParam pointData;
InitPointParam (pointData, dgFloat32 (1.0f), contact.m_point, contact.m_point);
CalculatePointDerivative (normalIndex, params, contact.m_normal, pointData);
dgVector velocError (pointData.m_veloc1 - pointData.m_veloc0);
dgFloat32 restitution = contact.m_restitution;
dgFloat32 relVelocErr = velocError % contact.m_normal;
dgFloat32 penetration = GetMin (contact.m_penetration, dgFloat32(0.5f));
dgFloat32 penetrationStiffness = dgFloat32 (50.0f) * contact.m_softness;
dgFloat32 penetrationVeloc = penetration * penetrationStiffness;
_ASSERTE (dgAbsf (penetrationVeloc - dgFloat32 (50.0f) * contact.m_softness * GetMin (contact.m_penetration, dgFloat32(0.5f))) < dgFloat32 (1.0e-6f));
if (relVelocErr > REST_RELATIVE_VELOCITY) {
relVelocErr *= (restitution + dgFloat32 (1.0f));
}
params.m_restitution[normalIndex] = restitution;
params.m_penetration[normalIndex] = penetration;
params.m_penetrationStiffness[normalIndex] = penetrationStiffness;
params.m_forceBounds[normalIndex].m_low = dgFloat32 (0.0f);
params.m_forceBounds[normalIndex].m_normalIndex = DG_NORMAL_CONSTRAINT;
params.m_forceBounds[normalIndex].m_jointForce = (dgFloat32*)&contact.m_normal_Force;
params.m_jointStiffness[normalIndex] = dgFloat32 (1.0f);
params.m_isMotor[normalIndex] = 0;
params.m_jointAccel[normalIndex] = GetMax (dgFloat32 (-4.0f), relVelocErr + penetrationVeloc) * params.m_invTimestep;
// params.m_jointAccel[normalIndex] = (penetrationVeloc + relVelocErr) * params.m_invTimestep;
if (contact.m_flags & dgContactMaterial::m_overrideNormalAccel__) {
params.m_jointAccel[normalIndex] += contact.m_normal_Force;
}
// first dir friction force
if (contact.m_flags & dgContactMaterial::m_friction0Enable__) {
dgInt32 jacobIndex = frictionIndex;
frictionIndex += 1;
CalculatePointDerivative (jacobIndex, params, contact.m_dir0, pointData);
relVelocErr = velocError % contact.m_dir0;
params.m_forceBounds[jacobIndex].m_normalIndex = normalIndex;
params.m_jointStiffness[jacobIndex] = dgFloat32 (1.0f);
params.m_restitution[jacobIndex] = dgFloat32 (0.0f);
params.m_penetration[jacobIndex] = dgFloat32 (0.0f);
params.m_penetrationStiffness[jacobIndex] = dgFloat32 (0.0f);
// if (contact.m_override0Accel) {
if (contact.m_flags & dgContactMaterial::m_override0Accel__) {
params.m_jointAccel[jacobIndex] = contact.m_dir0_Force;
params.m_isMotor[jacobIndex] = 1;
} else {
params.m_jointAccel[jacobIndex] = relVelocErr * params.m_invTimestep;
params.m_isMotor[jacobIndex] = 0;
}
if (dgAbsf (relVelocErr) > MAX_DYNAMIC_FRICTION_SPEED) {
params.m_forceBounds[jacobIndex].m_low = -contact.m_dynamicFriction0;
params.m_forceBounds[jacobIndex].m_upper = contact.m_dynamicFriction0;
} else {
params.m_forceBounds[jacobIndex].m_low = -contact.m_staticFriction0;
params.m_forceBounds[jacobIndex].m_upper = contact.m_staticFriction0;
}
params.m_forceBounds[jacobIndex].m_jointForce = (dgFloat32*)&contact.m_dir0_Force;
}
// if (contact.m_friction1Enable) {
if (contact.m_flags & dgContactMaterial::m_friction1Enable__) {
dgInt32 jacobIndex = frictionIndex;
frictionIndex += 1;
CalculatePointDerivative (jacobIndex, params, contact.m_dir1, pointData);
relVelocErr = velocError % contact.m_dir1;
params.m_forceBounds[jacobIndex].m_normalIndex = normalIndex;
params.m_jointStiffness[jacobIndex] = dgFloat32 (1.0f);
params.m_restitution[jacobIndex] = dgFloat32 (0.0f);
params.m_penetration[jacobIndex] = dgFloat32 (0.0f);
params.m_penetrationStiffness[jacobIndex] = dgFloat32 (0.0f);
// if (contact.m_override1Accel) {
if (contact.m_flags & dgContactMaterial::m_override1Accel__) {
_ASSERTE (0);
params.m_jointAccel[jacobIndex] = contact.m_dir1_Force;
params.m_isMotor[jacobIndex] = 1;
} else {
params.m_jointAccel[jacobIndex] = relVelocErr * params.m_invTimestep;
params.m_isMotor[jacobIndex] = 0;
}
if (dgAbsf (relVelocErr) > MAX_DYNAMIC_FRICTION_SPEED) {
params.m_forceBounds[jacobIndex].m_low = - contact.m_dynamicFriction1;
params.m_forceBounds[jacobIndex].m_upper = contact.m_dynamicFriction1;
} else {
params.m_forceBounds[jacobIndex].m_low = - contact.m_staticFriction1;
params.m_forceBounds[jacobIndex].m_upper = contact.m_staticFriction1;
}
params.m_forceBounds[jacobIndex].m_jointForce = (dgFloat32*)&contact.m_dir1_Force;
}
//dgTrace (("p(%f %f %f)\n", params.m_jointAccel[normalIndex], params.m_jointAccel[normalIndex + 1], params.m_jointAccel[normalIndex + 2]));
}
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);
}
void dgContact::JacobianContactDerivative (dgContraintDescritor& params, const dgContactMaterial& contact, dgInt32 normalIndex, dgInt32& frictionIndex)
{
dgPointParam pointData;
dgFloat32 impulseOrForceScale = (params.m_timestep > dgFloat32 (0.0f)) ? params.m_invTimestep : dgFloat32 (1.0f);
InitPointParam (pointData, dgFloat32 (1.0f), contact.m_point, contact.m_point);
CalculatePointDerivative (normalIndex, params, contact.m_normal, pointData);
dgVector velocError (pointData.m_veloc1 - pointData.m_veloc0);
dgFloat32 restitution = contact.m_restitution;
dgFloat32 relVelocErr = velocError % contact.m_normal;
dgFloat32 penetration = dgClamp (contact.m_penetration - DG_RESTING_CONTACT_PENETRATION, dgFloat32(0.0f), dgFloat32(0.5f));
dgFloat32 penetrationStiffness = MAX_PENETRATION_STIFFNESS * contact.m_softness;
dgFloat32 penetrationVeloc = penetration * penetrationStiffness;
dgAssert (dgAbsf (penetrationVeloc - MAX_PENETRATION_STIFFNESS * contact.m_softness * penetration) < dgFloat32 (1.0e-6f));
if (relVelocErr > REST_RELATIVE_VELOCITY) {
relVelocErr *= (restitution + dgFloat32 (1.0f));
}
params.m_restitution[normalIndex] = restitution;
params.m_penetration[normalIndex] = penetration;
params.m_penetrationStiffness[normalIndex] = penetrationStiffness;
params.m_forceBounds[normalIndex].m_low = dgFloat32 (0.0f);
params.m_forceBounds[normalIndex].m_normalIndex = DG_NORMAL_CONSTRAINT;
params.m_forceBounds[normalIndex].m_jointForce = (dgForceImpactPair*) &contact.m_normal_Force;
params.m_jointStiffness[normalIndex] = dgFloat32 (0.5f);
// params.m_jointAccel[normalIndex] = GetMax (dgFloat32 (-4.0f), relVelocErr + penetrationVeloc) * params.m_invTimestep;
params.m_jointAccel[normalIndex] = dgMax (dgFloat32 (-4.0f), relVelocErr + penetrationVeloc) * impulseOrForceScale;
if (contact.m_flags & dgContactMaterial::m_overrideNormalAccel) {
params.m_jointAccel[normalIndex] += contact.m_normal_Force.m_force;
}
// first dir friction force
if (contact.m_flags & dgContactMaterial::m_friction0Enable) {
dgInt32 jacobIndex = frictionIndex;
frictionIndex += 1;
CalculatePointDerivative (jacobIndex, params, contact.m_dir0, pointData);
relVelocErr = velocError % contact.m_dir0;
params.m_forceBounds[jacobIndex].m_normalIndex = (contact.m_flags & dgContactMaterial::m_override0Friction) ? DG_BILATERAL_FRICTION_CONSTRAINT : normalIndex;
params.m_jointStiffness[jacobIndex] = dgFloat32 (0.5f);
params.m_restitution[jacobIndex] = dgFloat32 (0.0f);
params.m_penetration[jacobIndex] = dgFloat32 (0.0f);
params.m_penetrationStiffness[jacobIndex] = dgFloat32 (0.0f);
if (contact.m_flags & dgContactMaterial::m_override0Accel) {
params.m_jointAccel[jacobIndex] = contact.m_dir0_Force.m_force;
} else {
params.m_jointAccel[jacobIndex] = relVelocErr * impulseOrForceScale;
}
if (dgAbsf (relVelocErr) > MAX_DYNAMIC_FRICTION_SPEED) {
params.m_forceBounds[jacobIndex].m_low = -contact.m_dynamicFriction0;
params.m_forceBounds[jacobIndex].m_upper = contact.m_dynamicFriction0;
} else {
params.m_forceBounds[jacobIndex].m_low = -contact.m_staticFriction0;
params.m_forceBounds[jacobIndex].m_upper = contact.m_staticFriction0;
}
params.m_forceBounds[jacobIndex].m_jointForce = (dgForceImpactPair*)&contact.m_dir0_Force;
}
if (contact.m_flags & dgContactMaterial::m_friction1Enable) {
dgInt32 jacobIndex = frictionIndex;
frictionIndex += 1;
CalculatePointDerivative (jacobIndex, params, contact.m_dir1, pointData);
relVelocErr = velocError % contact.m_dir1;
params.m_forceBounds[jacobIndex].m_normalIndex = (contact.m_flags & dgContactMaterial::m_override0Friction) ? DG_BILATERAL_FRICTION_CONSTRAINT : normalIndex;
params.m_jointStiffness[jacobIndex] = dgFloat32 (0.5f);
params.m_restitution[jacobIndex] = dgFloat32 (0.0f);
params.m_penetration[jacobIndex] = dgFloat32 (0.0f);
params.m_penetrationStiffness[jacobIndex] = dgFloat32 (0.0f);
if (contact.m_flags & dgContactMaterial::m_override1Accel) {
params.m_jointAccel[jacobIndex] = contact.m_dir1_Force.m_force;
} else {
params.m_jointAccel[jacobIndex] = relVelocErr * impulseOrForceScale;
}
if (dgAbsf (relVelocErr) > MAX_DYNAMIC_FRICTION_SPEED) {
params.m_forceBounds[jacobIndex].m_low = - contact.m_dynamicFriction1;
params.m_forceBounds[jacobIndex].m_upper = contact.m_dynamicFriction1;
} else {
params.m_forceBounds[jacobIndex].m_low = - contact.m_staticFriction1;
params.m_forceBounds[jacobIndex].m_upper = contact.m_staticFriction1;
}
params.m_forceBounds[jacobIndex].m_jointForce = (dgForceImpactPair*)&contact.m_dir1_Force;
}
}
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);
}
