public HingeConstraint(RigidBody rbA, ref IndexedMatrix rbAFrame, bool useReferenceFrameA)
: base(TypedConstraintType.HINGE_CONSTRAINT_TYPE, rbA)
{
m_rbAFrame = rbAFrame;
m_rbBFrame = rbAFrame;
m_angularOnly = false;
m_enableAngularMotor = false;
m_useOffsetForConstraintFrame = HINGE_USE_FRAME_OFFSET;
m_useReferenceFrameA = useReferenceFrameA;
m_flags = 0;
m_rbBFrame._origin = m_rbA.GetCenterOfMassTransform() * (m_rbAFrame._origin);
#if _BT_USE_CENTER_LIMIT_
m_limit = new AngularLimit();
#else
//start with free
m_lowerLimit = 1f;
m_upperLimit = -1f;
m_biasFactor = 0.3f;
m_relaxationFactor = 1.0f;
m_limitSoftness = 0.9f;
m_solveLimit = false;
#endif
m_referenceSign = m_useReferenceFrameA ? -1.0f : 1.0f;
}
public HingeConstraint(RigidBody rbA, ref IndexedVector3 pivotInA, ref IndexedVector3 axisInA, bool useReferenceFrameA)
: base(TypedConstraintType.HINGE_CONSTRAINT_TYPE, rbA)
{
m_angularOnly = false;
m_enableAngularMotor = false;
m_useReferenceFrameA = useReferenceFrameA;
m_useOffsetForConstraintFrame = HINGE_USE_FRAME_OFFSET;
m_flags = 0;
#if _BT_USE_CENTER_LIMIT_
m_limit = new AngularLimit();
#endif
// since no frame is given, assume this to be zero angle and just pick rb transform axis
// fixed axis in worldspace
IndexedVector3 rbAxisA1, rbAxisA2;
TransformUtil.PlaneSpace1(ref axisInA, out rbAxisA1, out rbAxisA2);
m_rbAFrame._origin = pivotInA;
//m_rbAFrame._basis = new IndexedBasisMatrix(ref rbAxisA1, ref rbAxisA2, ref axisInA);
m_rbAFrame._basis = new IndexedBasisMatrix(rbAxisA1.X, rbAxisA2.X, axisInA.X,
rbAxisA1.Y, rbAxisA2.Y, axisInA.Y,
rbAxisA1.Z, rbAxisA2.Z, axisInA.Z);
IndexedVector3 axisInB = rbA.GetCenterOfMassTransform()._basis *axisInA;
IndexedQuaternion rotationArc = MathUtil.ShortestArcQuat(ref axisInA, ref axisInB);
IndexedVector3 rbAxisB1 = MathUtil.QuatRotate(ref rotationArc, ref rbAxisA1);
IndexedVector3 rbAxisB2 = IndexedVector3.Cross(axisInB, rbAxisB1);
m_rbBFrame._origin = rbA.GetCenterOfMassTransform() * pivotInA;
//m_rbBFrame._basis = new IndexedBasisMatrix(ref rbAxisB1, ref rbAxisB2, ref axisInB);
m_rbBFrame._basis = new IndexedBasisMatrix(rbAxisB1.X, rbAxisB2.X, axisInB.X,
rbAxisB1.Y, rbAxisB2.Y, axisInB.Y,
rbAxisB1.Z, rbAxisB2.Z, axisInB.Z);
//start with free
#if !_BT_USE_CENTER_LIMIT_
m_lowerLimit = 1f;
m_upperLimit = -1f;
m_biasFactor = 0.3f;
m_relaxationFactor = 1.0f;
m_limitSoftness = 0.9f;
m_solveLimit = false;
#endif
m_referenceSign = m_useReferenceFrameA ? -1.0f : 1.0f;
}
public HingeConstraint(RigidBody rbA, RigidBody rbB, ref IndexedVector3 pivotInA, ref IndexedVector3 pivotInB, ref IndexedVector3 axisInA, ref IndexedVector3 axisInB, bool useReferenceFrameA)
: base(TypedConstraintType.HINGE_CONSTRAINT_TYPE, rbA, rbB)
{
m_angularOnly = false;
m_enableAngularMotor = false;
m_useOffsetForConstraintFrame = HINGE_USE_FRAME_OFFSET;
m_useReferenceFrameA = useReferenceFrameA;
m_rbAFrame._origin = pivotInA;
#if _BT_USE_CENTER_LIMIT_
m_limit = new AngularLimit();
#endif
m_flags = 0;
// since no frame is given, assume this to be zero angle and just pick rb transform axis
IndexedVector3 rbAxisA1 = rbA.GetCenterOfMassTransform()._basis.GetColumn(0);
IndexedVector3 rbAxisA2 = IndexedVector3.Zero;
float projection = IndexedVector3.Dot(axisInA, rbAxisA1);
if (projection >= 1.0f - MathUtil.SIMD_EPSILON)
{
rbAxisA1 = -rbA.GetCenterOfMassTransform()._basis.GetColumn(2);
rbAxisA2 = rbA.GetCenterOfMassTransform()._basis.GetColumn(1);
}
else if (projection <= -1.0f + MathUtil.SIMD_EPSILON)
{
rbAxisA1 = rbA.GetCenterOfMassTransform()._basis.GetColumn(2);
rbAxisA2 = rbA.GetCenterOfMassTransform()._basis.GetColumn(1);
}
else
{
rbAxisA2 = IndexedVector3.Cross(axisInA, rbAxisA1);
rbAxisA1 = IndexedVector3.Cross(rbAxisA2, axisInA);
}
//m_rbAFrame._basis = new IndexedBasisMatrix(ref rbAxisA1, ref rbAxisA2, ref axisInA);
m_rbAFrame._basis = new IndexedBasisMatrix(rbAxisA1.X, rbAxisA2.X, axisInA.X,
rbAxisA1.Y, rbAxisA2.Y, axisInA.Y,
rbAxisA1.Z, rbAxisA2.Z, axisInA.Z);
IndexedQuaternion rotationArc = MathUtil.ShortestArcQuat(ref axisInA, ref axisInB);
IndexedVector3 rbAxisB1 = MathUtil.QuatRotate(ref rotationArc, ref rbAxisA1);
IndexedVector3 rbAxisB2 = IndexedVector3.Cross(axisInB, rbAxisB1);
m_rbBFrame._origin = pivotInB;
//m_rbBFrame._basis = new IndexedBasisMatrix(ref rbAxisB1, ref rbAxisB2, ref axisInB);
m_rbBFrame._basis = new IndexedBasisMatrix(rbAxisB1.X, rbAxisB2.X, axisInB.X,
rbAxisB1.Y, rbAxisB2.Y, axisInB.Y,
rbAxisB1.Z, rbAxisB2.Z, axisInB.Z);
#if !_BT_USE_CENTER_LIMIT_
//start with free
m_lowerLimit = float(1.0f);
m_upperLimit = float(-1.0f);
m_biasFactor = 0.3f;
m_relaxationFactor = 1.0f;
m_limitSoftness = 0.9f;
m_solveLimit = false;
#endif
m_referenceSign = m_useReferenceFrameA ? -1f : 1f;
}
