///bilateral constraint between two dynamic objects
///positive distance = separation, negative distance = penetration
public static void ResolveSingleBilateral(RigidBody body1, ref IndexedVector3 pos1,
RigidBody body2, ref IndexedVector3 pos2,
float distance, ref IndexedVector3 normal, ref float impulse, float timeStep)
{
float normalLenSqr = normal.LengthSquared();
Debug.Assert(Math.Abs(normalLenSqr) < 1.1f);
if (normalLenSqr > 1.1f)
{
impulse = 0f;
return;
}
IndexedVector3 rel_pos1 = pos1 - body1.GetCenterOfMassPosition();
IndexedVector3 rel_pos2 = pos2 - body2.GetCenterOfMassPosition();
//this jacobian entry could be re-used for all iterations
IndexedVector3 vel1 = body1.GetVelocityInLocalPoint(ref rel_pos1);
IndexedVector3 vel2 = body2.GetVelocityInLocalPoint(ref rel_pos2);
IndexedVector3 vel = vel1 - vel2;
IndexedBasisMatrix m1 = body1.GetCenterOfMassTransform()._basis.Transpose();
IndexedBasisMatrix m2 = body2.GetCenterOfMassTransform()._basis.Transpose();
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(),
body1.GetCenterOfMassTransform()._basis.Transpose() * body1.GetAngularVelocity(),
body2.GetLinearVelocity(),
body2.GetCenterOfMassTransform()._basis.Transpose() * body2.GetAngularVelocity());
float a = jacDiagABInv;
rel_vel = normal.Dot(ref 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;
}
}
public SliderConstraint(RigidBody rbB, ref IndexedMatrix frameInB, bool useLinearReferenceFrameA)
: base(TypedConstraintType.SLIDER_CONSTRAINT_TYPE, GetFixedBody(), rbB)
{
m_frameInB = frameInB;
m_frameInA = rbB.GetCenterOfMassTransform() * m_frameInB;
InitParams();
}
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;
}
// constructor
// anchor, axis1 and axis2 are in world coordinate system
// axis1 must be orthogonal to axis2
public Hinge2Constraint(RigidBody rbA, RigidBody rbB, ref IndexedVector3 anchor, ref IndexedVector3 axis1, ref IndexedVector3 axis2) : base(rbA, rbB, IndexedMatrix.Identity, IndexedMatrix.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
IndexedVector3 zAxis = IndexedVector3.Normalize(axis1);
IndexedVector3 xAxis = IndexedVector3.Normalize(axis2);
IndexedVector3 yAxis = IndexedVector3.Cross(zAxis, xAxis); // we want right coordinate system
IndexedMatrix frameInW = IndexedMatrix.Identity;
frameInW._basis = new IndexedBasisMatrix(xAxis.X, yAxis.X, zAxis.X,
xAxis.Y, yAxis.Y, zAxis.Y,
xAxis.Z, yAxis.Z, zAxis.Z);
frameInW._origin = anchor;
// now get constraint frame in local coordinate systems
m_frameInA = rbA.GetCenterOfMassTransform().Inverse() * frameInW;
m_frameInB = rbB.GetCenterOfMassTransform().Inverse() * frameInW;
// sei limits
SetLinearLowerLimit(new IndexedVector3(0.0f, 0.0f, -1.0f));
SetLinearUpperLimit(new IndexedVector3(0.0f, 0.0f, 1.0f));
// like front wheels of a car
SetAngularLowerLimit(new IndexedVector3(1.0f, 0.0f, -MathUtil.SIMD_HALF_PI * 0.5f));
SetAngularUpperLimit(new IndexedVector3(-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();
}
// constructor
// anchor, axis1 and axis2 are in world coordinate system
// axis1 must be orthogonal to axis2
public UniversalConstraint(RigidBody rbA, RigidBody rbB, ref IndexedVector3 anchor, ref IndexedVector3 axis1, ref IndexedVector3 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
IndexedVector3 zAxis = IndexedVector3.Normalize(m_axis1);
IndexedVector3 yAxis = IndexedVector3.Normalize(m_axis2);
IndexedVector3 xAxis = IndexedVector3.Cross(yAxis, zAxis); // we want right coordinate system
IndexedMatrix frameInW = IndexedMatrix.Identity;
frameInW._basis = new IndexedBasisMatrix(xAxis.X, yAxis.X, zAxis.X,
xAxis.Y, yAxis.Y, zAxis.Y,
xAxis.Z, yAxis.Z, zAxis.Z);
frameInW._origin = 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 = rbA.GetCenterOfMassTransform().Inverse() * frameInW;
m_frameInB = rbB.GetCenterOfMassTransform().Inverse() * frameInW;
// sei limits
SetLinearLowerLimit(IndexedVector3.Zero);
SetLinearUpperLimit(IndexedVector3.Zero);
SetAngularLowerLimit(new IndexedVector3(0.0f, -MathUtil.SIMD_HALF_PI + UNIV_EPS, -MathUtil.SIMD_PI + UNIV_EPS));
SetAngularUpperLimit(new IndexedVector3(0.0f, MathUtil.SIMD_HALF_PI - UNIV_EPS, MathUtil.SIMD_PI - UNIV_EPS));
}
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;
}
public Point2PointConstraint(RigidBody rbA, ref IndexedVector3 pivotInA) : base(TypedConstraintType.POINT2POINT_CONSTRAINT_TYPE, rbA)
{
m_pivotInA = pivotInA;
m_pivotInB = rbA.GetCenterOfMassTransform() * pivotInA;
}
