public Generic6DofConstraint(RigidBody rbA, RigidBody rbB, ref Matrix frameInA, ref Matrix 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_flags = 0;
m_linearLimits = new TranslationalLimitMotor();
m_angularLimits[0] = new RotationalLimitMotor();
m_angularLimits[1] = new RotationalLimitMotor();
m_angularLimits[2] = new RotationalLimitMotor();
CalculateTransforms();
}
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;
}
protected Generic6DofConstraint(RigidBody rbB, ref Matrix frameInB, bool useLinearReferenceFrameB) : base(TypedConstraintType.D6_CONSTRAINT_TYPE,GetFixedBody(),rbB)
{
m_frameInB = frameInB;
m_useLinearReferenceFrameA = useLinearReferenceFrameB;
m_useOffsetForConstraintFrame = D6_USE_FRAME_OFFSET;
m_flags = 0;
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 = MathUtil.BulletMatrixMultiply(rbB.GetCenterOfMassTransform(),m_frameInB);
CalculateTransforms();
}
protected virtual int SetLinearLimits(ConstraintInfo2 info, int row, ref Matrix transA,ref Matrix transB,ref Vector3 linVelA,ref Vector3 linVelB,ref Vector3 angVelA,ref Vector3 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 = MathUtil.VectorComponent(ref m_linearLimits.m_currentLinearDiff,i);
limot.m_currentLimitError = MathUtil.VectorComponent(ref m_linearLimits.m_currentLimitError,i);
limot.m_damping = m_linearLimits.m_damping;
limot.m_enableMotor = m_linearLimits.m_enableMotor[i];
limot.m_hiLimit = MathUtil.VectorComponent(m_linearLimits.m_upperLimit,i);
limot.m_limitSoftness = m_linearLimits.m_limitSoftness;
limot.m_loLimit = MathUtil.VectorComponent(m_linearLimits.m_lowerLimit,i);
limot.m_maxLimitForce = 0f;
limot.m_maxMotorForce = MathUtil.VectorComponent(m_linearLimits.m_maxMotorForce,i);
limot.m_targetVelocity = MathUtil.VectorComponent(m_linearLimits.m_targetVelocity,i);
Vector3 axis = MathUtil.MatrixColumn(m_calculatedTransformA,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) ? MathUtil.VectorComponent(ref m_linearLimits.m_normalCFM,i) : info.m_solverConstraints[0].m_cfm;
limot.m_stopCFM = ((flags & SixDofFlags.BT_6DOF_FLAGS_CFM_STOP) != 0) ? MathUtil.VectorComponent(ref m_linearLimits.m_stopCFM,i) : info.m_solverConstraints[0].m_cfm;
limot.m_stopERP = ((flags & SixDofFlags.BT_6DOF_FLAGS_ERP_STOP) != 0) ? MathUtil.VectorComponent(ref 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 virtual int GetLimitMotorInfo2(RotationalLimitMotor limot,
ref Matrix transA,ref Matrix transB,ref Vector3 linVelA,
ref Vector3 linVelB,ref Vector3 angVelA,ref Vector3 angVelB,
ConstraintInfo2 info, int row, ref Vector3 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
//btScalar* J1 = rotational ? info->m_J1angularAxis : info->m_J1linearAxis;
//btScalar* 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)
{
Vector3 tmpA = Vector3.Zero, tmpB= Vector3.Zero, relA= Vector3.Zero, relB= Vector3.Zero;
// get vector from bodyB to frameB in WCS
relB = m_calculatedTransformB.Translation - transB.Translation;
// get its projection to constraint axis
Vector3 projB = ax1 * Vector3.Dot(relB,ax1);
// get vector directed from bodyB to constraint axis (and orthogonal to it)
Vector3 orthoB = relB - projB;
// same for bodyA
relA = m_calculatedTransformA.Translation - transA.Translation;
Vector3 projA = ax1 * Vector3.Dot(relA,ax1);
Vector3 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
Vector3 totalDist = projA + ax1 * desiredOffs - projB;
// get offset vectors relA and relB
relA = orthoA + totalDist * m_factA;
relB = orthoB - totalDist * m_factB;
tmpA = Vector3.Cross(relA,ax1);
tmpB = Vector3.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
{
Vector3 ltd; // Linear Torque Decoupling vector
Vector3 c = m_calculatedTransformB.Translation - transA.Translation;
ltd = Vector3.Cross(c,ax1);
info.m_solverConstraints[row].m_relpos1CrossNormal = ltd;
MathUtil.ZeroCheckVector(info.m_solverConstraints[row].m_relpos1CrossNormal);
c = m_calculatedTransformB.Translation - transB.Translation;
ltd = -Vector3.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 = Vector3.Dot(angVelA,ax1);
vel -= Vector3.Dot(angVelB,ax1);
}
else
{
vel = Vector3.Dot(linVelA,ax1);
vel -= Vector3.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;
}