protected void InternalUpdateSprings(ConstraintInfo2 info)
{
// it is assumed that calculateTransforms() have been called before this call
IndexedVector3 relVel = m_rbB.GetLinearVelocity() - m_rbA.GetLinearVelocity();
for (int i = 0; i < 3; i++)
{
if (m_springEnabled[i])
{
// get current position of constraint
float currPos = m_calculatedLinearDiff[i];
// calculate difference
float delta = currPos - m_equilibriumPoint[i];
// spring force is (delta * m_stiffness) according to Hooke's Law
float force = delta * m_springStiffness[i];
float velFactor = info.fps * m_springDamping[i] / (float)info.m_numIterations;
m_linearLimits.m_targetVelocity[i] = velFactor * force;
m_linearLimits.m_maxMotorForce[i] = Math.Abs(force) / info.fps;
}
}
for (int i = 0; i < 3; i++)
{
if (m_springEnabled[i + 3])
{
// get current position of constraint
float currPos = m_calculatedAxisAngleDiff[i];
// calculate difference
float delta = currPos - m_equilibriumPoint[i + 3];
// spring force is (-delta * m_stiffness) according to Hooke's Law
float force = -delta * m_springStiffness[i + 3];
float velFactor = info.fps * m_springDamping[i + 3] / (float)info.m_numIterations;
m_angularLimits[i].m_targetVelocity = velFactor * force;
m_angularLimits[i].m_maxMotorForce = Math.Abs(force) / info.fps;
}
}
}
protected virtual int SetAngularLimits(ConstraintInfo2 info, int row_offset, ref IndexedMatrix transA, ref IndexedMatrix transB, ref IndexedVector3 linVelA, ref IndexedVector3 linVelB, ref IndexedVector3 angVelA, ref IndexedVector3 angVelB)
{
Generic6DofConstraint d6constraint = this;
int row = row_offset;
//solve angular limits
for (int i = 0; i < 3; i++)
{
if (d6constraint.GetRotationalLimitMotor(i).NeedApplyTorques())
{
IndexedVector3 axis = d6constraint.GetAxis(i);
int tempFlags = ((int)m_flags) >> ((i + 3) * BT_6DOF_FLAGS_AXIS_SHIFT);
SixDofFlags flags = (SixDofFlags)tempFlags;
if (0 == (flags & SixDofFlags.BT_6DOF_FLAGS_CFM_NORM))
{
m_angularLimits[i].m_normalCFM = info.m_solverConstraints[0].m_cfm;
}
if (0 == (flags & SixDofFlags.BT_6DOF_FLAGS_CFM_STOP))
{
m_angularLimits[i].m_stopCFM = info.m_solverConstraints[0].m_cfm;
}
if (0 == (flags & SixDofFlags.BT_6DOF_FLAGS_ERP_STOP))
{
m_angularLimits[i].m_stopERP = info.erp;
}
row += GetLimitMotorInfo2(d6constraint.GetRotationalLimitMotor(i),
ref transA, ref transB, ref linVelA, ref linVelB, ref angVelA, ref angVelB, info, row, ref axis, 1, false);
}
}
return row;
}
public override void GetInfo2(ConstraintInfo2 info)
{
if (m_useOffsetForConstraintFrame)
{
GetInfo2InternalUsingFrameOffset(info, m_rbA.GetCenterOfMassTransform(), m_rbB.GetCenterOfMassTransform(), m_rbA.GetAngularVelocity(), m_rbB.GetAngularVelocity());
}
else
{
GetInfo2Internal(info, m_rbA.GetCenterOfMassTransform(), m_rbB.GetCenterOfMassTransform(), m_rbA.GetAngularVelocity(), m_rbB.GetAngularVelocity());
}
}
protected void InternalUpdateSprings(ConstraintInfo2 info)
{
// it is assumed that calculateTransforms() have been called before this call
IndexedVector3 relVel = m_rbB.GetLinearVelocity() - m_rbA.GetLinearVelocity();
for (int i = 0; i < 3; i++)
{
if (m_springEnabled[i])
{
// get current position of constraint
float currPos = m_calculatedLinearDiff[i];
// calculate difference
float delta = currPos - m_equilibriumPoint[i];
// spring force is (delta * m_stiffness) according to Hooke's Law
float force = delta * m_springStiffness[i];
float velFactor = info.fps * m_springDamping[i] / (float)info.m_numIterations;
m_linearLimits.m_targetVelocity[i] = velFactor * force;
m_linearLimits.m_maxMotorForce[i] = Math.Abs(force) / info.fps;
}
}
for (int i = 0; i < 3; i++)
{
if (m_springEnabled[i + 3])
{
// get current position of constraint
float currPos = m_calculatedAxisAngleDiff[i];
// calculate difference
float delta = currPos - m_equilibriumPoint[i + 3];
// spring force is (-delta * m_stiffness) according to Hooke's Law
float force = -delta * m_springStiffness[i + 3];
float velFactor = info.fps * m_springDamping[i + 3] / (float)info.m_numIterations;
m_angularLimits[i].m_targetVelocity = velFactor * force;
m_angularLimits[i].m_maxMotorForce = Math.Abs(force) / info.fps;
}
}
}
public virtual void GetInfo2(ConstraintInfo2 info)
{
}
public virtual void GetInfo2(ConstraintInfo2 info)
{
}
public override void GetInfo2(ConstraintInfo2 info)
{
GetInfo2NonVirtual(info, m_rbA.GetCenterOfMassTransform(), m_rbB.GetCenterOfMassTransform(), m_rbA.GetLinearVelocity(), m_rbB.GetLinearVelocity(), m_rbA.GetInvMass(), m_rbB.GetInvMass());
}
public void TestLinLimits2(ConstraintInfo2 info)
{
}
public override void GetInfo2(ConstraintInfo2 info)
{
GetInfo2NonVirtual(info, m_rbA.GetCenterOfMassTransform(), m_rbB.GetCenterOfMassTransform(), m_rbA.GetLinearVelocity(), m_rbB.GetLinearVelocity(), m_rbA.GetInvMass(), m_rbB.GetInvMass());
}
public void GetInfo2InternalUsingFrameOffset(ConstraintInfo2 info, IndexedMatrix transA, IndexedMatrix transB, IndexedVector3 angVelA, IndexedVector3 angVelB)
{
GetInfo2InternalUsingFrameOffset(info, ref transA, ref transB, ref angVelA, ref angVelB);
}
public override void GetInfo2(ConstraintInfo2 info)
{
}
public void GetInfo2NonVirtual(ConstraintInfo2 info, IndexedMatrix body0_trans, IndexedMatrix body1_trans)
{
// anchor points in global coordinates with respect to body PORs.
// set jacobian
info.m_solverConstraints[0].m_contactNormal.X = 1;
info.m_solverConstraints[1].m_contactNormal.Y = 1;
info.m_solverConstraints[2].m_contactNormal.Z = 1;
IndexedVector3 a1 = body0_trans._basis * GetPivotInA();
{
IndexedVector3 a1neg = -a1;
MathUtil.GetSkewSymmetricMatrix(ref a1neg,
out info.m_solverConstraints[0].m_relpos1CrossNormal,
out info.m_solverConstraints[1].m_relpos1CrossNormal,
out info.m_solverConstraints[2].m_relpos1CrossNormal);
}
/*info->m_J2linearAxis[0] = -1;
* info->m_J2linearAxis[s+1] = -1;
* info->m_J2linearAxis[2*s+2] = -1;
*/
IndexedVector3 a2 = body1_trans._basis * GetPivotInB();
{
IndexedVector3 a2n = -a2;
MathUtil.GetSkewSymmetricMatrix(ref a2,
out info.m_solverConstraints[0].m_relpos2CrossNormal,
out info.m_solverConstraints[1].m_relpos2CrossNormal,
out info.m_solverConstraints[2].m_relpos2CrossNormal);
}
// set right hand side
float currERP = ((m_flags & Point2PointFlags.BT_P2P_FLAGS_ERP) != 0) ? m_erp : info.erp;
float k = info.fps * currERP;
int j;
IndexedVector3 body0Origin = body0_trans._origin;
IndexedVector3 body1Origin = body1_trans._origin;
for (j = 0; j < 3; j++)
{
info.m_solverConstraints[j].m_rhs = k * (a2[j] + body1Origin[j] - a1[j] - body0Origin[j]);
//printf("info->m_constraintError[%d]=%f\n",j,info->m_constraintError[j]);
}
if ((m_flags & Point2PointFlags.BT_P2P_FLAGS_CFM) != 0)
{
for (j = 0; j < 3; j++)
{
info.m_solverConstraints[j].m_cfm = m_cfm;
}
}
float impulseClamp = m_setting.m_impulseClamp;//
for (j = 0; j < 3; j++)
{
if (m_setting.m_impulseClamp > 0)
{
info.m_solverConstraints[j].m_lowerLimit = -impulseClamp;
info.m_solverConstraints[j].m_upperLimit = impulseClamp;
}
}
info.m_damping = m_setting.m_damping;
}
public void GetInfo2NonVirtual(ConstraintInfo2 info, IndexedMatrix transA, IndexedMatrix transB, IndexedVector3 angVelA, IndexedVector3 angVelB)
{
///the regular (virtual) implementation getInfo2 already performs 'testLimit' during getInfo1, so we need to do it now
TestLimit(ref transA, ref transB);
GetInfo2Internal(info, transA, transB, angVelA, angVelB);
}
public void GetInfo2NonVirtual(ConstraintInfo2 info,IndexedMatrix body0_trans,IndexedMatrix body1_trans)
{
// anchor points in global coordinates with respect to body PORs.
// set jacobian
info.m_solverConstraints[0].m_contactNormal.X = 1;
info.m_solverConstraints[1].m_contactNormal.Y = 1;
info.m_solverConstraints[2].m_contactNormal.Z = 1;
IndexedVector3 a1 = body0_trans._basis * GetPivotInA();
{
IndexedVector3 a1neg = -a1;
MathUtil.GetSkewSymmetricMatrix(ref a1neg,
out info.m_solverConstraints[0].m_relpos1CrossNormal,
out info.m_solverConstraints[1].m_relpos1CrossNormal,
out info.m_solverConstraints[2].m_relpos1CrossNormal);
}
/*info->m_J2linearAxis[0] = -1;
info->m_J2linearAxis[s+1] = -1;
info->m_J2linearAxis[2*s+2] = -1;
*/
IndexedVector3 a2 = body1_trans._basis * GetPivotInB();
{
IndexedVector3 a2n = -a2;
MathUtil.GetSkewSymmetricMatrix(ref a2,
out info.m_solverConstraints[0].m_relpos2CrossNormal,
out info.m_solverConstraints[1].m_relpos2CrossNormal,
out info.m_solverConstraints[2].m_relpos2CrossNormal);
}
// set right hand side
float currERP = ((m_flags & Point2PointFlags.BT_P2P_FLAGS_ERP) != 0) ? m_erp : info.erp;
float k = info.fps * currERP;
int j;
IndexedVector3 body0Origin = body0_trans._origin;
IndexedVector3 body1Origin = body1_trans._origin;
for (j = 0; j < 3; j++)
{
info.m_solverConstraints[j].m_rhs = k * (a2[j] + body1Origin[j] - a1[j] - body0Origin[j]);
//printf("info->m_constraintError[%d]=%f\n",j,info->m_constraintError[j]);
}
if ((m_flags & Point2PointFlags.BT_P2P_FLAGS_CFM) != 0)
{
for (j = 0; j < 3; j++)
{
info.m_solverConstraints[j].m_cfm = m_cfm;
}
}
float impulseClamp = m_setting.m_impulseClamp;//
for (j = 0; j < 3; j++)
{
if (m_setting.m_impulseClamp > 0)
{
info.m_solverConstraints[j].m_lowerLimit = -impulseClamp;
info.m_solverConstraints[j].m_upperLimit = impulseClamp;
}
}
info.m_damping = m_setting.m_damping;
}
public void GetInfo2NonVirtual(ConstraintInfo2 info, IndexedMatrix transA, IndexedMatrix transB, IndexedBasisMatrix invInertiaWorldA, IndexedBasisMatrix invInertiaWorldB)
{
CalcAngleInfo2(ref transA, ref transB, ref invInertiaWorldA, ref invInertiaWorldB);
Debug.Assert(!m_useSolveConstraintObsolete);
// set jacobian
info.m_solverConstraints[0].m_contactNormal.X = 1f;
info.m_solverConstraints[1].m_contactNormal.Y = 1f;
info.m_solverConstraints[2].m_contactNormal.Z = 1f;
IndexedVector3 a1 = transA._basis * m_rbAFrame._origin;
{
IndexedVector3 a1neg = -a1;
MathUtil.GetSkewSymmetricMatrix(ref a1neg,
out info.m_solverConstraints[0].m_relpos1CrossNormal,
out info.m_solverConstraints[1].m_relpos1CrossNormal,
out info.m_solverConstraints[2].m_relpos1CrossNormal);
}
IndexedVector3 a2 = transB._basis * m_rbBFrame._origin;
{
MathUtil.GetSkewSymmetricMatrix(ref a2,
out info.m_solverConstraints[0].m_relpos2CrossNormal,
out info.m_solverConstraints[1].m_relpos2CrossNormal,
out info.m_solverConstraints[2].m_relpos2CrossNormal);
}
// set right hand side
float linERP = ((m_flags & (int)ConeTwistFlags.BT_CONETWIST_FLAGS_LIN_ERP) != 0) ? m_linERP : info.erp;
float k = info.fps * linERP;
for (int j = 0; j < 3; j++)
{
info.m_solverConstraints[j].m_rhs = k * (a2[j] + transB._origin[j] - a1[j] - transA._origin[j]);
info.m_solverConstraints[j].m_lowerLimit = -MathUtil.SIMD_INFINITY;
info.m_solverConstraints[j].m_upperLimit = MathUtil.SIMD_INFINITY;
if ((m_flags & (int)ConeTwistFlags.BT_CONETWIST_FLAGS_LIN_CFM) != 0)
{
info.m_solverConstraints[j].m_cfm = m_linCFM;
}
}
int row = 3;
IndexedVector3 ax1;
// angular limits
if (m_solveSwingLimit)
{
if ((m_swingSpan1 < m_fixThresh) && (m_swingSpan2 < m_fixThresh))
{
IndexedMatrix trA = transA * m_rbAFrame;
IndexedVector3 p = trA._basis.GetColumn(1);
IndexedVector3 q = trA._basis.GetColumn(2);
info.m_solverConstraints[row].m_relpos1CrossNormal = p;
info.m_solverConstraints[row + 1].m_relpos1CrossNormal = q;
info.m_solverConstraints[row].m_relpos2CrossNormal = -p;
info.m_solverConstraints[row + 1].m_relpos2CrossNormal = -q;
float fact = info.fps * m_relaxationFactor;
info.m_solverConstraints[row].m_rhs = fact * m_swingAxis.Dot(ref p);
info.m_solverConstraints[row + 1].m_rhs = fact * m_swingAxis.Dot(ref q);
info.m_solverConstraints[row].m_lowerLimit = -MathUtil.SIMD_INFINITY;
info.m_solverConstraints[row].m_upperLimit = MathUtil.SIMD_INFINITY;
info.m_solverConstraints[row + 1].m_lowerLimit = -MathUtil.SIMD_INFINITY;
info.m_solverConstraints[row + 1].m_upperLimit = MathUtil.SIMD_INFINITY;
row += 2;
}
else
{
ax1 = m_swingAxis * m_relaxationFactor * m_relaxationFactor;
info.m_solverConstraints[row].m_relpos1CrossNormal = ax1;
info.m_solverConstraints[row].m_relpos2CrossNormal = -ax1;
float k1 = info.fps * m_biasFactor;
info.m_solverConstraints[row].m_rhs = k1 * m_swingCorrection;
if ((m_flags & (int)ConeTwistFlags.BT_CONETWIST_FLAGS_ANG_CFM) != 0)
{
info.m_solverConstraints[row].m_cfm = m_angCFM;
}
// m_swingCorrection is always positive or 0
info.m_solverConstraints[row].m_lowerLimit = 0;
info.m_solverConstraints[row].m_upperLimit = MathUtil.SIMD_INFINITY;
++row;
}
}
if (m_solveTwistLimit)
{
ax1 = m_twistAxis * m_relaxationFactor * m_relaxationFactor;
info.m_solverConstraints[row].m_relpos1CrossNormal = ax1;
info.m_solverConstraints[row].m_relpos2CrossNormal = -ax1;
float k1 = info.fps * m_biasFactor;
info.m_solverConstraints[row].m_rhs = k1 * m_twistCorrection;
if ((m_flags & (int)ConeTwistFlags.BT_CONETWIST_FLAGS_ANG_CFM) != 0)
{
info.m_solverConstraints[row].m_cfm = m_angCFM;
}
if (m_twistSpan > 0.0f)
{
if (m_twistCorrection > 0.0f)
{
info.m_solverConstraints[row].m_lowerLimit = 0;
info.m_solverConstraints[row].m_upperLimit = MathUtil.SIMD_INFINITY;
}
else
{
info.m_solverConstraints[row].m_lowerLimit = -MathUtil.SIMD_INFINITY;
info.m_solverConstraints[row].m_upperLimit = 0;
}
}
else
{
info.m_solverConstraints[row].m_lowerLimit = -MathUtil.SIMD_INFINITY;
info.m_solverConstraints[row].m_upperLimit = MathUtil.SIMD_INFINITY;
}
++row;
}
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugConstraints)
{
//PrintInfo2(BulletGlobals.g_streamWriter, this, info);
}
}
public override void GetInfo2(ConstraintInfo2 info)
{
GetInfo2NonVirtual(info, m_rbA.GetCenterOfMassTransform(),
m_rbB.GetCenterOfMassTransform(),
m_rbA.GetInvInertiaTensorWorld(),
m_rbB.GetInvInertiaTensorWorld());
}
public void GetInfo2NonVirtual(ConstraintInfo2 info, IndexedMatrix transA, IndexedMatrix transB, IndexedVector3 angVelA, IndexedVector3 angVelB)
{
///the regular (virtual) implementation getInfo2 already performs 'testLimit' during getInfo1, so we need to do it now
TestLimit(ref transA, ref transB);
GetInfo2Internal(info, transA, transB, angVelA, angVelB);
}
public void GetInfo2Internal(ConstraintInfo2 info, IndexedMatrix transA, IndexedMatrix transB, IndexedVector3 angVelA, IndexedVector3 angVelB)
{
// transforms in world space
IndexedMatrix trA = transA * m_rbAFrame;
IndexedMatrix trB = transB * m_rbBFrame;
// pivot point
IndexedVector3 pivotAInW = trA._origin;
IndexedVector3 pivotBInW = trB._origin;
// linear (all fixed)
//info.m_J1linearAxis[0] = 1;
//info.m_J1linearAxis[s + 1] = 1;
//info.m_J1linearAxis[2 * s + 2] = 1;
if (!m_angularOnly)
{
info.m_solverConstraints[0].m_contactNormal.X = 1f;
info.m_solverConstraints[1].m_contactNormal.Y = 1f;
info.m_solverConstraints[2].m_contactNormal.Z = 1f;
}
IndexedVector3 a1 = pivotAInW - transA._origin;
{
IndexedVector3 a1neg = -a1;
MathUtil.GetSkewSymmetricMatrix(ref a1neg,
out info.m_solverConstraints[0].m_relpos1CrossNormal,
out info.m_solverConstraints[1].m_relpos1CrossNormal,
out info.m_solverConstraints[2].m_relpos1CrossNormal);
//if (info.m_solverConstraints[0].m_relpos1CrossNormal.X == 0.15)
//{
// int ibreak = 0;
//}
int ibreak = 0;
}
IndexedVector3 a2 = pivotBInW - transB._origin;
{
MathUtil.GetSkewSymmetricMatrix(ref a2,
out info.m_solverConstraints[0].m_relpos2CrossNormal,
out info.m_solverConstraints[1].m_relpos2CrossNormal,
out info.m_solverConstraints[2].m_relpos2CrossNormal);
}
// linear RHS
float k = info.fps * info.erp;
if (!m_angularOnly)
{
for (int i = 0; i < 3; i++)
{
float val = k * (pivotBInW[i] - pivotAInW[i]);
info.m_solverConstraints[i].m_rhs = val;
}
}
// make rotations around X and Y equal
// the hinge axis should be the only unconstrained
// rotational axis, the angular velocity of the two bodies perpendicular to
// the hinge axis should be equal. thus the constraint equations are
// p*w1 - p*w2 = 0
// q*w1 - q*w2 = 0
// where p and q are unit vectors normal to the hinge axis, and w1 and w2
// are the angular velocity vectors of the two bodies.
// get hinge axis (Z)
IndexedVector3 ax1 = trA._basis.GetColumn(2);
// get 2 orthos to hinge axis (X, Y)
IndexedVector3 p = trA._basis.GetColumn(0);
IndexedVector3 q = trA._basis.GetColumn(1);
// set the two hinge angular rows
MathUtil.SanityCheckVector(ax1);
MathUtil.SanityCheckVector(p);
MathUtil.SanityCheckVector(q);
int s3 = 3;
int s4 = 4;
info.m_solverConstraints[s3].m_relpos1CrossNormal = p;
info.m_solverConstraints[s4].m_relpos1CrossNormal = q;
info.m_solverConstraints[s3].m_relpos2CrossNormal = -p;
info.m_solverConstraints[s4].m_relpos2CrossNormal = -q;
// compute the right hand side of the constraint equation. set relative
// body velocities along p and q to bring the hinge back into alignment.
// if ax1,ax2 are the unit length hinge axes as computed from body1 and
// body2, we need to rotate both bodies along the axis u = (ax1 x ax2).
// if `theta' is the angle between ax1 and ax2, we need an angular velocity
// along u to cover angle erp*theta in one step :
// |angular_velocity| = angle/time = erp*theta / stepsize
// = (erp*fps) * theta
// angular_velocity = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2|
// = (erp*fps) * theta * (ax1 x ax2) / sin(theta)
// ...as ax1 and ax2 are unit length. if theta is smallish,
// theta ~= sin(theta), so
// angular_velocity = (erp*fps) * (ax1 x ax2)
// ax1 x ax2 is in the plane space of ax1, so we project the angular
// velocity to p and q to find the right hand side.
IndexedVector3 ax2 = trB._basis.GetColumn(2);
IndexedVector3 u = IndexedVector3.Cross(ax1, ax2);
info.m_solverConstraints[s3].m_rhs = k * IndexedVector3.Dot(u, p);
info.m_solverConstraints[s4].m_rhs = k * IndexedVector3.Dot(u, q);
// check angular limits
int nrow = 4; // last filled row
float limit_err = 0.0f;
int limit = 0;
if (GetSolveLimit())
{
#if _BT_USE_CENTER_LIMIT_
limit_err = m_limit.GetCorrection() * m_referenceSign;
#else
limit_err = m_correction * m_referenceSign;
#endif
}
// if the hinge has joint limits or motor, add in the extra row
bool powered = false;
if (GetEnableAngularMotor())
{
powered = true;
}
if (limit != 0 || powered)
{
nrow++;
info.m_solverConstraints[nrow].m_relpos1CrossNormal = ax1;
info.m_solverConstraints[nrow].m_relpos2CrossNormal = -ax1;
float lostop = GetLowerLimit();
float histop = GetUpperLimit();
if (limit != 0 && (MathUtil.CompareFloat(lostop, histop)))
{ // the joint motor is ineffective
powered = false;
}
info.m_solverConstraints[nrow].m_rhs = 0.0f;
float currERP = ((m_flags & (int)HingeFlags.BT_HINGE_FLAGS_ERP_STOP) != 0) ? m_stopERP : info.erp;
if (powered)
{
if ((m_flags & (int)HingeFlags.BT_HINGE_FLAGS_CFM_NORM) != 0)
{
info.m_solverConstraints[nrow].m_cfm = m_normalCFM;
}
float mot_fact = GetMotorFactor(m_hingeAngle, lostop, histop, m_motorTargetVelocity, info.fps * currERP);
info.m_solverConstraints[nrow].m_rhs += mot_fact * m_motorTargetVelocity * m_referenceSign;
info.m_solverConstraints[nrow].m_lowerLimit = -m_maxMotorImpulse;
info.m_solverConstraints[nrow].m_upperLimit = m_maxMotorImpulse;
}
if (limit != 0)
{
k = info.fps * currERP;
info.m_solverConstraints[nrow].m_rhs += k * limit_err;
if ((m_flags & (int)HingeFlags.BT_HINGE_FLAGS_CFM_STOP) != 0)
{
info.m_solverConstraints[nrow].m_cfm = m_stopCFM;
}
if (MathUtil.CompareFloat(lostop, histop))
{
// limited low and high simultaneously
info.m_solverConstraints[nrow].m_lowerLimit = -MathUtil.SIMD_INFINITY;
info.m_solverConstraints[nrow].m_upperLimit = MathUtil.SIMD_INFINITY;
}
else if (limit == 1)
{ // low limit
info.m_solverConstraints[nrow].m_lowerLimit = 0f;
info.m_solverConstraints[nrow].m_upperLimit = MathUtil.SIMD_INFINITY;
}
else
{ // high limit
info.m_solverConstraints[nrow].m_lowerLimit = -MathUtil.SIMD_INFINITY;
info.m_solverConstraints[nrow].m_upperLimit = 0f;
}
// bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that)
#if _BT_USE_CENTER_LIMIT_
float bounce = m_limit.GetRelaxationFactor();
#else
float bounce = m_relaxationFactor;
#endif
if (bounce > 0f)
{
float vel = IndexedVector3.Dot(angVelA, ax1);
vel -= IndexedVector3.Dot(angVelB, ax1);
// only apply bounce if the velocity is incoming, and if the
// resulting c[] exceeds what we already have.
if (limit == 1)
{ // low limit
if (vel < 0)
{
float newc = -bounce * vel;
if (newc > info.m_solverConstraints[nrow].m_rhs)
{
info.m_solverConstraints[nrow].m_rhs = newc;
}
}
}
else
{ // high limit - all those computations are reversed
if (vel > 0)
{
float newc = -bounce * vel;
if (newc < info.m_solverConstraints[nrow].m_rhs)
{
info.m_solverConstraints[nrow].m_rhs = newc;
}
}
}
}
#if _BT_USE_CENTER_LIMIT_
info.m_solverConstraints[nrow].m_rhs *= m_limit.GetBiasFactor();
#else
info.m_solverConstraints[nrow].m_rhs *= m_biasFactor;
#endif
} // if(limit)
} // if angular limit or powered
#if DEBUG
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugConstraints)
{
PrintInfo2(BulletGlobals.g_streamWriter, this, info);
}
#endif
}
public override void GetInfo2(ConstraintInfo2 info)
{
}
public override void GetInfo2(ConstraintInfo2 info)
{
if (m_useOffsetForConstraintFrame)
{
GetInfo2InternalUsingFrameOffset(info, m_rbA.GetCenterOfMassTransform(), m_rbB.GetCenterOfMassTransform(), m_rbA.GetAngularVelocity(), m_rbB.GetAngularVelocity());
}
else
{
GetInfo2Internal(info, m_rbA.GetCenterOfMassTransform(), m_rbB.GetCenterOfMassTransform(), m_rbA.GetAngularVelocity(), m_rbB.GetAngularVelocity());
}
}
public void GetInfo2InternalUsingFrameOffset(ConstraintInfo2 info, IndexedMatrix transA, IndexedMatrix transB, IndexedVector3 angVelA, IndexedVector3 angVelB)
{
GetInfo2InternalUsingFrameOffset(info, ref transA, ref transB, ref angVelA, ref angVelB);
}
public void GetInfo2Internal(ConstraintInfo2 info, IndexedMatrix transA, IndexedMatrix transB, IndexedVector3 angVelA, IndexedVector3 angVelB)
{
// transforms in world space
IndexedMatrix trA = transA * m_rbAFrame;
IndexedMatrix trB = transB * m_rbBFrame;
// pivot point
IndexedVector3 pivotAInW = trA._origin;
IndexedVector3 pivotBInW = trB._origin;
// linear (all fixed)
//info.m_J1linearAxis[0] = 1;
//info.m_J1linearAxis[s + 1] = 1;
//info.m_J1linearAxis[2 * s + 2] = 1;
if (!m_angularOnly)
{
info.m_solverConstraints[0].m_contactNormal.X = 1f;
info.m_solverConstraints[1].m_contactNormal.Y = 1f;
info.m_solverConstraints[2].m_contactNormal.Z = 1f;
}
IndexedVector3 a1 = pivotAInW - transA._origin;
{
IndexedVector3 a1neg = -a1;
MathUtil.GetSkewSymmetricMatrix(ref a1neg,
out info.m_solverConstraints[0].m_relpos1CrossNormal,
out info.m_solverConstraints[1].m_relpos1CrossNormal,
out info.m_solverConstraints[2].m_relpos1CrossNormal);
//if (info.m_solverConstraints[0].m_relpos1CrossNormal.X == 0.15)
//{
// int ibreak = 0;
//}
int ibreak = 0;
}
IndexedVector3 a2 = pivotBInW - transB._origin;
{
MathUtil.GetSkewSymmetricMatrix(ref a2,
out info.m_solverConstraints[0].m_relpos2CrossNormal,
out info.m_solverConstraints[1].m_relpos2CrossNormal,
out info.m_solverConstraints[2].m_relpos2CrossNormal);
}
// linear RHS
float k = info.fps * info.erp;
if (!m_angularOnly)
{
for (int i = 0; i < 3; i++)
{
float val = k * (pivotBInW[i] - pivotAInW[i]);
info.m_solverConstraints[i].m_rhs = val;
}
}
// make rotations around X and Y equal
// the hinge axis should be the only unconstrained
// rotational axis, the angular velocity of the two bodies perpendicular to
// the hinge axis should be equal. thus the constraint equations are
// p*w1 - p*w2 = 0
// q*w1 - q*w2 = 0
// where p and q are unit vectors normal to the hinge axis, and w1 and w2
// are the angular velocity vectors of the two bodies.
// get hinge axis (Z)
IndexedVector3 ax1 = trA._basis.GetColumn(2);
// get 2 orthos to hinge axis (X, Y)
IndexedVector3 p = trA._basis.GetColumn(0);
IndexedVector3 q = trA._basis.GetColumn(1);
// set the two hinge angular rows
MathUtil.SanityCheckVector(ax1);
MathUtil.SanityCheckVector(p);
MathUtil.SanityCheckVector(q);
int s3 = 3;
int s4 = 4;
info.m_solverConstraints[s3].m_relpos1CrossNormal = p;
info.m_solverConstraints[s4].m_relpos1CrossNormal = q;
info.m_solverConstraints[s3].m_relpos2CrossNormal = -p;
info.m_solverConstraints[s4].m_relpos2CrossNormal = -q;
// compute the right hand side of the constraint equation. set relative
// body velocities along p and q to bring the hinge back into alignment.
// if ax1,ax2 are the unit length hinge axes as computed from body1 and
// body2, we need to rotate both bodies along the axis u = (ax1 x ax2).
// if `theta' is the angle between ax1 and ax2, we need an angular velocity
// along u to cover angle erp*theta in one step :
// |angular_velocity| = angle/time = erp*theta / stepsize
// = (erp*fps) * theta
// angular_velocity = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2|
// = (erp*fps) * theta * (ax1 x ax2) / sin(theta)
// ...as ax1 and ax2 are unit length. if theta is smallish,
// theta ~= sin(theta), so
// angular_velocity = (erp*fps) * (ax1 x ax2)
// ax1 x ax2 is in the plane space of ax1, so we project the angular
// velocity to p and q to find the right hand side.
IndexedVector3 ax2 = trB._basis.GetColumn(2);
IndexedVector3 u = IndexedVector3.Cross(ax1, ax2);
info.m_solverConstraints[s3].m_rhs = k * IndexedVector3.Dot(u, p);
info.m_solverConstraints[s4].m_rhs = k * IndexedVector3.Dot(u, q);
// check angular limits
int nrow = 4; // last filled row
float limit_err = 0.0f;
int limit = 0;
if (GetSolveLimit())
{
#if _BT_USE_CENTER_LIMIT_
limit_err = m_limit.GetCorrection() * m_referenceSign;
#else
limit_err = m_correction * m_referenceSign;
#endif
}
// if the hinge has joint limits or motor, add in the extra row
bool powered = false;
if (GetEnableAngularMotor())
{
powered = true;
}
if (limit != 0 || powered)
{
nrow++;
info.m_solverConstraints[nrow].m_relpos1CrossNormal = ax1;
info.m_solverConstraints[nrow].m_relpos2CrossNormal = -ax1;
float lostop = GetLowerLimit();
float histop = GetUpperLimit();
if (limit != 0 && (MathUtil.CompareFloat(lostop, histop)))
{ // the joint motor is ineffective
powered = false;
}
info.m_solverConstraints[nrow].m_rhs = 0.0f;
float currERP = ((m_flags & (int)HingeFlags.BT_HINGE_FLAGS_ERP_STOP) != 0) ? m_stopERP : info.erp;
if (powered)
{
if ((m_flags & (int)HingeFlags.BT_HINGE_FLAGS_CFM_NORM) != 0)
{
info.m_solverConstraints[nrow].m_cfm = m_normalCFM;
}
float mot_fact = GetMotorFactor(m_hingeAngle, lostop, histop, m_motorTargetVelocity, info.fps * currERP);
info.m_solverConstraints[nrow].m_rhs += mot_fact * m_motorTargetVelocity * m_referenceSign;
info.m_solverConstraints[nrow].m_lowerLimit = -m_maxMotorImpulse;
info.m_solverConstraints[nrow].m_upperLimit = m_maxMotorImpulse;
}
if (limit != 0)
{
k = info.fps * currERP;
info.m_solverConstraints[nrow].m_rhs += k * limit_err;
if ((m_flags & (int)HingeFlags.BT_HINGE_FLAGS_CFM_STOP) != 0)
{
info.m_solverConstraints[nrow].m_cfm = m_stopCFM;
}
if (MathUtil.CompareFloat(lostop, histop))
{
// limited low and high simultaneously
info.m_solverConstraints[nrow].m_lowerLimit = -MathUtil.SIMD_INFINITY;
info.m_solverConstraints[nrow].m_upperLimit = MathUtil.SIMD_INFINITY;
}
else if (limit == 1)
{ // low limit
info.m_solverConstraints[nrow].m_lowerLimit = 0f;
info.m_solverConstraints[nrow].m_upperLimit = MathUtil.SIMD_INFINITY;
}
else
{ // high limit
info.m_solverConstraints[nrow].m_lowerLimit = -MathUtil.SIMD_INFINITY;
info.m_solverConstraints[nrow].m_upperLimit = 0f;
}
// bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that)
#if _BT_USE_CENTER_LIMIT_
float bounce = m_limit.GetRelaxationFactor();
#else
float bounce = m_relaxationFactor;
#endif
if (bounce > 0f)
{
float vel = IndexedVector3.Dot(angVelA, ax1);
vel -= IndexedVector3.Dot(angVelB, ax1);
// only apply bounce if the velocity is incoming, and if the
// resulting c[] exceeds what we already have.
if (limit == 1)
{ // low limit
if (vel < 0)
{
float newc = -bounce * vel;
if (newc > info.m_solverConstraints[nrow].m_rhs)
{
info.m_solverConstraints[nrow].m_rhs = newc;
}
}
}
else
{ // high limit - all those computations are reversed
if (vel > 0)
{
float newc = -bounce * vel;
if (newc < info.m_solverConstraints[nrow].m_rhs)
{
info.m_solverConstraints[nrow].m_rhs = newc;
}
}
}
}
#if _BT_USE_CENTER_LIMIT_
info.m_solverConstraints[nrow].m_rhs *= m_limit.GetBiasFactor();
#else
info.m_solverConstraints[nrow].m_rhs *= m_biasFactor;
#endif
} // if(limit)
} // if angular limit or powered
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugConstraints)
{
PrintInfo2(BulletGlobals.g_streamWriter, this, info);
}
}
public void GetInfo2InternalUsingFrameOffset(ConstraintInfo2 info, ref IndexedMatrix transA, ref IndexedMatrix transB, ref IndexedVector3 angVelA, ref IndexedVector3 angVelB)
{
// transforms in world space
#if DEBUG
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugConstraints)
{
MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "rbAFrame", m_rbAFrame);
MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "rbBFrame", m_rbBFrame);
MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "transA", transA);
MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "transB", transB);
}
#endif
IndexedMatrix trA = transA * m_rbAFrame;
IndexedMatrix trB = transB * m_rbBFrame;
#if DEBUG
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugConstraints)
{
MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "trA", trA);
MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "trB", trB);
}
#endif
// pivot point
IndexedVector3 pivotAInW = trA._origin;
IndexedVector3 pivotBInW = trB._origin;
#if true
// difference between frames in WCS
IndexedVector3 ofs = trB._origin - trA._origin;
// now get weight factors depending on masses
float miA = GetRigidBodyA().GetInvMass();
float miB = GetRigidBodyB().GetInvMass();
bool hasStaticBody = (miA < MathUtil.SIMD_EPSILON) || (miB < MathUtil.SIMD_EPSILON);
float miS = miA + miB;
float factA, factB;
if (miS > 0.0f)
{
factA = miB / miS;
}
else
{
factA = 0.5f;
}
factB = 1.0f - factA;
// get the desired direction of hinge axis
// as weighted sum of Z-orthos of frameA and frameB in WCS
IndexedVector3 ax1A = trA._basis.GetColumn(2);
IndexedVector3 ax1B = trB._basis.GetColumn(2);
IndexedVector3 ax1 = ax1A * factA + ax1B * factB;
ax1.Normalize();
// fill first 3 rows
// we want: velA + wA x relA == velB + wB x relB
IndexedMatrix bodyA_trans = transA;
IndexedMatrix bodyB_trans = transB;
int s0 = 0;
int s1 = 1;
int s2 = 2;
int nrow = 2; // last filled row
IndexedVector3 tmpA, tmpB, relA, relB, p, q;
// get vector from bodyB to frameB in WCS
relB = trB._origin - bodyB_trans._origin;
// get its projection to hinge axis
IndexedVector3 projB = ax1 * IndexedVector3.Dot(relB, ax1);
// get vector directed from bodyB to hinge axis (and orthogonal to it)
IndexedVector3 orthoB = relB - projB;
// same for bodyA
relA = trA._origin - bodyA_trans._origin;
IndexedVector3 projA = ax1 * IndexedVector3.Dot(relA, ax1);
IndexedVector3 orthoA = relA - projA;
IndexedVector3 totalDist = projA - projB;
// get offset vectors relA and relB
relA = orthoA + totalDist * factA;
relB = orthoB - totalDist * factB;
// now choose average ortho to hinge axis
p = orthoB * factA + orthoA * factB;
float len2 = p.LengthSquared();
if (len2 > MathUtil.SIMD_EPSILON)
{
p.Normalize();
}
else
{
p = trA._basis.GetColumn(1);
}
// make one more ortho
q = IndexedVector3.Cross(ax1, p);
// fill three rows
tmpA = IndexedVector3.Cross(relA, p);
tmpB = IndexedVector3.Cross(relB, p);
info.m_solverConstraints[s0].m_relpos1CrossNormal = tmpA;
info.m_solverConstraints[s0].m_relpos2CrossNormal = -tmpB;
tmpA = IndexedVector3.Cross(ref relA, ref q);
tmpB = IndexedVector3.Cross(ref relB, ref q);
if (hasStaticBody && GetSolveLimit())
{ // to make constraint between static and dynamic objects more rigid
// remove wA (or wB) from equation if angular limit is hit
tmpB *= factB;
tmpA *= factA;
}
info.m_solverConstraints[s1].m_relpos1CrossNormal = tmpA;
info.m_solverConstraints[s1].m_relpos2CrossNormal = -tmpB;
tmpA = IndexedVector3.Cross(ref relA, ref ax1);
tmpB = IndexedVector3.Cross(ref relB, ref ax1);
if (hasStaticBody)
{ // to make constraint between static and dynamic objects more rigid
// remove wA (or wB) from equation
tmpB *= factB;
tmpA *= factA;
}
info.m_solverConstraints[s2].m_relpos1CrossNormal = tmpA;
info.m_solverConstraints[s2].m_relpos2CrossNormal = -tmpB;
float k = info.fps * info.erp;
if (!m_angularOnly)
{
info.m_solverConstraints[s0].m_contactNormal = p;
info.m_solverConstraints[s1].m_contactNormal = q;
info.m_solverConstraints[s2].m_contactNormal = ax1;
// compute three elements of right hand side
float rhs = k * IndexedVector3.Dot(ref p, ref ofs);
info.m_solverConstraints[s0].m_rhs = rhs;
rhs = k * IndexedVector3.Dot(ref q, ref ofs);
info.m_solverConstraints[s1].m_rhs = rhs;
rhs = k * IndexedVector3.Dot(ref ax1, ref ofs);
info.m_solverConstraints[s2].m_rhs = rhs;
}
// the hinge axis should be the only unconstrained
// rotational axis, the angular velocity of the two bodies perpendicular to
// the hinge axis should be equal. thus the constraint equations are
// p*w1 - p*w2 = 0
// q*w1 - q*w2 = 0
// where p and q are unit vectors normal to the hinge axis, and w1 and w2
// are the angular velocity vectors of the two bodies.
int s3 = 3;
int s4 = 4;
info.m_solverConstraints[s3].m_relpos1CrossNormal = p;
info.m_solverConstraints[s4].m_relpos1CrossNormal = q;
info.m_solverConstraints[s3].m_relpos2CrossNormal = -p;
info.m_solverConstraints[s4].m_relpos2CrossNormal = -q;
// compute the right hand side of the constraint equation. set relative
// body velocities along p and q to bring the hinge back into alignment.
// if ax1A,ax1B are the unit length hinge axes as computed from bodyA and
// bodyB, we need to rotate both bodies along the axis u = (ax1 x ax2).
// if "theta" is the angle between ax1 and ax2, we need an angular velocity
// along u to cover angle erp*theta in one step :
// |angular_velocity| = angle/time = erp*theta / stepsize
// = (erp*fps) * theta
// angular_velocity = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2|
// = (erp*fps) * theta * (ax1 x ax2) / sin(theta)
// ...as ax1 and ax2 are unit length. if theta is smallish,
// theta ~= sin(theta), so
// angular_velocity = (erp*fps) * (ax1 x ax2)
// ax1 x ax2 is in the plane space of ax1, so we project the angular
// velocity to p and q to find the right hand side.
k = info.fps * info.erp;
IndexedVector3 u = IndexedVector3.Cross(ref ax1A, ref ax1B);
info.m_solverConstraints[s3].m_rhs = k * IndexedVector3.Dot(u, p);
info.m_solverConstraints[s4].m_rhs = k * IndexedVector3.Dot(u, q);
#endif
// check angular limits
nrow = 4; // last filled row
int srow;
float limit_err = 0f;
int limit = 0;
if (GetSolveLimit())
{
#if _BT_USE_CENTER_LIMIT_
limit_err = m_limit.GetCorrection() * m_referenceSign;
#else
limit_err = m_correction * m_referenceSign;
#endif
limit = (limit_err > 0f) ? 1 : 2;
}
// if the hinge has joint limits or motor, add in the extra row
bool powered = false;
if (GetEnableAngularMotor())
{
powered = true;
}
if (limit != 0 || powered)
{
nrow++;
srow = nrow;
info.m_solverConstraints[srow].m_relpos1CrossNormal = ax1;
info.m_solverConstraints[srow].m_relpos2CrossNormal = -ax1;
float lostop = GetLowerLimit();
float histop = GetUpperLimit();
if (limit != 0 && (MathUtil.CompareFloat(lostop, histop)))
{ // the joint motor is ineffective
powered = false;
}
info.m_solverConstraints[srow].m_rhs = 0f;
float currERP = ((m_flags & (int)HingeFlags.BT_HINGE_FLAGS_ERP_STOP) != 0) ? m_stopERP : info.erp;
if (powered)
{
if ((m_flags & (int)HingeFlags.BT_HINGE_FLAGS_CFM_NORM) != 0)
{
info.m_solverConstraints[srow].m_cfm = m_normalCFM;
}
float mot_fact = GetMotorFactor(m_hingeAngle, lostop, histop, m_motorTargetVelocity, info.fps * currERP);
info.m_solverConstraints[srow].m_rhs += mot_fact * m_motorTargetVelocity * m_referenceSign;
info.m_solverConstraints[srow].m_lowerLimit = -m_maxMotorImpulse;
info.m_solverConstraints[srow].m_upperLimit = m_maxMotorImpulse;
}
if (limit != 0)
{
k = info.fps * currERP;
info.m_solverConstraints[srow].m_rhs += k * limit_err;
if ((m_flags & (int)HingeFlags.BT_HINGE_FLAGS_CFM_STOP) != 0)
{
info.m_solverConstraints[srow].m_cfm = m_stopCFM;
}
if (MathUtil.CompareFloat(lostop, histop))
{
// limited low and high simultaneously
info.m_solverConstraints[srow].m_lowerLimit = -MathUtil.SIMD_INFINITY;
info.m_solverConstraints[srow].m_upperLimit = MathUtil.SIMD_INFINITY;
}
else if (limit == 1)
{ // low limit
info.m_solverConstraints[srow].m_lowerLimit = 0;
info.m_solverConstraints[srow].m_upperLimit = MathUtil.SIMD_INFINITY;
}
else
{ // high limit
info.m_solverConstraints[srow].m_lowerLimit = -MathUtil.SIMD_INFINITY;
info.m_solverConstraints[srow].m_upperLimit = 0;
}
// bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that)
#if _BT_USE_CENTER_LIMIT_
float bounce = m_limit.GetRelaxationFactor();
#else
float bounce = m_relaxationFactor;
#endif
if (bounce > 0f)
{
float vel = IndexedVector3.Dot(ref angVelA, ref ax1);
vel -= IndexedVector3.Dot(ref angVelB, ref ax1);
// only apply bounce if the velocity is incoming, and if the
// resulting c[] exceeds what we already have.
if (limit == 1)
{ // low limit
if (vel < 0)
{
float newc = -bounce * vel;
if (newc > info.m_solverConstraints[srow].m_rhs)
{
info.m_solverConstraints[srow].m_rhs = newc;
}
}
}
else
{ // high limit - all those computations are reversed
if (vel > 0)
{
float newc = -bounce * vel;
if (newc < info.m_solverConstraints[srow].m_rhs)
{
info.m_solverConstraints[srow].m_rhs = newc;
}
}
}
}
#if _BT_USE_CENTER_LIMIT_
info.m_solverConstraints[srow].m_rhs *= m_limit.GetBiasFactor();
#else
info.m_solverConstraints[srow].m_rhs *= m_biasFactor;
#endif
} // if(limit)
} // if angular limit or powered
#if DEBUG
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugConstraints)
{
PrintInfo2(BulletGlobals.g_streamWriter, this, info);
}
#endif
}
public void GetInfo2InternalUsingFrameOffset(ConstraintInfo2 info, ref IndexedMatrix transA, ref IndexedMatrix transB, ref IndexedVector3 angVelA, ref IndexedVector3 angVelB)
{
// transforms in world space
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugConstraints)
{
MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "rbAFrame", m_rbAFrame);
MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "rbBFrame", m_rbBFrame);
MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "transA", transA);
MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "transB", transB);
}
IndexedMatrix trA = transA * m_rbAFrame;
IndexedMatrix trB = transB * m_rbBFrame;
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugConstraints)
{
MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "trA", trA);
MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "trB", trB);
}
// pivot point
IndexedVector3 pivotAInW = trA._origin;
IndexedVector3 pivotBInW = trB._origin;
#if true
// difference between frames in WCS
IndexedVector3 ofs = trB._origin - trA._origin;
// now get weight factors depending on masses
float miA = GetRigidBodyA().GetInvMass();
float miB = GetRigidBodyB().GetInvMass();
bool hasStaticBody = (miA < MathUtil.SIMD_EPSILON) || (miB < MathUtil.SIMD_EPSILON);
float miS = miA + miB;
float factA, factB;
if (miS > 0.0f)
{
factA = miB / miS;
}
else
{
factA = 0.5f;
}
factB = 1.0f - factA;
// get the desired direction of hinge axis
// as weighted sum of Z-orthos of frameA and frameB in WCS
IndexedVector3 ax1A = trA._basis.GetColumn(2);
IndexedVector3 ax1B = trB._basis.GetColumn(2);
IndexedVector3 ax1 = ax1A * factA + ax1B * factB;
ax1.Normalize();
// fill first 3 rows
// we want: velA + wA x relA == velB + wB x relB
IndexedMatrix bodyA_trans = transA;
IndexedMatrix bodyB_trans = transB;
int s0 = 0;
int s1 = 1;
int s2 = 2;
int nrow = 2; // last filled row
IndexedVector3 tmpA, tmpB, relA, relB, p, q;
// get vector from bodyB to frameB in WCS
relB = trB._origin - bodyB_trans._origin;
// get its projection to hinge axis
IndexedVector3 projB = ax1 * IndexedVector3.Dot(relB, ax1);
// get vector directed from bodyB to hinge axis (and orthogonal to it)
IndexedVector3 orthoB = relB - projB;
// same for bodyA
relA = trA._origin - bodyA_trans._origin;
IndexedVector3 projA = ax1 * IndexedVector3.Dot(relA, ax1);
IndexedVector3 orthoA = relA - projA;
IndexedVector3 totalDist = projA - projB;
// get offset vectors relA and relB
relA = orthoA + totalDist * factA;
relB = orthoB - totalDist * factB;
// now choose average ortho to hinge axis
p = orthoB * factA + orthoA * factB;
float len2 = p.LengthSquared();
if (len2 > MathUtil.SIMD_EPSILON)
{
p.Normalize();
}
else
{
p = trA._basis.GetColumn(1);
}
// make one more ortho
q = IndexedVector3.Cross(ax1, p);
// fill three rows
tmpA = IndexedVector3.Cross(relA, p);
tmpB = IndexedVector3.Cross(relB, p);
info.m_solverConstraints[s0].m_relpos1CrossNormal = tmpA;
info.m_solverConstraints[s0].m_relpos2CrossNormal = -tmpB;
tmpA = IndexedVector3.Cross(ref relA, ref q);
tmpB = IndexedVector3.Cross(ref relB, ref q);
if (hasStaticBody && GetSolveLimit())
{ // to make constraint between static and dynamic objects more rigid
// remove wA (or wB) from equation if angular limit is hit
tmpB *= factB;
tmpA *= factA;
}
info.m_solverConstraints[s1].m_relpos1CrossNormal = tmpA;
info.m_solverConstraints[s1].m_relpos2CrossNormal = -tmpB;
tmpA = IndexedVector3.Cross(ref relA, ref ax1);
tmpB = IndexedVector3.Cross(ref relB, ref ax1);
if (hasStaticBody)
{ // to make constraint between static and dynamic objects more rigid
// remove wA (or wB) from equation
tmpB *= factB;
tmpA *= factA;
}
info.m_solverConstraints[s2].m_relpos1CrossNormal = tmpA;
info.m_solverConstraints[s2].m_relpos2CrossNormal = -tmpB;
float k = info.fps * info.erp;
if (!m_angularOnly)
{
info.m_solverConstraints[s0].m_contactNormal = p;
info.m_solverConstraints[s1].m_contactNormal = q;
info.m_solverConstraints[s2].m_contactNormal = ax1;
// compute three elements of right hand side
float rhs = k * IndexedVector3.Dot(ref p, ref ofs);
info.m_solverConstraints[s0].m_rhs = rhs;
rhs = k * IndexedVector3.Dot(ref q, ref ofs);
info.m_solverConstraints[s1].m_rhs = rhs;
rhs = k * IndexedVector3.Dot(ref ax1, ref ofs);
info.m_solverConstraints[s2].m_rhs = rhs;
}
// the hinge axis should be the only unconstrained
// rotational axis, the angular velocity of the two bodies perpendicular to
// the hinge axis should be equal. thus the constraint equations are
// p*w1 - p*w2 = 0
// q*w1 - q*w2 = 0
// where p and q are unit vectors normal to the hinge axis, and w1 and w2
// are the angular velocity vectors of the two bodies.
int s3 = 3;
int s4 = 4;
info.m_solverConstraints[s3].m_relpos1CrossNormal = p;
info.m_solverConstraints[s4].m_relpos1CrossNormal = q;
info.m_solverConstraints[s3].m_relpos2CrossNormal = -p;
info.m_solverConstraints[s4].m_relpos2CrossNormal = -q;
// compute the right hand side of the constraint equation. set relative
// body velocities along p and q to bring the hinge back into alignment.
// if ax1A,ax1B are the unit length hinge axes as computed from bodyA and
// bodyB, we need to rotate both bodies along the axis u = (ax1 x ax2).
// if "theta" is the angle between ax1 and ax2, we need an angular velocity
// along u to cover angle erp*theta in one step :
// |angular_velocity| = angle/time = erp*theta / stepsize
// = (erp*fps) * theta
// angular_velocity = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2|
// = (erp*fps) * theta * (ax1 x ax2) / sin(theta)
// ...as ax1 and ax2 are unit length. if theta is smallish,
// theta ~= sin(theta), so
// angular_velocity = (erp*fps) * (ax1 x ax2)
// ax1 x ax2 is in the plane space of ax1, so we project the angular
// velocity to p and q to find the right hand side.
k = info.fps * info.erp;
IndexedVector3 u = IndexedVector3.Cross(ref ax1A, ref ax1B);
info.m_solverConstraints[s3].m_rhs = k * IndexedVector3.Dot(u, p);
info.m_solverConstraints[s4].m_rhs = k * IndexedVector3.Dot(u, q);
#endif
// check angular limits
nrow = 4; // last filled row
int srow;
float limit_err = 0f;
int limit = 0;
if (GetSolveLimit())
{
#if _BT_USE_CENTER_LIMIT_
limit_err = m_limit.GetCorrection() * m_referenceSign;
#else
limit_err = m_correction * m_referenceSign;
#endif
limit = (limit_err > 0f) ? 1 : 2;
}
// if the hinge has joint limits or motor, add in the extra row
bool powered = false;
if (GetEnableAngularMotor())
{
powered = true;
}
if (limit != 0 || powered)
{
nrow++;
srow = nrow;
info.m_solverConstraints[srow].m_relpos1CrossNormal = ax1;
info.m_solverConstraints[srow].m_relpos2CrossNormal = -ax1;
float lostop = GetLowerLimit();
float histop = GetUpperLimit();
if (limit != 0 && (MathUtil.CompareFloat(lostop, histop)))
{ // the joint motor is ineffective
powered = false;
}
info.m_solverConstraints[srow].m_rhs = 0f;
float currERP = ((m_flags & (int)HingeFlags.BT_HINGE_FLAGS_ERP_STOP) != 0) ? m_stopERP : info.erp;
if (powered)
{
if ((m_flags & (int)HingeFlags.BT_HINGE_FLAGS_CFM_NORM) != 0)
{
info.m_solverConstraints[srow].m_cfm = m_normalCFM;
}
float mot_fact = GetMotorFactor(m_hingeAngle, lostop, histop, m_motorTargetVelocity, info.fps * currERP);
info.m_solverConstraints[srow].m_rhs += mot_fact * m_motorTargetVelocity * m_referenceSign;
info.m_solverConstraints[srow].m_lowerLimit = -m_maxMotorImpulse;
info.m_solverConstraints[srow].m_upperLimit = m_maxMotorImpulse;
}
if (limit != 0)
{
k = info.fps * currERP;
info.m_solverConstraints[srow].m_rhs += k * limit_err;
if ((m_flags & (int)HingeFlags.BT_HINGE_FLAGS_CFM_STOP) != 0)
{
info.m_solverConstraints[srow].m_cfm = m_stopCFM;
}
if (MathUtil.CompareFloat(lostop, histop))
{
// limited low and high simultaneously
info.m_solverConstraints[srow].m_lowerLimit = -MathUtil.SIMD_INFINITY;
info.m_solverConstraints[srow].m_upperLimit = MathUtil.SIMD_INFINITY;
}
else if (limit == 1)
{ // low limit
info.m_solverConstraints[srow].m_lowerLimit = 0;
info.m_solverConstraints[srow].m_upperLimit = MathUtil.SIMD_INFINITY;
}
else
{ // high limit
info.m_solverConstraints[srow].m_lowerLimit = -MathUtil.SIMD_INFINITY;
info.m_solverConstraints[srow].m_upperLimit = 0;
}
// bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that)
#if _BT_USE_CENTER_LIMIT_
float bounce = m_limit.GetRelaxationFactor();
#else
float bounce = m_relaxationFactor;
#endif
if (bounce > 0f)
{
float vel = IndexedVector3.Dot(ref angVelA, ref ax1);
vel -= IndexedVector3.Dot(ref angVelB, ref ax1);
// only apply bounce if the velocity is incoming, and if the
// resulting c[] exceeds what we already have.
if (limit == 1)
{ // low limit
if (vel < 0)
{
float newc = -bounce * vel;
if (newc > info.m_solverConstraints[srow].m_rhs)
{
info.m_solverConstraints[srow].m_rhs = newc;
}
}
}
else
{ // high limit - all those computations are reversed
if (vel > 0)
{
float newc = -bounce * vel;
if (newc < info.m_solverConstraints[srow].m_rhs)
{
info.m_solverConstraints[srow].m_rhs = newc;
}
}
}
}
#if _BT_USE_CENTER_LIMIT_
info.m_solverConstraints[srow].m_rhs *= m_limit.GetBiasFactor();
#else
info.m_solverConstraints[srow].m_rhs *= m_biasFactor;
#endif
} // if(limit)
} // if angular limit or powered
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugConstraints)
{
PrintInfo2(BulletGlobals.g_streamWriter, this, info);
}
}
public override void GetInfo2(ConstraintInfo2 info)
{
IndexedMatrix transA = m_rbA.GetCenterOfMassTransform();
IndexedMatrix transB = m_rbB.GetCenterOfMassTransform();
IndexedVector3 linVelA = m_rbA.GetLinearVelocity();
IndexedVector3 linVelB = m_rbB.GetLinearVelocity();
IndexedVector3 angVelA = m_rbA.GetAngularVelocity();
IndexedVector3 angVelB = m_rbB.GetAngularVelocity();
if (m_useOffsetForConstraintFrame)
{ // for stability better to solve angular limits first
int row = SetAngularLimits(info, 0, ref transA, ref transB, ref linVelA, ref linVelB, ref angVelA, ref angVelB);
SetLinearLimits(info, row, ref transA, ref transB, ref linVelA, ref linVelB, ref angVelA, ref angVelB);
}
else
{ // leave old version for compatibility
int row = SetLinearLimits(info, 0, ref transA, ref transB, ref linVelA, ref linVelB, ref angVelA, ref angVelB);
SetAngularLimits(info, row, ref transA, ref transB, ref linVelA, ref linVelB, ref angVelA, ref angVelB);
}
}
public void GetInfo2NonVirtual(ConstraintInfo2 info, IndexedMatrix transA, IndexedMatrix transB, IndexedVector3 linVelA, IndexedVector3 linVelB, float rbAinvMass, float rbBinvMass)
{
IndexedMatrix trA = GetCalculatedTransformA();
IndexedMatrix trB = GetCalculatedTransformB();
Debug.Assert(!m_useSolveConstraintObsolete);
int i, s = 1;
float signFact = m_useLinearReferenceFrameA ? 1.0f : -1.0f;
// difference between frames in WCS
IndexedVector3 ofs = trB._origin - trA._origin;
// now get weight factors depending on masses
float miA = rbAinvMass;
float miB = rbBinvMass;
bool hasStaticBody = (miA < MathUtil.SIMD_EPSILON) || (miB < MathUtil.SIMD_EPSILON);
float miS = miA + miB;
float factA, factB;
if (miS > 0.0f)
{
factA = miB / miS;
}
else
{
factA = 0.5f;
}
factB = 1.0f - factA;
IndexedVector3 ax1 = IndexedVector3.Zero, p, q;
IndexedVector3 ax1A = trA._basis.GetColumn(0);
IndexedVector3 ax1B = trB._basis.GetColumn(0);
if (m_useOffsetForConstraintFrame)
{
// get the desired direction of slider axis
// as weighted sum of X-orthos of frameA and frameB in WCS
ax1 = ax1A * factA + ax1B * factB;
ax1.Normalize();
// construct two orthos to slider axis
TransformUtil.PlaneSpace1(ref ax1, out p, out q);
}
else
{ // old way - use frameA
ax1 = trA._basis.GetColumn(0);
// get 2 orthos to slider axis (Y, Z)
p = trA._basis.GetColumn(1);
q = trA._basis.GetColumn(2);
}
// make rotations around these orthos equal
// the slider axis should be the only unconstrained
// rotational axis, the angular velocity of the two bodies perpendicular to
// the slider axis should be equal. thus the constraint equations are
// p*w1 - p*w2 = 0
// q*w1 - q*w2 = 0
// where p and q are unit vectors normal to the slider axis, and w1 and w2
// are the angular velocity vectors of the two bodies.
info.m_solverConstraints[0].m_relpos1CrossNormal = p;
info.m_solverConstraints[s].m_relpos1CrossNormal = q;
info.m_solverConstraints[0].m_relpos2CrossNormal = -p;
info.m_solverConstraints[s].m_relpos2CrossNormal = -q;
// compute the right hand side of the constraint equation. set relative
// body velocities along p and q to bring the slider back into alignment.
// if ax1A,ax1B are the unit length slider axes as computed from bodyA and
// bodyB, we need to rotate both bodies along the axis u = (ax1 x ax2).
// if "theta" is the angle between ax1 and ax2, we need an angular velocity
// along u to cover angle erp*theta in one step :
// |angular_velocity| = angle/time = erp*theta / stepsize
// = (erp*fps) * theta
// angular_velocity = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2|
// = (erp*fps) * theta * (ax1 x ax2) / sin(theta)
// ...as ax1 and ax2 are unit length. if theta is smallish,
// theta ~= sin(theta), so
// angular_velocity = (erp*fps) * (ax1 x ax2)
// ax1 x ax2 is in the plane space of ax1, so we project the angular
// velocity to p and q to find the right hand side.
// float k = info.fps * info.erp * getSoftnessOrthoAng();
float currERP = ((m_flags & (int)SliderFlags.BT_SLIDER_FLAGS_ERP_ORTANG) != 0) ? m_softnessOrthoAng : m_softnessOrthoAng * info.erp;
float k = info.fps * currERP;
IndexedVector3 u = IndexedVector3.Cross(ax1A, ax1B);
info.m_solverConstraints[0].m_rhs = k * IndexedVector3.Dot(u, p);
info.m_solverConstraints[s].m_rhs = k * IndexedVector3.Dot(u, q);
if ((m_flags & (int)SliderFlags.BT_SLIDER_FLAGS_CFM_ORTANG) != 0)
{
info.m_solverConstraints[0].m_cfm = m_cfmOrthoAng;
info.m_solverConstraints[s].m_cfm = m_cfmOrthoAng;
}
int nrow = 1; // last filled row
int srow = nrow;
float limit_err;
int limit;
bool powered;
// next two rows.
// we want: velA + wA x relA == velB + wB x relB ... but this would
// result in three equations, so we project along two orthos to the slider axis
IndexedMatrix bodyA_trans = transA;
IndexedMatrix bodyB_trans = transB;
nrow++;
int s2 = nrow * s;
nrow++;
int s3 = nrow * s;
IndexedVector3 tmpA = IndexedVector3.Zero, tmpB = IndexedVector3.Zero, relA = IndexedVector3.Zero, relB = IndexedVector3.Zero, c = IndexedVector3.Zero;
if (m_useOffsetForConstraintFrame)
{
// get vector from bodyB to frameB in WCS
relB = trB._origin - bodyB_trans._origin;
// get its projection to slider axis
IndexedVector3 projB = ax1 * IndexedVector3.Dot(relB, ax1);
// get vector directed from bodyB to slider axis (and orthogonal to it)
IndexedVector3 orthoB = relB - projB;
// same for bodyA
relA = trA._origin - bodyA_trans._origin;
IndexedVector3 projA = ax1 * IndexedVector3.Dot(relA, ax1);
IndexedVector3 orthoA = relA - projA;
// get desired offset between frames A and B along slider axis
float sliderOffs = m_linPos - m_depth.X;
// desired vector from projection of center of bodyA to projection of center of bodyB to slider axis
IndexedVector3 totalDist = projA + ax1 * sliderOffs - projB;
// get offset vectors relA and relB
relA = orthoA + totalDist * factA;
relB = orthoB - totalDist * factB;
// now choose average ortho to slider axis
p = orthoB * factA + orthoA * factB;
float len2 = p.LengthSquared();
if (len2 > MathUtil.SIMD_EPSILON)
{
p.Normalize();
}
else
{
p = trA._basis.GetColumn(1);
}
// make one more ortho
q = IndexedVector3.Cross(ax1, p);
// fill two rows
tmpA = IndexedVector3.Cross(relA, p);
tmpB = IndexedVector3.Cross(relB, p);
info.m_solverConstraints[s2].m_relpos1CrossNormal = tmpA;
info.m_solverConstraints[s2].m_relpos2CrossNormal = -tmpB;
tmpA = IndexedVector3.Cross(relA, q);
tmpB = IndexedVector3.Cross(relB, q);
if (hasStaticBody && GetSolveAngLimit())
{ // to make constraint between static and dynamic objects more rigid
// remove wA (or wB) from equation if angular limit is hit
tmpB *= factB;
tmpA *= factA;
}
info.m_solverConstraints[s3].m_relpos1CrossNormal = tmpA;
info.m_solverConstraints[s3].m_relpos2CrossNormal = -tmpB;
info.m_solverConstraints[s2].m_contactNormal = p;
info.m_solverConstraints[s3].m_contactNormal = q;
}
else
{
// old way - maybe incorrect if bodies are not on the slider axis
// see discussion "Bug in slider constraint" http://bulletphysics.org/Bullet/phpBB3/viewtopic.php?f=9&t=4024&start=0
IndexedVector3 tmp = IndexedVector3.Cross(c, p);
info.m_solverConstraints[s2].m_relpos1CrossNormal = factA * tmp;
info.m_solverConstraints[s2].m_relpos2CrossNormal = factB * tmp;
tmp = IndexedVector3.Cross(c, q);
info.m_solverConstraints[s3].m_relpos1CrossNormal = factA * tmp;
info.m_solverConstraints[s3].m_relpos2CrossNormal = factB * tmp;
info.m_solverConstraints[s2].m_contactNormal = p;
info.m_solverConstraints[s3].m_contactNormal = q;
}
// compute two elements of right hand side
// k = info.fps * info.erp * getSoftnessOrthoLin();
currERP = ((m_flags & (int)SliderFlags.BT_SLIDER_FLAGS_ERP_ORTLIN) != 0) ? m_softnessOrthoLin : m_softnessOrthoLin * info.erp;
k = info.fps * currERP;
float rhs = k * IndexedVector3.Dot(p, ofs);
info.m_solverConstraints[s2].m_rhs = rhs;
rhs = k * IndexedVector3.Dot(q, ofs);
info.m_solverConstraints[s3].m_rhs = rhs;
if ((m_flags & (int)SliderFlags.BT_SLIDER_FLAGS_CFM_ORTLIN) != 0)
{
info.m_solverConstraints[s2].m_cfm = m_cfmOrthoLin;
info.m_solverConstraints[s3].m_cfm = m_cfmOrthoLin;
}
// check linear limits
limit_err = 0.0f;
limit = 0;
if (GetSolveLinLimit())
{
limit_err = GetLinDepth() * signFact;
limit = (limit_err > 0f) ? 2 : 1;
}
powered = false;
if (GetPoweredLinMotor())
{
powered = true;
}
// if the slider has joint limits or motor, add in the extra row
if (limit != 0 || powered)
{
nrow++;
srow = nrow;
info.m_solverConstraints[srow].m_contactNormal = ax1;
// linear torque decoupling step:
//
// we have to be careful that the linear constraint forces (+/- ax1) applied to the two bodies
// do not create a torque couple. in other words, the points that the
// constraint force is applied at must lie along the same ax1 axis.
// a torque couple will result in limited slider-jointed free
// bodies from gaining angular momentum.
if (m_useOffsetForConstraintFrame)
{
// this is needed only when bodyA and bodyB are both dynamic.
if (!hasStaticBody)
{
tmpA = IndexedVector3.Cross(relA, ax1);
tmpB = IndexedVector3.Cross(relB, ax1);
info.m_solverConstraints[srow].m_relpos1CrossNormal = tmpA;
info.m_solverConstraints[srow].m_relpos2CrossNormal = -tmpB;
}
}
else
{
// The old way. May be incorrect if bodies are not on the slider axis
IndexedVector3 ltd = IndexedVector3.Cross(c, ax1); // Linear Torque Decoupling vector (a torque)
info.m_solverConstraints[nrow].m_relpos1CrossNormal = factA * ltd;
info.m_solverConstraints[nrow].m_relpos2CrossNormal = factB * ltd;
}
// right-hand part
float lostop = GetLowerLinLimit();
float histop = GetUpperLinLimit();
if (limit != 0 && (MathUtil.CompareFloat(lostop, histop)))
{ // the joint motor is ineffective
powered = false;
}
info.m_solverConstraints[nrow].m_rhs = 0f;
info.m_solverConstraints[nrow].m_lowerLimit = 0f;
info.m_solverConstraints[nrow].m_upperLimit = 0f;
currERP = ((m_flags & (int)SliderFlags.BT_SLIDER_FLAGS_ERP_LIMLIN) != 0) ? m_softnessLimLin : info.erp;
if (powered)
{
if ((m_flags & (int)SliderFlags.BT_SLIDER_FLAGS_CFM_DIRLIN) != 0)
{
info.m_solverConstraints[nrow].m_cfm = m_cfmDirLin;
}
float tag_vel = GetTargetLinMotorVelocity();
float mot_fact = GetMotorFactor(m_linPos, m_lowerLinLimit, m_upperLinLimit, tag_vel, info.fps * currERP);
info.m_solverConstraints[nrow].m_rhs -= signFact * mot_fact * GetTargetLinMotorVelocity();
info.m_solverConstraints[nrow].m_lowerLimit += -GetMaxLinMotorForce() * info.fps;
info.m_solverConstraints[nrow].m_upperLimit += GetMaxLinMotorForce() * info.fps;
}
if (limit != 0)
{
k = info.fps * currERP;
info.m_solverConstraints[nrow].m_rhs += k * limit_err;
if ((m_flags & (int)SliderFlags.BT_SLIDER_FLAGS_CFM_LIMLIN) != 0)
{
info.m_solverConstraints[nrow].m_cfm = m_cfmLimLin;
}
if (MathUtil.CompareFloat(lostop, histop))
{ // limited low and high simultaneously
info.m_solverConstraints[nrow].m_lowerLimit = -MathUtil.SIMD_INFINITY;
info.m_solverConstraints[nrow].m_upperLimit = MathUtil.SIMD_INFINITY;
}
else if (limit == 1)
{ // low limit
info.m_solverConstraints[nrow].m_lowerLimit = -MathUtil.SIMD_INFINITY;
info.m_solverConstraints[nrow].m_upperLimit = 0f;
}
else
{ // high limit
info.m_solverConstraints[nrow].m_lowerLimit = 0f;
info.m_solverConstraints[nrow].m_upperLimit = MathUtil.SIMD_INFINITY;
}
// bounce (we'll use slider parameter abs(1.0 - m_dampingLimLin) for that)
float bounce = Math.Abs(1.0f - GetDampingLimLin());
if (bounce > 0.0f)
{
float vel = IndexedVector3.Dot(linVelA, ax1);
vel -= IndexedVector3.Dot(linVelB, ax1);
vel *= signFact;
// only apply bounce if the velocity is incoming, and if the
// resulting c[] exceeds what we already have.
if (limit == 1)
{ // low limit
if (vel < 0)
{
float newc = -bounce * vel;
if (newc > info.m_solverConstraints[nrow].m_rhs)
{
info.m_solverConstraints[nrow].m_rhs = newc;
}
}
}
else
{ // high limit - all those computations are reversed
if (vel > 0)
{
float newc = -bounce * vel;
if (newc < info.m_solverConstraints[nrow].m_rhs)
{
info.m_solverConstraints[nrow].m_rhs = newc;
}
}
}
}
info.m_solverConstraints[nrow].m_rhs *= GetSoftnessLimLin();
} // if(limit)
} // if linear limit
// check angular limits
limit_err = 0.0f;
limit = 0;
if (GetSolveAngLimit())
{
limit_err = GetAngDepth();
limit = (limit_err > 0.0f) ? 1 : 2;
}
// if the slider has joint limits, add in the extra row
powered = false;
if (GetPoweredAngMotor())
{
powered = true;
}
if (limit != 0 || powered)
{
nrow++;
srow = nrow;
info.m_solverConstraints[srow].m_relpos1CrossNormal = ax1;
info.m_solverConstraints[srow].m_relpos2CrossNormal = -ax1;
float lostop = GetLowerAngLimit();
float histop = GetUpperAngLimit();
if (limit != 0 && (MathUtil.CompareFloat(lostop, histop)))
{ // the joint motor is ineffective
powered = false;
}
currERP = ((m_flags & (int)SliderFlags.BT_SLIDER_FLAGS_ERP_LIMANG) != 0) ? m_softnessLimAng : info.erp;
if (powered)
{
if ((m_flags & (int)SliderFlags.BT_SLIDER_FLAGS_CFM_DIRANG) != 0)
{
info.m_solverConstraints[nrow].m_cfm = m_cfmDirAng;
}
float mot_fact = GetMotorFactor(m_angPos, m_lowerAngLimit, m_upperAngLimit, GetTargetAngMotorVelocity(), info.fps * currERP);
info.m_solverConstraints[nrow].m_rhs = mot_fact * GetTargetAngMotorVelocity();
info.m_solverConstraints[nrow].m_lowerLimit = -GetMaxAngMotorForce() * info.fps;
info.m_solverConstraints[nrow].m_upperLimit = GetMaxAngMotorForce() * info.fps;
}
if (limit != 0)
{
k = info.fps * currERP;
info.m_solverConstraints[nrow].m_rhs += k * limit_err;
if ((m_flags & (int)SliderFlags.BT_SLIDER_FLAGS_CFM_LIMANG) != 0)
{
info.m_solverConstraints[nrow].m_cfm = m_cfmLimAng;
}
if (MathUtil.CompareFloat(lostop, histop))
{
// limited low and high simultaneously
info.m_solverConstraints[nrow].m_lowerLimit = -MathUtil.SIMD_INFINITY;
info.m_solverConstraints[nrow].m_upperLimit = MathUtil.SIMD_INFINITY;
}
else if (limit == 1)
{ // low limit
info.m_solverConstraints[nrow].m_lowerLimit = 0;
info.m_solverConstraints[nrow].m_upperLimit = MathUtil.SIMD_INFINITY;
}
else
{ // high limit
info.m_solverConstraints[nrow].m_lowerLimit = -MathUtil.SIMD_INFINITY;
info.m_solverConstraints[nrow].m_upperLimit = 0;
}
// bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that)
float bounce = Math.Abs(1.0f - GetDampingLimAng());
if (bounce > 0.0f)
{
float vel = IndexedVector3.Dot(m_rbA.GetAngularVelocity(), ax1);
vel -= IndexedVector3.Dot(m_rbB.GetAngularVelocity(), ax1);
// only apply bounce if the velocity is incoming, and if the
// resulting c[] exceeds what we already have.
if (limit == 1)
{ // low limit
if (vel < 0)
{
float newc = -bounce * vel;
if (newc > info.m_solverConstraints[nrow].m_rhs)
{
info.m_solverConstraints[nrow].m_rhs = newc;
}
}
}
else
{ // high limit - all those computations are reversed
if (vel > 0)
{
float newc = -bounce * vel;
if (newc < info.m_solverConstraints[nrow].m_rhs)
{
info.m_solverConstraints[nrow].m_rhs = newc;
}
}
}
}
info.m_solverConstraints[nrow].m_rhs *= GetSoftnessLimAng();
} // if(limit)
} // if angular limit or powered
#if DEBUG
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugConstraints)
{
PrintInfo2(BulletGlobals.g_streamWriter, this, info);
}
#endif
}
public void GetInfo2NonVirtual(ConstraintInfo2 info, IndexedMatrix transA, IndexedMatrix transB, IndexedVector3 linVelA, IndexedVector3 linVelB, IndexedVector3 angVelA, IndexedVector3 angVelB)
{
//prepare constraint
CalculateTransforms(ref transA, ref transB);
for (int i = 0; i < 3; i++)
{
TestAngularLimitMotor(i);
}
if (m_useOffsetForConstraintFrame)
{ // for stability better to solve angular limits first
int row = SetAngularLimits(info, 0, ref transA, ref transB, ref linVelA, ref linVelB, ref angVelA, ref angVelB);
SetLinearLimits(info, row, ref transA, ref transB, ref linVelA, ref linVelB, ref angVelA, ref angVelB);
}
else
{ // leave old version for compatibility
int row = SetLinearLimits(info, 0, ref transA, ref transB, ref linVelA, ref linVelB, ref angVelA, ref angVelB);
SetAngularLimits(info, row, ref transA, ref transB, ref linVelA, ref linVelB, ref angVelA, ref angVelB);
}
}
public void TestLinLimits2(ConstraintInfo2 info)
{
}
public virtual int GetLimitMotorInfo2(RotationalLimitMotor limot,
ref IndexedMatrix transA, ref IndexedMatrix transB, ref IndexedVector3 linVelA,
ref IndexedVector3 linVelB, ref IndexedVector3 angVelA, ref IndexedVector3 angVelB,
ConstraintInfo2 info, int row, ref IndexedVector3 ax1, int rotational, bool rotAllowed)
{
bool powered = limot.m_enableMotor;
int limit = limot.m_currentLimit;
if (powered || limit != 0)
{
// if the joint is powered, or has joint limits, add in the extra row
//float* J1 = rotational ? info->m_J1angularAxis : info->m_J1linearAxis;
//float* J2 = rotational ? info->m_J2angularAxis : 0;
//info2.m_J1linearAxis = currentConstraintRow->m_contactNormal;
//info2.m_J1angularAxis = currentConstraintRow->m_relpos1CrossNormal;
//info2.m_J2linearAxis = 0;
//info2.m_J2angularAxis = currentConstraintRow->m_relpos2CrossNormal;
if (rotational != 0)
{
info.m_solverConstraints[row].m_relpos1CrossNormal = ax1;
MathUtil.ZeroCheckVector(info.m_solverConstraints[row].m_relpos1CrossNormal);
}
else
{
info.m_solverConstraints[row].m_contactNormal = ax1;
MathUtil.ZeroCheckVector(info.m_solverConstraints[row].m_contactNormal);
}
if (rotational != 0)
{
info.m_solverConstraints[row].m_relpos2CrossNormal = -ax1;
}
//MathUtil.zeroCheckVector(info.m_solverConstraints[row].m_relpos2CrossNormal);
if (rotational == 0)
{
if (m_useOffsetForConstraintFrame)
{
IndexedVector3 tmpA = IndexedVector3.Zero, tmpB = IndexedVector3.Zero, relA = IndexedVector3.Zero, relB = IndexedVector3.Zero;
// get vector from bodyB to frameB in WCS
relB = m_calculatedTransformB._origin - transB._origin;
// get its projection to constraint axis
IndexedVector3 projB = ax1 * IndexedVector3.Dot(relB, ax1);
// get vector directed from bodyB to constraint axis (and orthogonal to it)
IndexedVector3 orthoB = relB - projB;
// same for bodyA
relA = m_calculatedTransformA._origin - transA._origin;
IndexedVector3 projA = ax1 * IndexedVector3.Dot(relA, ax1);
IndexedVector3 orthoA = relA - projA;
// get desired offset between frames A and B along constraint axis
float desiredOffs = limot.m_currentPosition - limot.m_currentLimitError;
// desired vector from projection of center of bodyA to projection of center of bodyB to constraint axis
IndexedVector3 totalDist = projA + ax1 * desiredOffs - projB;
// get offset vectors relA and relB
relA = orthoA + totalDist * m_factA;
relB = orthoB - totalDist * m_factB;
tmpA = IndexedVector3.Cross(relA, ax1);
tmpB = IndexedVector3.Cross(relB, ax1);
if (m_hasStaticBody && (!rotAllowed))
{
tmpA *= m_factA;
tmpB *= m_factB;
}
info.m_solverConstraints[row].m_relpos1CrossNormal = tmpA;
MathUtil.ZeroCheckVector(ref tmpA);
info.m_solverConstraints[row].m_relpos2CrossNormal = -tmpB;
MathUtil.ZeroCheckVector(ref tmpB);
}
else
{
IndexedVector3 ltd; // Linear Torque Decoupling vector
IndexedVector3 c = m_calculatedTransformB._origin - transA._origin;
ltd = IndexedVector3.Cross(c, ax1);
info.m_solverConstraints[row].m_relpos1CrossNormal = ltd;
MathUtil.ZeroCheckVector(info.m_solverConstraints[row].m_relpos1CrossNormal);
c = m_calculatedTransformB._origin - transB._origin;
ltd = -IndexedVector3.Cross(c, ax1);
info.m_solverConstraints[row].m_relpos2CrossNormal = ltd;
MathUtil.ZeroCheckVector(info.m_solverConstraints[row].m_relpos2CrossNormal);
}
}
// if we're limited low and high simultaneously, the joint motor is
// ineffective
if (limit != 0 && (MathUtil.CompareFloat(limot.m_loLimit, limot.m_hiLimit)))
{
powered = false;
}
info.m_solverConstraints[row].m_rhs = 0f;
if (powered)
{
info.m_solverConstraints[row].m_cfm = limot.m_normalCFM;
if (limit == 0)
{
float tag_vel = (rotational != 0) ? limot.m_targetVelocity : -limot.m_targetVelocity;
float mot_fact = GetMotorFactor(limot.m_currentPosition,
limot.m_loLimit,
limot.m_hiLimit,
tag_vel,
info.fps * limot.m_stopERP);
info.m_solverConstraints[row].m_rhs += mot_fact * limot.m_targetVelocity;
info.m_solverConstraints[row].m_lowerLimit = -limot.m_maxMotorForce;
info.m_solverConstraints[row].m_upperLimit = limot.m_maxMotorForce;
}
}
if (limit != 0)
{
float k = info.fps * limot.m_stopERP;
if (rotational == 0)
{
info.m_solverConstraints[row].m_rhs += k * limot.m_currentLimitError;
}
else
{
info.m_solverConstraints[row].m_rhs += -k * limot.m_currentLimitError;
}
info.m_solverConstraints[row].m_cfm = limot.m_stopCFM;
if (MathUtil.CompareFloat(limot.m_loLimit, limot.m_hiLimit))
{ // limited low and high simultaneously
info.m_solverConstraints[row].m_lowerLimit = -MathUtil.SIMD_INFINITY;
info.m_solverConstraints[row].m_upperLimit = MathUtil.SIMD_INFINITY;
}
else
{
if (limit == 1)
{
info.m_solverConstraints[row].m_lowerLimit = 0;
info.m_solverConstraints[row].m_upperLimit = MathUtil.SIMD_INFINITY;
}
else
{
info.m_solverConstraints[row].m_lowerLimit = -MathUtil.SIMD_INFINITY;
info.m_solverConstraints[row].m_upperLimit = 0;
}
// deal with bounce
if (limot.m_bounce > 0)
{
// calculate joint velocity
float vel;
if (rotational != 0)
{
vel = IndexedVector3.Dot(angVelA, ax1);
vel -= IndexedVector3.Dot(angVelB, ax1);
}
else
{
vel = IndexedVector3.Dot(linVelA, ax1);
vel -= IndexedVector3.Dot(linVelB, ax1);
}
// only apply bounce if the velocity is incoming, and if the
// resulting c[] exceeds what we already have.
if (limit == 1)
{
if (vel < 0)
{
float newc = -limot.m_bounce * vel;
if (newc > info.m_solverConstraints[row].m_rhs)
{
info.m_solverConstraints[row].m_rhs = newc;
}
}
}
else
{
if (vel > 0)
{
float newc = -limot.m_bounce * vel;
if (newc < info.m_solverConstraints[row].m_rhs)
{
info.m_solverConstraints[row].m_rhs = newc;
}
}
}
}
}
}
return 1;
}
else return 0;
}
public void GetInfo2NonVirtual(ConstraintInfo2 info, IndexedMatrix transA, IndexedMatrix transB, IndexedVector3 linVelA, IndexedVector3 linVelB, float rbAinvMass, float rbBinvMass)
{
IndexedMatrix trA = GetCalculatedTransformA();
IndexedMatrix trB = GetCalculatedTransformB();
Debug.Assert(!m_useSolveConstraintObsolete);
int i, s = 1;
float signFact = m_useLinearReferenceFrameA ? 1.0f : -1.0f;
// difference between frames in WCS
IndexedVector3 ofs = trB._origin - trA._origin;
// now get weight factors depending on masses
float miA = rbAinvMass;
float miB = rbBinvMass;
bool hasStaticBody = (miA < MathUtil.SIMD_EPSILON) || (miB < MathUtil.SIMD_EPSILON);
float miS = miA + miB;
float factA, factB;
if (miS > 0.0f)
{
factA = miB / miS;
}
else
{
factA = 0.5f;
}
factB = 1.0f - factA;
IndexedVector3 ax1 = IndexedVector3.Zero, p, q;
IndexedVector3 ax1A = trA._basis.GetColumn(0);
IndexedVector3 ax1B = trB._basis.GetColumn(0);
if (m_useOffsetForConstraintFrame)
{
// get the desired direction of slider axis
// as weighted sum of X-orthos of frameA and frameB in WCS
ax1 = ax1A * factA + ax1B * factB;
ax1.Normalize();
// construct two orthos to slider axis
TransformUtil.PlaneSpace1(ref ax1, out p, out q);
}
else
{ // old way - use frameA
ax1 = trA._basis.GetColumn(0);
// get 2 orthos to slider axis (Y, Z)
p = trA._basis.GetColumn(1);
q = trA._basis.GetColumn(2);
}
// make rotations around these orthos equal
// the slider axis should be the only unconstrained
// rotational axis, the angular velocity of the two bodies perpendicular to
// the slider axis should be equal. thus the constraint equations are
// p*w1 - p*w2 = 0
// q*w1 - q*w2 = 0
// where p and q are unit vectors normal to the slider axis, and w1 and w2
// are the angular velocity vectors of the two bodies.
info.m_solverConstraints[0].m_relpos1CrossNormal = p;
info.m_solverConstraints[s].m_relpos1CrossNormal = q;
info.m_solverConstraints[0].m_relpos2CrossNormal = -p;
info.m_solverConstraints[s].m_relpos2CrossNormal = -q;
// compute the right hand side of the constraint equation. set relative
// body velocities along p and q to bring the slider back into alignment.
// if ax1A,ax1B are the unit length slider axes as computed from bodyA and
// bodyB, we need to rotate both bodies along the axis u = (ax1 x ax2).
// if "theta" is the angle between ax1 and ax2, we need an angular velocity
// along u to cover angle erp*theta in one step :
// |angular_velocity| = angle/time = erp*theta / stepsize
// = (erp*fps) * theta
// angular_velocity = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2|
// = (erp*fps) * theta * (ax1 x ax2) / sin(theta)
// ...as ax1 and ax2 are unit length. if theta is smallish,
// theta ~= sin(theta), so
// angular_velocity = (erp*fps) * (ax1 x ax2)
// ax1 x ax2 is in the plane space of ax1, so we project the angular
// velocity to p and q to find the right hand side.
// float k = info.fps * info.erp * getSoftnessOrthoAng();
float currERP = ((m_flags & (int)SliderFlags.BT_SLIDER_FLAGS_ERP_ORTANG) != 0) ? m_softnessOrthoAng : m_softnessOrthoAng * info.erp;
float k = info.fps * currERP;
IndexedVector3 u = IndexedVector3.Cross(ax1A, ax1B);
info.m_solverConstraints[0].m_rhs = k * IndexedVector3.Dot(u, p);
info.m_solverConstraints[s].m_rhs = k * IndexedVector3.Dot(u, q);
if ((m_flags & (int)SliderFlags.BT_SLIDER_FLAGS_CFM_ORTANG) != 0)
{
info.m_solverConstraints[0].m_cfm = m_cfmOrthoAng;
info.m_solverConstraints[s].m_cfm = m_cfmOrthoAng;
}
int nrow = 1; // last filled row
int srow = nrow;
float limit_err;
int limit;
bool powered;
// next two rows.
// we want: velA + wA x relA == velB + wB x relB ... but this would
// result in three equations, so we project along two orthos to the slider axis
IndexedMatrix bodyA_trans = transA;
IndexedMatrix bodyB_trans = transB;
nrow++;
int s2 = nrow * s;
nrow++;
int s3 = nrow * s;
IndexedVector3 tmpA = IndexedVector3.Zero, tmpB = IndexedVector3.Zero, relA = IndexedVector3.Zero, relB = IndexedVector3.Zero, c = IndexedVector3.Zero;
if (m_useOffsetForConstraintFrame)
{
// get vector from bodyB to frameB in WCS
relB = trB._origin - bodyB_trans._origin;
// get its projection to slider axis
IndexedVector3 projB = ax1 * IndexedVector3.Dot(relB, ax1);
// get vector directed from bodyB to slider axis (and orthogonal to it)
IndexedVector3 orthoB = relB - projB;
// same for bodyA
relA = trA._origin - bodyA_trans._origin;
IndexedVector3 projA = ax1 * IndexedVector3.Dot(relA, ax1);
IndexedVector3 orthoA = relA - projA;
// get desired offset between frames A and B along slider axis
float sliderOffs = m_linPos - m_depth.X;
// desired vector from projection of center of bodyA to projection of center of bodyB to slider axis
IndexedVector3 totalDist = projA + ax1 * sliderOffs - projB;
// get offset vectors relA and relB
relA = orthoA + totalDist * factA;
relB = orthoB - totalDist * factB;
// now choose average ortho to slider axis
p = orthoB * factA + orthoA * factB;
float len2 = p.LengthSquared();
if (len2 > MathUtil.SIMD_EPSILON)
{
p.Normalize();
}
else
{
p = trA._basis.GetColumn(1);
}
// make one more ortho
q = IndexedVector3.Cross(ax1, p);
// fill two rows
tmpA = IndexedVector3.Cross(relA, p);
tmpB = IndexedVector3.Cross(relB, p);
info.m_solverConstraints[s2].m_relpos1CrossNormal = tmpA;
info.m_solverConstraints[s2].m_relpos2CrossNormal = -tmpB;
tmpA = IndexedVector3.Cross(relA, q);
tmpB = IndexedVector3.Cross(relB, q);
if (hasStaticBody && GetSolveAngLimit())
{ // to make constraint between static and dynamic objects more rigid
// remove wA (or wB) from equation if angular limit is hit
tmpB *= factB;
tmpA *= factA;
}
info.m_solverConstraints[s3].m_relpos1CrossNormal = tmpA;
info.m_solverConstraints[s3].m_relpos2CrossNormal = -tmpB;
info.m_solverConstraints[s2].m_contactNormal = p;
info.m_solverConstraints[s3].m_contactNormal = q;
}
else
{
// old way - maybe incorrect if bodies are not on the slider axis
// see discussion "Bug in slider constraint" http://bulletphysics.org/Bullet/phpBB3/viewtopic.php?f=9&t=4024&start=0
IndexedVector3 tmp = IndexedVector3.Cross(c, p);
info.m_solverConstraints[s2].m_relpos1CrossNormal = factA * tmp;
info.m_solverConstraints[s2].m_relpos2CrossNormal = factB * tmp;
tmp = IndexedVector3.Cross(c, q);
info.m_solverConstraints[s3].m_relpos1CrossNormal = factA * tmp;
info.m_solverConstraints[s3].m_relpos2CrossNormal = factB * tmp;
info.m_solverConstraints[s2].m_contactNormal = p;
info.m_solverConstraints[s3].m_contactNormal = q;
}
// compute two elements of right hand side
// k = info.fps * info.erp * getSoftnessOrthoLin();
currERP = ((m_flags & (int)SliderFlags.BT_SLIDER_FLAGS_ERP_ORTLIN) != 0) ? m_softnessOrthoLin : m_softnessOrthoLin * info.erp;
k = info.fps * currERP;
float rhs = k * IndexedVector3.Dot(p, ofs);
info.m_solverConstraints[s2].m_rhs = rhs;
rhs = k * IndexedVector3.Dot(q, ofs);
info.m_solverConstraints[s3].m_rhs = rhs;
if ((m_flags & (int)SliderFlags.BT_SLIDER_FLAGS_CFM_ORTLIN) != 0)
{
info.m_solverConstraints[s2].m_cfm = m_cfmOrthoLin;
info.m_solverConstraints[s3].m_cfm = m_cfmOrthoLin;
}
// check linear limits
limit_err = 0.0f;
limit = 0;
if (GetSolveLinLimit())
{
limit_err = GetLinDepth() * signFact;
limit = (limit_err > 0f) ? 2 : 1;
}
powered = false;
if (GetPoweredLinMotor())
{
powered = true;
}
// if the slider has joint limits or motor, add in the extra row
if (limit != 0 || powered)
{
nrow++;
srow = nrow;
info.m_solverConstraints[srow].m_contactNormal = ax1;
// linear torque decoupling step:
//
// we have to be careful that the linear constraint forces (+/- ax1) applied to the two bodies
// do not create a torque couple. in other words, the points that the
// constraint force is applied at must lie along the same ax1 axis.
// a torque couple will result in limited slider-jointed free
// bodies from gaining angular momentum.
if (m_useOffsetForConstraintFrame)
{
// this is needed only when bodyA and bodyB are both dynamic.
if (!hasStaticBody)
{
tmpA = IndexedVector3.Cross(relA, ax1);
tmpB = IndexedVector3.Cross(relB, ax1);
info.m_solverConstraints[srow].m_relpos1CrossNormal = tmpA;
info.m_solverConstraints[srow].m_relpos2CrossNormal = -tmpB;
}
}
else
{
// The old way. May be incorrect if bodies are not on the slider axis
IndexedVector3 ltd = IndexedVector3.Cross(c, ax1); // Linear Torque Decoupling vector (a torque)
info.m_solverConstraints[nrow].m_relpos1CrossNormal = factA * ltd;
info.m_solverConstraints[nrow].m_relpos2CrossNormal = factB * ltd;
}
// right-hand part
float lostop = GetLowerLinLimit();
float histop = GetUpperLinLimit();
if (limit != 0 && (MathUtil.CompareFloat(lostop, histop)))
{ // the joint motor is ineffective
powered = false;
}
info.m_solverConstraints[nrow].m_rhs = 0f;
info.m_solverConstraints[nrow].m_lowerLimit = 0f;
info.m_solverConstraints[nrow].m_upperLimit = 0f;
currERP = ((m_flags & (int)SliderFlags.BT_SLIDER_FLAGS_ERP_LIMLIN) != 0) ? m_softnessLimLin : info.erp;
if (powered)
{
if ((m_flags & (int)SliderFlags.BT_SLIDER_FLAGS_CFM_DIRLIN) != 0)
{
info.m_solverConstraints[nrow].m_cfm = m_cfmDirLin;
}
float tag_vel = GetTargetLinMotorVelocity();
float mot_fact = GetMotorFactor(m_linPos, m_lowerLinLimit, m_upperLinLimit, tag_vel, info.fps * currERP);
info.m_solverConstraints[nrow].m_rhs -= signFact * mot_fact * GetTargetLinMotorVelocity();
info.m_solverConstraints[nrow].m_lowerLimit += -GetMaxLinMotorForce() * info.fps;
info.m_solverConstraints[nrow].m_upperLimit += GetMaxLinMotorForce() * info.fps;
}
if (limit != 0)
{
k = info.fps * currERP;
info.m_solverConstraints[nrow].m_rhs += k * limit_err;
if ((m_flags & (int)SliderFlags.BT_SLIDER_FLAGS_CFM_LIMLIN) != 0)
{
info.m_solverConstraints[nrow].m_cfm = m_cfmLimLin;
}
if (MathUtil.CompareFloat(lostop, histop))
{ // limited low and high simultaneously
info.m_solverConstraints[nrow].m_lowerLimit = -MathUtil.SIMD_INFINITY;
info.m_solverConstraints[nrow].m_upperLimit = MathUtil.SIMD_INFINITY;
}
else if (limit == 1)
{ // low limit
info.m_solverConstraints[nrow].m_lowerLimit = -MathUtil.SIMD_INFINITY;
info.m_solverConstraints[nrow].m_upperLimit = 0f;
}
else
{ // high limit
info.m_solverConstraints[nrow].m_lowerLimit = 0f;
info.m_solverConstraints[nrow].m_upperLimit = MathUtil.SIMD_INFINITY;
}
// bounce (we'll use slider parameter abs(1.0 - m_dampingLimLin) for that)
float bounce = Math.Abs(1.0f - GetDampingLimLin());
if (bounce > 0.0f)
{
float vel = IndexedVector3.Dot(linVelA, ax1);
vel -= IndexedVector3.Dot(linVelB, ax1);
vel *= signFact;
// only apply bounce if the velocity is incoming, and if the
// resulting c[] exceeds what we already have.
if (limit == 1)
{ // low limit
if (vel < 0)
{
float newc = -bounce * vel;
if (newc > info.m_solverConstraints[nrow].m_rhs)
{
info.m_solverConstraints[nrow].m_rhs = newc;
}
}
}
else
{ // high limit - all those computations are reversed
if (vel > 0)
{
float newc = -bounce * vel;
if (newc < info.m_solverConstraints[nrow].m_rhs)
{
info.m_solverConstraints[nrow].m_rhs = newc;
}
}
}
}
info.m_solverConstraints[nrow].m_rhs *= GetSoftnessLimLin();
} // if(limit)
} // if linear limit
// check angular limits
limit_err = 0.0f;
limit = 0;
if (GetSolveAngLimit())
{
limit_err = GetAngDepth();
limit = (limit_err > 0.0f) ? 1 : 2;
}
// if the slider has joint limits, add in the extra row
powered = false;
if (GetPoweredAngMotor())
{
powered = true;
}
if (limit != 0 || powered)
{
nrow++;
srow = nrow;
info.m_solverConstraints[srow].m_relpos1CrossNormal = ax1;
info.m_solverConstraints[srow].m_relpos2CrossNormal = -ax1;
float lostop = GetLowerAngLimit();
float histop = GetUpperAngLimit();
if (limit != 0 && (MathUtil.CompareFloat(lostop, histop)))
{ // the joint motor is ineffective
powered = false;
}
currERP = ((m_flags & (int)SliderFlags.BT_SLIDER_FLAGS_ERP_LIMANG) != 0) ? m_softnessLimAng : info.erp;
if (powered)
{
if ((m_flags & (int)SliderFlags.BT_SLIDER_FLAGS_CFM_DIRANG) != 0)
{
info.m_solverConstraints[nrow].m_cfm = m_cfmDirAng;
}
float mot_fact = GetMotorFactor(m_angPos, m_lowerAngLimit, m_upperAngLimit, GetTargetAngMotorVelocity(), info.fps * currERP);
info.m_solverConstraints[nrow].m_rhs = mot_fact * GetTargetAngMotorVelocity();
info.m_solverConstraints[nrow].m_lowerLimit = -GetMaxAngMotorForce() * info.fps;
info.m_solverConstraints[nrow].m_upperLimit = GetMaxAngMotorForce() * info.fps;
}
if (limit != 0)
{
k = info.fps * currERP;
info.m_solverConstraints[nrow].m_rhs += k * limit_err;
if ((m_flags & (int)SliderFlags.BT_SLIDER_FLAGS_CFM_LIMANG) != 0)
{
info.m_solverConstraints[nrow].m_cfm = m_cfmLimAng;
}
if (MathUtil.CompareFloat(lostop, histop))
{
// limited low and high simultaneously
info.m_solverConstraints[nrow].m_lowerLimit = -MathUtil.SIMD_INFINITY;
info.m_solverConstraints[nrow].m_upperLimit = MathUtil.SIMD_INFINITY;
}
else if (limit == 1)
{ // low limit
info.m_solverConstraints[nrow].m_lowerLimit = 0;
info.m_solverConstraints[nrow].m_upperLimit = MathUtil.SIMD_INFINITY;
}
else
{ // high limit
info.m_solverConstraints[nrow].m_lowerLimit = -MathUtil.SIMD_INFINITY;
info.m_solverConstraints[nrow].m_upperLimit = 0;
}
// bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that)
float bounce = Math.Abs(1.0f - GetDampingLimAng());
if (bounce > 0.0f)
{
float vel = IndexedVector3.Dot(m_rbA.GetAngularVelocity(), ax1);
vel -= IndexedVector3.Dot(m_rbB.GetAngularVelocity(), ax1);
// only apply bounce if the velocity is incoming, and if the
// resulting c[] exceeds what we already have.
if (limit == 1)
{ // low limit
if (vel < 0)
{
float newc = -bounce * vel;
if (newc > info.m_solverConstraints[nrow].m_rhs)
{
info.m_solverConstraints[nrow].m_rhs = newc;
}
}
}
else
{ // high limit - all those computations are reversed
if (vel > 0)
{
float newc = -bounce * vel;
if (newc < info.m_solverConstraints[nrow].m_rhs)
{
info.m_solverConstraints[nrow].m_rhs = newc;
}
}
}
}
info.m_solverConstraints[nrow].m_rhs *= GetSoftnessLimAng();
} // if(limit)
} // if angular limit or powered
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugConstraints)
{
PrintInfo2(BulletGlobals.g_streamWriter, this, info);
}
}
protected virtual int SetLinearLimits(ConstraintInfo2 info, int row, ref IndexedMatrix transA, ref IndexedMatrix transB, ref IndexedVector3 linVelA, ref IndexedVector3 linVelB, ref IndexedVector3 angVelA, ref IndexedVector3 angVelB)
{
//solve linear limits
RotationalLimitMotor limot = new RotationalLimitMotor();
for (int i = 0; i < 3; i++)
{
if (m_linearLimits.NeedApplyForce(i))
{ // re-use rotational motor code
limot.m_bounce = 0f;
limot.m_currentLimit = m_linearLimits.m_currentLimit[i];
limot.m_currentPosition = m_linearLimits.m_currentLinearDiff[i];
limot.m_currentLimitError = m_linearLimits.m_currentLimitError[i];
limot.m_damping = m_linearLimits.m_damping;
limot.m_enableMotor = m_linearLimits.m_enableMotor[i];
limot.m_hiLimit = m_linearLimits.m_upperLimit[i];
limot.m_limitSoftness = m_linearLimits.m_limitSoftness;
limot.m_loLimit = m_linearLimits.m_lowerLimit[i];
limot.m_maxLimitForce = 0.0f;
limot.m_maxMotorForce = m_linearLimits.m_maxMotorForce[i];
limot.m_targetVelocity = m_linearLimits.m_targetVelocity[i];
IndexedVector3 axis = m_calculatedTransformA._basis.GetColumn(i);
int tempFlags = (((int)m_flags) >> (i * BT_6DOF_FLAGS_AXIS_SHIFT));
SixDofFlags flags = (SixDofFlags)tempFlags;
limot.m_normalCFM = ((flags & SixDofFlags.BT_6DOF_FLAGS_CFM_NORM) != 0) ? m_linearLimits.m_normalCFM[i] : info.m_solverConstraints[0].m_cfm;
limot.m_stopCFM = ((flags & SixDofFlags.BT_6DOF_FLAGS_CFM_STOP) != 0) ? m_linearLimits.m_stopCFM[i] : info.m_solverConstraints[0].m_cfm;
limot.m_stopERP = ((flags & SixDofFlags.BT_6DOF_FLAGS_ERP_STOP) != 0) ? m_linearLimits.m_stopERP[i] : info.erp;
if (m_useOffsetForConstraintFrame)
{
int indx1 = (i + 1) % 3;
int indx2 = (i + 2) % 3;
bool rotAllowed = true; // rotations around orthos to current axis
if (m_angularLimits[indx1].m_currentLimit != 0 && m_angularLimits[indx2].m_currentLimit != 0)
{
rotAllowed = false;
}
row += GetLimitMotorInfo2(limot, ref transA, ref transB, ref linVelA, ref linVelB, ref angVelA, ref angVelB, info, row, ref axis, 0, rotAllowed);
}
else
{
row += GetLimitMotorInfo2(limot, ref transA, ref transB, ref linVelA, ref linVelB, ref angVelA, ref angVelB, info, row, ref axis, 0, false);
}
}
}
return row;
}
public static void PrintInfo2(StreamWriter writer, TypedConstraint constraint, ConstraintInfo2 info2)
{
if (writer != null)
{
writer.WriteLine(String.Format("getInfo2 [{0}] [{1}] [{2}] [{3}]", constraint.m_userConstraintId, constraint.GetObjectType(), (string)constraint.GetRigidBodyA().GetUserPointer(), (string)constraint.GetRigidBodyB().GetUserPointer()));
writer.WriteLine(String.Format("numRows [{0}] fps[{1:0.00000000}] erp[{2:0.00000000}] findex[{3}] numIter[{4}]", info2.m_numRows, info2.fps, info2.erp, info2.findex, info2.m_numIterations));
for (int i = 0; i < info2.m_numRows; ++i)
{
writer.WriteLine(String.Format("TypedConstraint[{0}]", i));
writer.WriteLine("ContactNormal");
MathUtil.PrintVector3(writer, info2.m_solverConstraints[i].m_contactNormal);
writer.WriteLine("rel1pos1CrossNormal");
MathUtil.PrintVector3(writer, info2.m_solverConstraints[i].m_relpos1CrossNormal);
writer.WriteLine("rel1pos2CrossNormal");
MathUtil.PrintVector3(writer, info2.m_solverConstraints[i].m_relpos2CrossNormal);
}
}
}
public override void GetInfo2 (ConstraintInfo2 info)
{
InternalUpdateSprings(info);
base.GetInfo2(info);
}
public static void PrintInfo2(StreamWriter writer, TypedConstraint constraint, ConstraintInfo2 info2)
{
if (writer != null)
{
writer.WriteLine(String.Format("getInfo2 [{0}] [{1}] [{2}] [{3}]", constraint.m_userConstraintId, constraint.GetObjectType(), (string)constraint.GetRigidBodyA().GetUserPointer(), (string)constraint.GetRigidBodyB().GetUserPointer()));
writer.WriteLine(String.Format("numRows [{0}] fps[{1:0.00000000}] erp[{2:0.00000000}] findex[{3}] numIter[{4}]", info2.m_numRows, info2.fps, info2.erp, info2.findex, info2.m_numIterations));
for (int i = 0; i < info2.m_numRows; ++i)
{
writer.WriteLine(String.Format("TypedConstraint[{0}]", i));
writer.WriteLine("ContactNormal");
MathUtil.PrintVector3(writer, info2.m_solverConstraints[i].m_contactNormal);
writer.WriteLine("rel1pos1CrossNormal");
MathUtil.PrintVector3(writer, info2.m_solverConstraints[i].m_relpos1CrossNormal);
writer.WriteLine("rel1pos2CrossNormal");
MathUtil.PrintVector3(writer, info2.m_solverConstraints[i].m_relpos2CrossNormal);
}
}
}
public override void GetInfo2(ConstraintInfo2 info)
{
InternalUpdateSprings(info);
base.GetInfo2(info);
}