// constructor
// anchor, axis1 and axis2 are in world coordinate system
// axis1 must be orthogonal to axis2
public UniversalConstraint(RigidBody rbA, RigidBody rbB, ref Vector3 anchor, ref Vector3 axis1, ref Vector3 axis2)
: base(rbA, rbB, ref BulletGlobals.IdentityMatrix, ref BulletGlobals.IdentityMatrix, true)
{
m_anchor = anchor;
m_axis1 = axis1;
m_axis2 = axis2;
// build frame basis
// 6DOF constraint uses Euler angles and to define limits
// it is assumed that rotational order is :
// Z - first, allowed limits are (-PI,PI);
// new position of Y - second (allowed limits are (-PI/2 + epsilon, PI/2 - epsilon), where epsilon is a small positive number
// used to prevent constraint from instability on poles;
// new position of X, allowed limits are (-PI,PI);
// So to simulate ODE Universal joint we should use parent axis as Z, child axis as Y and limit all other DOFs
// Build the frame in world coordinate system first
Vector3 zAxis = Vector3.Normalize(axis1);
Vector3 yAxis = Vector3.Normalize(axis2);
Vector3 xAxis = Vector3.Cross(yAxis,zAxis); // we want right coordinate system
Matrix frameInW = Matrix.Identity;
MathUtil.SetBasis(ref frameInW,ref xAxis,ref yAxis,ref zAxis);
frameInW.Translation = anchor;
// now get constraint frame in local coordinate systems
//m_frameInA = MathUtil.inverseTimes(rbA.getCenterOfMassTransform(),frameInW);
//m_frameInB = MathUtil.inverseTimes(rbB.getCenterOfMassTransform(),frameInW);
m_frameInA = MathUtil.BulletMatrixMultiply(Matrix.Invert(rbA.GetCenterOfMassTransform()), frameInW);
m_frameInB = MathUtil.BulletMatrixMultiply(Matrix.Invert(rbB.GetCenterOfMassTransform()), frameInW);
// sei limits
SetLinearLowerLimit(Vector3.Zero);
SetLinearUpperLimit(Vector3.Zero);
SetAngularLowerLimit(new Vector3(0.0f, -MathUtil.SIMD_HALF_PI + UNIV_EPS, -MathUtil.SIMD_PI + UNIV_EPS));
SetAngularUpperLimit(new Vector3(0.0f, MathUtil.SIMD_HALF_PI - UNIV_EPS, MathUtil.SIMD_PI - UNIV_EPS));
}
///bilateral constraint between two dynamic objects
///positive distance = separation, negative distance = penetration
public static void ResolveSingleBilateral(RigidBody body1, ref Vector3 pos1,
RigidBody body2, ref Vector3 pos2,
float distance, ref Vector3 normal, ref float impulse, float timeStep)
{
float normalLenSqr = normal.LengthSquared();
Debug.Assert(System.Math.Abs(normalLenSqr) < 1.1f);
if (normalLenSqr > 1.1f)
{
impulse = 0f;
return;
}
Vector3 rel_pos1 = pos1 - body1.GetCenterOfMassPosition();
Vector3 rel_pos2 = pos2 - body2.GetCenterOfMassPosition();
//this jacobian entry could be re-used for all iterations
Vector3 vel1 = body1.GetVelocityInLocalPoint(ref rel_pos1);
Vector3 vel2 = body2.GetVelocityInLocalPoint(ref rel_pos2);
Vector3 vel = vel1 - vel2;
Matrix m1 = MathUtil.TransposeBasis(body1.GetCenterOfMassTransform());
Matrix m2 = MathUtil.TransposeBasis(body2.GetCenterOfMassTransform());
JacobianEntry jac = new JacobianEntry(m1,m2,rel_pos1,rel_pos2,normal,
body1.GetInvInertiaDiagLocal(),body1.GetInvMass(),
body2.GetInvInertiaDiagLocal(),body2.GetInvMass());
float jacDiagAB = jac.GetDiagonal();
float jacDiagABInv = 1f / jacDiagAB;
float rel_vel = jac.GetRelativeVelocity(
body1.GetLinearVelocity(),Vector3.TransformNormal(body1.GetAngularVelocity(),m1),
body2.GetLinearVelocity(),Vector3.TransformNormal(body2.GetAngularVelocity(),m2));
float a = jacDiagABInv;
rel_vel = Vector3.Dot(normal,vel);
//todo: move this into proper structure
float contactDamping = 0.2f;
if(ONLY_USE_LINEAR_MASS)
{
float massTerm = 1f / (body1.GetInvMass() + body2.GetInvMass());
impulse = - contactDamping * rel_vel * massTerm;
}
else
{
float velocityImpulse = -contactDamping * rel_vel * jacDiagABInv;
impulse = velocityImpulse;
}
}
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();
}
// constructor
// anchor, axis1 and axis2 are in world coordinate system
// axis1 must be orthogonal to axis2
public Hinge2Constraint(RigidBody rbA, RigidBody rbB, ref Vector3 anchor, ref Vector3 axis1, ref Vector3 axis2) : base(rbA,rbB,Matrix.Identity,Matrix.Identity,true)
{
m_anchor = anchor;
m_axis1 = axis1;
m_axis2 = axis2;
// build frame basis
// 6DOF constraint uses Euler angles and to define limits
// it is assumed that rotational order is :
// Z - first, allowed limits are (-PI,PI);
// new position of Y - second (allowed limits are (-PI/2 + epsilon, PI/2 - epsilon), where epsilon is a small positive number
// used to prevent constraint from instability on poles;
// new position of X, allowed limits are (-PI,PI);
// So to simulate ODE Universal joint we should use parent axis as Z, child axis as Y and limit all other DOFs
// Build the frame in world coordinate system first
Vector3 zAxis = Vector3.Normalize(axis1);
Vector3 xAxis = Vector3.Normalize(axis2);
Vector3 yAxis = Vector3.Cross(zAxis,xAxis); // we want right coordinate system
Matrix frameInW = Matrix.Identity;
MathUtil.SetBasis(ref frameInW, ref xAxis, ref yAxis, ref zAxis);
frameInW.Translation = anchor;
// now get constraint frame in local coordinate systems
m_frameInA = MathUtil.InverseTimes(rbA.GetCenterOfMassTransform(),frameInW);
m_frameInB = MathUtil.InverseTimes(rbB.GetCenterOfMassTransform(), frameInW);
// sei limits
SetLinearLowerLimit(new Vector3(0.0f, 0.0f, -1.0f));
SetLinearUpperLimit(new Vector3(0.0f, 0.0f, 1.0f));
// like front wheels of a car
SetAngularLowerLimit(new Vector3(1.0f, 0.0f, -MathUtil.SIMD_HALF_PI * 0.5f));
SetAngularUpperLimit(new Vector3(-1.0f, 0.0f, MathUtil.SIMD_HALF_PI * 0.5f));
// enable suspension
EnableSpring(2, true);
SetStiffness(2, MathUtil.SIMD_PI * MathUtil.SIMD_PI * 4.0f); // period 1 sec for 1 kilogramm weel :-)
SetDamping(2, 0.01f);
SetEquilibriumPoint();
}
public Point2PointConstraint(RigidBody rbA,ref Vector3 pivotInA) : base(TypedConstraintType.POINT2POINT_CONSTRAINT_TYPE,rbA)
{
m_pivotInA = pivotInA;
m_pivotInB = Vector3.Transform(pivotInA, rbA.GetCenterOfMassTransform());
}
public HingeConstraint(RigidBody rbA, RigidBody rbB, ref Vector3 pivotInA, ref Vector3 pivotInB, ref Vector3 axisInA, ref Vector3 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.Translation = pivotInA;
m_flags = 0;
// since no frame is given, assume this to be zero angle and just pick rb transform axis
Vector3 rbAxisA1 = MathUtil.MatrixColumn(rbA.GetCenterOfMassTransform(),0);
Vector3 rbAxisA2 = Vector3.Zero;
float projection = Vector3.Dot(axisInA,rbAxisA1);
if (projection >= 1.0f - MathUtil.SIMD_EPSILON)
{
rbAxisA1 = -MathUtil.MatrixColumn(rbA.GetCenterOfMassTransform(),2);
rbAxisA2 = MathUtil.MatrixColumn(rbA.GetCenterOfMassTransform(),1);
}
else if (projection <= -1.0f + MathUtil.SIMD_EPSILON)
{
rbAxisA1 = MathUtil.MatrixColumn(rbA.GetCenterOfMassTransform(),2);
rbAxisA2 = MathUtil.MatrixColumn(rbA.GetCenterOfMassTransform(),1);
}
else
{
rbAxisA2 = Vector3.Cross(axisInA,rbAxisA1);
rbAxisA1 = Vector3.Cross(rbAxisA2,axisInA);
}
MathUtil.SetBasis(ref m_rbAFrame,ref rbAxisA1,ref rbAxisA2,ref axisInA);
Quaternion rotationArc = MathUtil.ShortestArcQuat(ref axisInA, ref axisInB);
Vector3 rbAxisB1 = MathUtil.QuatRotate(ref rotationArc, ref rbAxisA1);
Vector3 rbAxisB2 = Vector3.Cross(axisInB,rbAxisB1);
m_rbBFrame.Translation = pivotInB;
MathUtil.SetBasis(ref m_rbBFrame,ref rbAxisB1,ref rbAxisB2,ref axisInB);
//start with free
m_lowerLimit = 1f;
m_upperLimit = -1f;
m_biasFactor = 0.3f;
m_relaxationFactor = 1.0f;
m_limitSoftness = 0.9f;
m_solveLimit = false;
m_referenceSign = m_useReferenceFrameA ? -1f : 1f;
}
public HingeConstraint(RigidBody rbA, ref Vector3 pivotInA, ref Vector3 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;
// since no frame is given, assume this to be zero angle and just pick rb transform axis
// fixed axis in worldspace
Vector3 rbAxisA1 = Vector3.Zero, rbAxisA2 = Vector3.Zero;
TransformUtil.PlaneSpace1(ref axisInA, ref rbAxisA1, ref rbAxisA2);
m_rbAFrame.Translation = pivotInA;
MathUtil.SetBasis(ref m_rbAFrame,ref rbAxisA1,ref rbAxisA2,ref axisInA);
Vector3 axisInB = Vector3.TransformNormal(axisInA,rbA.GetCenterOfMassTransform());
Quaternion rotationArc = MathUtil.ShortestArcQuat(ref axisInA, ref axisInB);
Vector3 rbAxisB1 = MathUtil.QuatRotate(ref rotationArc, ref rbAxisA1);
Vector3 rbAxisB2 = Vector3.Cross(axisInB,rbAxisB1);
m_rbBFrame.Translation = Vector3.Transform(pivotInA,rbA.GetCenterOfMassTransform());
MathUtil.SetBasis(ref m_rbBFrame, ref rbAxisB1, ref rbAxisB2, ref axisInB);
//start with free
m_lowerLimit = 1f;
m_upperLimit = -1f;
m_biasFactor = 0.3f;
m_relaxationFactor = 1.0f;
m_limitSoftness = 0.9f;
m_solveLimit = false;
m_referenceSign = m_useReferenceFrameA ? -1.0f : 1.0f;
}
public SliderConstraint(RigidBody rbB, ref Matrix frameInB, bool useLinearReferenceFrameA)
: base(TypedConstraintType.SLIDER_CONSTRAINT_TYPE, GetFixedBody(), rbB)
{
m_frameInB = frameInB;
m_frameInA = MathUtil.BulletMatrixMultiply(rbB.GetCenterOfMassTransform(),m_frameInB);
InitParams();
}