void dgBilateralConstraint::SetSpringDamperAcceleration (dgInt32 index, dgContraintDescritor& desc, dgFloat32 spring, dgFloat32 damper)
{
if (desc.m_timestep > dgFloat32 (0.0f)) {
dgAssert (m_body1);
const dgJacobian &jacobian0 = desc.m_jacobian[index].m_jacobianM0;
const dgJacobian &jacobian1 = desc.m_jacobian[index].m_jacobianM1;
dgVector veloc0 (m_body0->m_veloc);
dgVector omega0 (m_body0->m_omega);
dgVector veloc1 (m_body1->m_veloc);
dgVector omega1 (m_body1->m_omega);
//dgFloat32 relPosit = (p1Global - p0Global) % jacobian0.m_linear + jointAngle;
dgFloat32 relPosit = desc.m_penetration[index];
dgFloat32 relVeloc = - (veloc0 % jacobian0.m_linear + veloc1 % jacobian1.m_linear + omega0 % jacobian0.m_angular + omega1 % jacobian1.m_angular);
//at = [- ks (x2 - x1) - kd * (v2 - v1) - dt * ks * (v2 - v1)] / [1 + dt * kd + dt * dt * ks]
dgFloat32 dt = desc.m_timestep;
dgFloat32 ks = dgAbsf (spring);
dgFloat32 kd = dgAbsf (damper);
dgFloat32 ksd = dt * ks;
dgFloat32 num = ks * relPosit + kd * relVeloc + ksd * relVeloc;
dgFloat32 den = dt * kd + dt * ksd;
dgFloat32 accel = num / (dgFloat32 (1.0f) + den);
// desc.m_jointStiffness[index] = - den / DG_PSD_DAMP_TOL ;
desc.m_jointStiffness[index] = - dgFloat32 (1.0f) - den / DG_PSD_DAMP_TOL;
SetMotorAcceleration (index, accel, desc);
}
}
void NewtonUserJoint::SetAcceleration (dgFloat32 acceleration)
{
dgInt32 index = m_rows - 1;
if ((index >= 0) && (index < dgInt32 (m_maxDOF))) {
SetMotorAcceleration (index, acceleration, *m_param);
}
}
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::SetSpringDamperAcceleration (dFloat springK, dFloat damperD)
{
dgInt32 index = m_rows - 1;
if ((index >= 0) && (index < dgInt32 (m_maxDOF)))
{
dgFloat32 accel = CalculateSpringDamperAcceleration (index, *m_param, m_lastJointAngle, m_lastPosit0, m_lastPosit1, springK, damperD);
SetMotorAcceleration (index, accel, *m_param);
}
}
void NewtonUserJoint::SetSpringDamperAcceleration (dFloat springK, dFloat springD)
{
dgInt32 index;
index = m_rows - 1;
if ((index >= 0) && (index < dgInt32 (m_maxDOF))) {
dgFloat32 accel;
accel = CalculateSpringDamperAcceleration (index, *m_param, m_lastJointAngle, m_lastPosit0, m_lastPosit1, springK, springD);
_ASSERTE (0);
// m_param->m_jointAccel[index] = accel;
SetMotorAcceleration (index, accel, *m_param);
}
}
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);
}
