public Generic6DofConstraint(RigidBody rbA, RigidBody rbB, ref IndexedMatrix frameInA, ref IndexedMatrix frameInB, bool useLinearReferenceFrameA)
: base(TypedConstraintType.D6_CONSTRAINT_TYPE, rbA, rbB)
{
m_frameInA = frameInA;
m_frameInB = frameInB;
m_useLinearReferenceFrameA = useLinearReferenceFrameA;
m_useOffsetForConstraintFrame = D6_USE_FRAME_OFFSET;
m_linearLimits = new TranslationalLimitMotor();
m_angularLimits[0] = new RotationalLimitMotor();
m_angularLimits[1] = new RotationalLimitMotor();
m_angularLimits[2] = new RotationalLimitMotor();
CalculateTransforms();
}
public Generic6DofConstraint(RigidBody rbB, ref IndexedMatrix frameInB, bool useLinearReferenceFrameB)
: base(TypedConstraintType.D6_CONSTRAINT_TYPE, GetFixedBody(), rbB)
{
m_frameInB = frameInB;
m_useLinearReferenceFrameA = useLinearReferenceFrameB;
m_useOffsetForConstraintFrame = D6_USE_FRAME_OFFSET;
m_linearLimits = new TranslationalLimitMotor();
m_angularLimits[0] = new RotationalLimitMotor();
m_angularLimits[1] = new RotationalLimitMotor();
m_angularLimits[2] = new RotationalLimitMotor();
///not providing rigidbody A means implicitly using worldspace for body A
m_frameInA = rbB.GetCenterOfMassTransform() * m_frameInB;
CalculateTransforms();
}
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 RotationalLimitMotor(RotationalLimitMotor limot)
{
m_targetVelocity = limot.m_targetVelocity;
m_maxMotorForce = limot.m_maxMotorForce;
m_limitSoftness = limot.m_limitSoftness;
m_loLimit = limot.m_loLimit;
m_hiLimit = limot.m_hiLimit;
m_normalCFM = limot.m_normalCFM;
m_stopERP = limot.m_stopERP;
m_stopCFM = limot.m_stopCFM;
m_bounce = limot.m_bounce;
m_currentLimit = limot.m_currentLimit;
m_currentLimitError = limot.m_currentLimitError;
m_enableMotor = limot.m_enableMotor;
}
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;
}
