public void Reset()
{
m_relpos1CrossNormal = IndexedVector3.Zero;
m_contactNormal = IndexedVector3.Zero;
m_relpos2CrossNormal = IndexedVector3.Zero;
m_angularComponentA = IndexedVector3.Zero;
m_angularComponentB = IndexedVector3.Zero;
m_appliedPushImpulse = 0f;
m_appliedImpulse = 0f;
m_friction = 0f;
m_jacDiagABInv = 0f;
m_numConsecutiveRowsPerKernel = 0;
m_frictionIndex = 0;
m_solverBodyA = null;
m_companionIdA = 0;
m_solverBodyB = null;
m_companionIdB = 0;
m_originalContactPoint = null;
//m_originalContactPointConstraint = null;
m_rhs = 0f;
m_cfm = 0;
m_lowerLimit = 0f;
m_upperLimit = 0f;
m_rhsPenetration = 0f;
m_overrideNumSolverIterations = -1;
}
public RaycastVehicle(VehicleTuning tuning, RigidBody chassis, IVehicleRaycaster raycaster)
{
m_vehicleRaycaster = raycaster;
m_pitchControl = 0f;
m_chassisBody = chassis;
m_indexRightAxis = 0;
m_indexUpAxis = 1;
m_indexForwardAxis = 2;
DefaultInit(ref tuning);
}
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();
}
//response between two dynamic objects without friction, assuming 0 penetration depth
public static float ResolveSingleCollision(
RigidBody body1,
CollisionObject colObj2,
ref IndexedVector3 contactPositionWorld,
ref IndexedVector3 contactNormalOnB,
ContactSolverInfo solverInfo,
float distance)
{
RigidBody body2 = RigidBody.Upcast(colObj2);
IndexedVector3 normal = contactNormalOnB;
IndexedVector3 rel_pos1 = contactPositionWorld - body1.GetWorldTransform()._origin;
IndexedVector3 rel_pos2 = contactPositionWorld - colObj2.GetWorldTransform()._origin;
IndexedVector3 vel1 = body1.GetVelocityInLocalPoint(ref rel_pos1);
IndexedVector3 vel2 = body2 != null ? body2.GetVelocityInLocalPoint(ref rel_pos2) : IndexedVector3.Zero;
IndexedVector3 vel = vel1 - vel2;
float rel_vel = normal.Dot(ref vel);
float combinedRestitution = body1.GetRestitution() * colObj2.GetRestitution();
float restitution = combinedRestitution * -rel_vel;
float positionalError = solverInfo.m_erp * -distance / solverInfo.m_timeStep;
float velocityError = -(1.0f + restitution) * rel_vel;// * damping;
float denom0 = body1.ComputeImpulseDenominator(ref contactPositionWorld, ref normal);
float denom1 = body2 != null ? body2.ComputeImpulseDenominator(ref contactPositionWorld, ref normal) : 0.0f;
float relaxation = 1.0f;
float jacDiagABInv = relaxation / (denom0 + denom1);
float penetrationImpulse = positionalError * jacDiagABInv;
float velocityImpulse = velocityError * jacDiagABInv;
float normalImpulse = penetrationImpulse + velocityImpulse;
normalImpulse = 0.0f > normalImpulse ? 0.0f : normalImpulse;
body1.ApplyImpulse(normal * (normalImpulse), rel_pos1);
if (body2 != null)
{
body2.ApplyImpulse(-normal * (normalImpulse), rel_pos2);
}
return normalImpulse;
}
// 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));
}
///internal method used by the constraint solver, don't use them directly
public virtual void SolveConstraintObsolete(RigidBody bodyA,RigidBody bodyB,float timeStep)
{
}
public TypedConstraint(TypedConstraintType type, RigidBody rbA, RigidBody rbB)
: base((int)type)
{
m_userConstraintType = -1;
m_userConstraintId = -1;
m_constraintType = type;
m_rbA = rbA;
m_rbB = rbB;
m_appliedImpulse = 0f;
m_breakingImpulseThreshold = MathUtil.SIMD_INFINITY;
m_isEnabled = true;
m_dbgDrawSize = DEFAULT_DEBUGDRAW_SIZE;
{
GetFixedBody().SetMassProps(0f, IndexedVector3.Zero);
}
}
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 void BuildCollisionData(Color[] map, int width, int height, Vector3 bottomLeft)
{
Vector2 dim = Vector2.One;
Vector3 scale = new Vector3(dim, 1);
BoxShape collisionBoxShape = new BoxShape(new IndexedVector3(0.5f));
for (int x = 0; x < width; x++)
{
for (int y = 0; y < height; y++)
{
int currentIndex = x + y * width;
if (map[currentIndex] == Color.White)
{
float yOffset = -dim.Y;//0f;// -(dim.Y / 2f);
Vector3 position = new Vector3(x - bottomLeft.X, (bottomLeft.Y - y) + yOffset, bottomLeft.Z);
m_waterLocations.Add(position);
}
if (map[currentIndex] == Color.White)
{
// check the 4 ordinals , if we're surrounded by other solid blocks
// then we don't need a block here.
bool upSet = false;
bool downSet = false;
bool leftSet = false;
bool rightSet = false;
if (x >= 1 && x < width - 1)
{
if (map[currentIndex - 1] == Color.White)
{
leftSet = true;
}
if (map[currentIndex + 1] == Color.White)
{
rightSet = true;
}
}
if (y >= 1 && y < height - 1)
{
if (map[currentIndex - height] == Color.White)
{
upSet = true;
}
if (map[currentIndex + height] == Color.White)
{
downSet = true;
}
}
// if we're not surrounded by blocks then add in.
if (!(upSet && downSet && leftSet && rightSet))
{
Object rigifdBody;
float yOffset = -dim.Y;//0f;// -(dim.Y / 2f);
Vector3 position = new Vector3(x - bottomLeft.X, (bottomLeft.Y - y) + yOffset, bottomLeft.Z);
RigidBodyConstructionInfo constructionInfo = new BulletXNA.BulletDynamics.RigidBodyConstructionInfo(0f, null, (BulletXNA.BulletCollision.CollisionShape)collisionBoxShape);
RigidBody rigidBody = new BulletXNA.BulletDynamics.RigidBody(constructionInfo);
Matrix bsm = Matrix.CreateTranslation(position);
rigidBody.SetWorldTransform(bsm);
// FIXME MAN - setup some collision flags on these bodies...
BulletXNA.BulletCollision.CollisionFilterGroups flags = (BulletXNA.BulletCollision.CollisionFilterGroups)(1 << 8);
BulletXNA.BulletCollision.CollisionFilterGroups mask = (BulletXNA.BulletCollision.CollisionFilterGroups)(1 << 9);
//rigidBody.CollisionFlags |= (BulletSharp.CollisionFlags)CollisionObjectType.Ground;
m_dynamicsWorld.AddRigidBody(rigidBody, flags, mask);
}
}
// Build water ghost objects.
foreach (Vector3 pos in m_waterLocations)
{
GhostObject ghostObject = new GhostObject();
ghostObject.SetCollisionShape((BulletXNA.BulletCollision.CollisionShape)collisionBoxShape);
CollisionFilterGroups flags = (CollisionFilterGroups)(1 << 10);
CollisionFilterGroups mask = (CollisionFilterGroups)(1 << 9);
ghostObject.SetCollisionFlags(CollisionFlags.CF_NO_CONTACT_RESPONSE | CollisionFlags.CF_STATIC_OBJECT); // We can choose to make it "solid" if we want...
ghostObject.SetWorldTransform(BulletXNA.LinearMath.IndexedMatrix.CreateTranslation(pos));
m_dynamicsWorld.AddCollisionObject(ghostObject, flags, mask);
break;
}
}
}
}
public HingeConstraint(RigidBody rbA, RigidBody rbB, ref IndexedVector3 pivotInA, ref IndexedVector3 pivotInB, ref IndexedVector3 axisInA, ref IndexedVector3 axisInB)
: this(rbA, rbB, ref pivotInA, ref pivotInB, ref axisInA, ref axisInB, false)
{
}
public HingeConstraint(RigidBody rbA, ref IndexedMatrix rbAFrame)
: this(rbA, ref rbAFrame, false)
{
}
public Point2PointConstraint(RigidBody rbA, ref IndexedVector3 pivotInA) : base(TypedConstraintType.POINT2POINT_CONSTRAINT_TYPE, rbA)
{
m_pivotInA = pivotInA;
m_pivotInB = rbA.GetCenterOfMassTransform() * pivotInA;
}
public override BulletBody CreateBodyWithDefaultMotionState( BulletShape pShape, uint pLocalID, Vector3 pRawPosition, Quaternion pRawOrientation)
{
IndexedMatrix mat =
IndexedMatrix.CreateFromQuaternion(new IndexedQuaternion(pRawOrientation.X, pRawOrientation.Y,
pRawOrientation.Z, pRawOrientation.W));
mat._origin = new IndexedVector3(pRawPosition.X, pRawPosition.Y, pRawPosition.Z);
CollisionShape shape = (pShape as BulletShapeXNA).shape;
// TODO: Feed Update array into null
RigidBody body = new RigidBody(0, new DefaultMotionState( mat, IndexedMatrix.Identity), shape, IndexedVector3.Zero);
body.SetWorldTransform(mat);
body.SetUserPointer(pLocalID);
return new BulletBodyXNA(pLocalID, body);
}
public Point2PointConstraint(RigidBody rbA, RigidBody rbB, ref IndexedVector3 pivotInA, ref IndexedVector3 pivotInB)
: base(TypedConstraintType.POINT2POINT_CONSTRAINT_TYPE, rbA, rbB)
{
m_pivotInA = pivotInA;
m_pivotInB = pivotInB;
}
protected void ResolveSingleConstraintRowGeneric(RigidBody body1, RigidBody body2, ref SolverConstraint c)
{
m_genericCount++;
float deltaImpulse = c.m_rhs - c.m_appliedImpulse * c.m_cfm;
float deltaVel1Dotn = (c.m_contactNormal.X * body1.m_deltaLinearVelocity.X) + (c.m_contactNormal.Y * body1.m_deltaLinearVelocity.Y) + (c.m_contactNormal.Z * body1.m_deltaLinearVelocity.Z) +
(c.m_relpos1CrossNormal.X * body1.m_deltaAngularVelocity.X) + (c.m_relpos1CrossNormal.Y * body1.m_deltaAngularVelocity.Y) + (c.m_relpos1CrossNormal.Z * body1.m_deltaAngularVelocity.Z);
float deltaVel2Dotn = -((c.m_contactNormal.X * body2.m_deltaLinearVelocity.X) + (c.m_contactNormal.Y * body2.m_deltaLinearVelocity.Y) + (c.m_contactNormal.Z * body2.m_deltaLinearVelocity.Z)) +
(c.m_relpos2CrossNormal.X * body2.m_deltaAngularVelocity.X) + (c.m_relpos2CrossNormal.Y * body2.m_deltaAngularVelocity.Y) + (c.m_relpos2CrossNormal.Z * body2.m_deltaAngularVelocity.Z);
float originalDeltaImpulse = deltaImpulse;
deltaImpulse -= deltaVel1Dotn * c.m_jacDiagABInv;
deltaImpulse -= deltaVel2Dotn * c.m_jacDiagABInv;
#if DEBUG
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugSolver && false)
{
BulletGlobals.g_streamWriter.WriteLine("ResolveSingleConstraintRowGeneric start [{0}][{1}][{2}][{3}].", originalDeltaImpulse, deltaVel1Dotn, deltaVel2Dotn, c.m_jacDiagABInv);
}
#endif
float sum = c.m_appliedImpulse + deltaImpulse;
if (sum < c.m_lowerLimit)
{
deltaImpulse = c.m_lowerLimit - c.m_appliedImpulse;
c.m_appliedImpulse = c.m_lowerLimit;
}
else if (sum > c.m_upperLimit)
{
deltaImpulse = c.m_upperLimit - c.m_appliedImpulse;
c.m_appliedImpulse = c.m_upperLimit;
}
else
{
c.m_appliedImpulse = sum;
}
IndexedVector3 temp = new IndexedVector3(c.m_contactNormal.X * body1.m_invMass.X, c.m_contactNormal.Y * body1.m_invMass.Y, c.m_contactNormal.Z * body1.m_invMass.Z);
body1.InternalApplyImpulse(ref temp, ref c.m_angularComponentA, deltaImpulse, "ResolveSingleConstraintGeneric-body1");
temp = new IndexedVector3(-c.m_contactNormal.X * body2.m_invMass.X, -c.m_contactNormal.Y * body2.m_invMass.Y, -c.m_contactNormal.Z * body2.m_invMass.Z);
body2.InternalApplyImpulse(ref temp, ref c.m_angularComponentB, deltaImpulse, "ResolveSingleConstraintGeneric-body2");
}
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;
}
protected void ResolveSingleConstraintRowLowerLimit(RigidBody body1, RigidBody body2, ref SolverConstraint c)
{
m_lowerLimitCount++;
//check magniture of applied impulse from SolverConstraint
float deltaImpulse = c.m_rhs - c.m_appliedImpulse * c.m_cfm;
if (deltaImpulse > 200)
{
int ibreak = 0;
}
float deltaVel1Dotn = (c.m_contactNormal.X * body1.m_deltaLinearVelocity.X) + (c.m_contactNormal.Y * body1.m_deltaLinearVelocity.Y) + (c.m_contactNormal.Z * body1.m_deltaLinearVelocity.Z) +
(c.m_relpos1CrossNormal.X * body1.m_deltaAngularVelocity.X) + (c.m_relpos1CrossNormal.Y * body1.m_deltaAngularVelocity.Y) + (c.m_relpos1CrossNormal.Z * body1.m_deltaAngularVelocity.Z);
float deltaVel2Dotn = -((c.m_contactNormal.X * body2.m_deltaLinearVelocity.X) + (c.m_contactNormal.Y * body2.m_deltaLinearVelocity.Y) + (c.m_contactNormal.Z * body2.m_deltaLinearVelocity.Z)) +
(c.m_relpos2CrossNormal.X * body2.m_deltaAngularVelocity.X) + (c.m_relpos2CrossNormal.Y * body2.m_deltaAngularVelocity.Y) + (c.m_relpos2CrossNormal.Z * body2.m_deltaAngularVelocity.Z);
deltaImpulse -= deltaVel1Dotn * c.m_jacDiagABInv;
deltaImpulse -= deltaVel2Dotn * c.m_jacDiagABInv;
float sum = c.m_appliedImpulse + deltaImpulse;
if (sum < c.m_lowerLimit)
{
deltaImpulse = c.m_lowerLimit - c.m_appliedImpulse;
c.m_appliedImpulse = c.m_lowerLimit;
}
else
{
c.m_appliedImpulse = sum;
}
IndexedVector3 temp = new IndexedVector3(c.m_contactNormal.X * body1.m_invMass.X,c.m_contactNormal.Y * body1.m_invMass.Y,c.m_contactNormal.Z * body1.m_invMass.Z);
body1.InternalApplyImpulse(ref temp, ref c.m_angularComponentA, deltaImpulse, "ResolveSingleConstraintRowLowerLimit-body1");
temp = new IndexedVector3(-c.m_contactNormal.X * body2.m_invMass.X, -c.m_contactNormal.Y * body2.m_invMass.Y, -c.m_contactNormal.Z * body2.m_invMass.Z);
body2.InternalApplyImpulse(ref temp, ref c.m_angularComponentB, deltaImpulse, "ResolveSingleConstraintRowLowerLimit-body2");
}
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;
}
protected void ResolveSplitPenetrationImpulseCacheFriendly(
RigidBody body1,
RigidBody body2,
ref SolverConstraint c)
{
if (c.m_rhsPenetration != 0f)
{
gNumSplitImpulseRecoveries++;
float deltaImpulse = c.m_rhsPenetration - (c.m_appliedPushImpulse * c.m_cfm);
float deltaVel1Dotn = IndexedVector3.Dot(c.m_contactNormal, body1.InternalGetPushVelocity()) + IndexedVector3.Dot(c.m_relpos1CrossNormal, body1.InternalGetTurnVelocity());
float deltaVel2Dotn = -IndexedVector3.Dot(c.m_contactNormal, body2.InternalGetPushVelocity()) + IndexedVector3.Dot(c.m_relpos2CrossNormal, body2.InternalGetTurnVelocity());
deltaImpulse -= deltaVel1Dotn * c.m_jacDiagABInv;
deltaImpulse -= deltaVel2Dotn * c.m_jacDiagABInv;
float sum = c.m_appliedPushImpulse + deltaImpulse;
if (sum < c.m_lowerLimit)
{
deltaImpulse = c.m_lowerLimit - c.m_appliedPushImpulse;
c.m_appliedPushImpulse = c.m_lowerLimit;
}
else
{
c.m_appliedPushImpulse = sum;
}
body1.InternalApplyPushImpulse(c.m_contactNormal * body1.InternalGetInvMass(), c.m_angularComponentA, deltaImpulse);
body2.InternalApplyPushImpulse(-c.m_contactNormal * body2.InternalGetInvMass(), c.m_angularComponentB, deltaImpulse);
}
}
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 static RigidBody GetFixedBody()
{
if (s_fixed == null)
{
s_fixed = new RigidBody(0f, null, null, IndexedVector3.Zero);
s_fixed.SetUserPointer("SICS:Fixed");
}
s_fixed.SetMassProps(0f, IndexedVector3.Zero);
return s_fixed;
}
//------------------------
// constructors
public SliderConstraint(RigidBody rbA, RigidBody rbB, ref IndexedMatrix frameInA, ref IndexedMatrix frameInB, bool useLinearReferenceFrameA)
: base(TypedConstraintType.SLIDER_CONSTRAINT_TYPE, rbA, rbB)
{
m_frameInA = frameInA;
m_frameInB = frameInB;
m_useLinearReferenceFrameA = useLinearReferenceFrameA;
InitParams();
}
public void SetupFrictionConstraint(ref SolverConstraint solverConstraint, ref IndexedVector3 normalAxis, RigidBody solverBodyA, RigidBody solverBodyB,
ManifoldPoint cp, ref IndexedVector3 rel_pos1, ref IndexedVector3 rel_pos2,
CollisionObject colObj0, CollisionObject colObj1, float relaxation)
{
SetupFrictionConstraint(ref solverConstraint, ref normalAxis, solverBodyA, solverBodyB, cp, ref rel_pos1, ref rel_pos2, colObj0, colObj1, relaxation, 0f, 0f);
}
public void UpdateWheel(RigidBody chassis, ref WheelRaycastInfo raycastInfo)
{
if (m_raycastInfo.m_isInContact)
{
float project= IndexedVector3.Dot(m_raycastInfo.m_contactNormalWS,m_raycastInfo.m_wheelDirectionWS );
IndexedVector3 chassis_velocity_at_contactPoint;
IndexedVector3 relpos = m_raycastInfo.m_contactPointWS - chassis.GetCenterOfMassPosition();
chassis_velocity_at_contactPoint = chassis.GetVelocityInLocalPoint( ref relpos );
float projVel = IndexedVector3.Dot(m_raycastInfo.m_contactNormalWS,chassis_velocity_at_contactPoint );
if ( project >= -0.1f)
{
m_suspensionRelativeVelocity = 0f;
m_clippedInvContactDotSuspension = 1.0f / 0.1f;
}
else
{
float inv = -1f / project;
m_suspensionRelativeVelocity = projVel * inv;
m_clippedInvContactDotSuspension = inv;
}
}
else // Not in contact : position wheel in a nice (rest length) position
{
m_raycastInfo.m_suspensionLength = this.GetSuspensionRestLength();
m_suspensionRelativeVelocity = 0f;
m_raycastInfo.m_contactNormalWS = -m_raycastInfo.m_wheelDirectionWS;
m_clippedInvContactDotSuspension = 1f;
}
}
public void SetupFrictionConstraint(ref SolverConstraint solverConstraint, ref IndexedVector3 normalAxis, RigidBody solverBodyA, RigidBody solverBodyB,
ManifoldPoint cp, ref IndexedVector3 rel_pos1, ref IndexedVector3 rel_pos2,
CollisionObject colObj0, CollisionObject colObj1, float relaxation,
float desiredVelocity, float cfmSlip)
{
RigidBody body0 = RigidBody.Upcast(colObj0);
RigidBody body1 = RigidBody.Upcast(colObj1);
solverConstraint.m_contactNormal = normalAxis;
solverConstraint.m_solverBodyA = body0 != null ? body0 : GetFixedBody();
solverConstraint.m_solverBodyB = body1 != null ? body1 : GetFixedBody();
solverConstraint.m_friction = cp.GetCombinedFriction();
#if DEBUG
if (BulletGlobals.g_streamWriter != null && (body0 != null || body1 != null) && BulletGlobals.debugSolver)
{
BulletGlobals.g_streamWriter.WriteLine("SetupFrictionConstraint[{0}][{1}]", (String)solverConstraint.m_solverBodyA.GetUserPointer(), (String)solverConstraint.m_solverBodyB.GetUserPointer());
MathUtil.PrintContactPoint(BulletGlobals.g_streamWriter, cp);
}
#endif
solverConstraint.m_originalContactPoint = null;
//solverConstraint.m_originalContactPointConstraint = null;
solverConstraint.m_appliedImpulse = 0f;
solverConstraint.m_appliedPushImpulse = 0f;
{
IndexedVector3 ftorqueAxis1 = IndexedVector3.Cross(rel_pos1, solverConstraint.m_contactNormal);
solverConstraint.m_relpos1CrossNormal = ftorqueAxis1;
solverConstraint.m_angularComponentA = body0 != null ? body0.GetInvInertiaTensorWorld() * ftorqueAxis1 * body0.GetAngularFactor() : IndexedVector3.Zero;
}
{
IndexedVector3 ftorqueAxis1 = IndexedVector3.Cross(rel_pos2, -solverConstraint.m_contactNormal);
solverConstraint.m_relpos2CrossNormal = ftorqueAxis1;
solverConstraint.m_angularComponentB = body1 != null ? body1.GetInvInertiaTensorWorld() * ftorqueAxis1 * body1.GetAngularFactor() : IndexedVector3.Zero;
}
#if COMPUTE_IMPULSE_DENOM
float denom0 = rb0.computeImpulseDenominator(pos1,solverConstraint.m_contactNormal);
float denom1 = rb1.computeImpulseDenominator(pos2,solverConstraint.m_contactNormal);
#else
IndexedVector3 vec;
float denom0 = 0f;
float denom1 = 0f;
if (body0 != null)
{
vec = IndexedVector3.Cross(solverConstraint.m_angularComponentA, rel_pos1);
denom0 = body0.GetInvMass() + IndexedVector3.Dot(normalAxis, vec);
}
if (body1 != null)
{
vec = IndexedVector3.Cross(-solverConstraint.m_angularComponentB, rel_pos2);
denom1 = body1.GetInvMass() + IndexedVector3.Dot(normalAxis, vec);
}
#endif //COMPUTE_IMPULSE_DENOM
float denom = relaxation / (denom0 + denom1);
solverConstraint.m_jacDiagABInv = denom;
MathUtil.SanityCheckFloat(solverConstraint.m_jacDiagABInv);
#if _USE_JACOBIAN
solverConstraint.m_jac = new JacobianEntry (
ref rel_pos1,ref rel_pos2,ref solverConstraint.m_contactNormal,
body0.getInvInertiaDiagLocal(),
body0.getInvMass(),
body1.getInvInertiaDiagLocal(),
body1.getInvMass());
#endif //_USE_JACOBIAN
{
float rel_vel;
float vel1Dotn = IndexedVector3.Dot(solverConstraint.m_contactNormal, body0 != null ? body0.GetLinearVelocity() : IndexedVector3.Zero)
+ IndexedVector3.Dot(solverConstraint.m_relpos1CrossNormal, body0 != null ? body0.GetAngularVelocity() : IndexedVector3.Zero);
float vel2Dotn = -IndexedVector3.Dot(solverConstraint.m_contactNormal, body1 != null ? body1.GetLinearVelocity() : IndexedVector3.Zero)
+ IndexedVector3.Dot(solverConstraint.m_relpos2CrossNormal, body1 != null ? body1.GetAngularVelocity() : IndexedVector3.Zero);
rel_vel = vel1Dotn + vel2Dotn;
//float positionalError = 0f;
float velocityError = desiredVelocity - rel_vel;
float damper = 1f;
float velocityImpulse = (velocityError * solverConstraint.m_jacDiagABInv) * damper;
solverConstraint.m_rhs = velocityImpulse;
solverConstraint.m_cfm = cfmSlip;
solverConstraint.m_lowerLimit = 0;
solverConstraint.m_upperLimit = 1e10f;
}
}
public static RigidBody GetFixedBody()
{
//static btRigidBody s_fixed(0, 0,0);
//s_fixed.setMassProps(float(0.),btVector3(float(0.),float(0.),float(0.)));
if (s_fixed == null)
{
s_fixed = new RigidBody(0f, null, null, IndexedVector3.Zero);
}
IndexedVector3 inertia = IndexedVector3.Zero;
s_fixed.SetMassProps(0f, ref inertia);
return s_fixed;
}
public override void AddRigidBody(RigidBody body)
{
body.SetGravity(ref m_gravity);
if (body.GetCollisionShape() != null)
{
AddCollisionObject(body);
}
}
//(sim.ptr, shape.ptr, prim.LocalID, prim.RawPosition, prim.RawOrientation);
public override BulletBody CreateBodyFromShape(BulletWorld pWorld, BulletShape pShape, uint pLocalID, Vector3 pRawPosition, Quaternion pRawOrientation)
{
CollisionWorld world = (pWorld as BulletWorldXNA).world;
IndexedMatrix mat =
IndexedMatrix.CreateFromQuaternion(new IndexedQuaternion(pRawOrientation.X, pRawOrientation.Y,
pRawOrientation.Z, pRawOrientation.W));
mat._origin = new IndexedVector3(pRawPosition.X, pRawPosition.Y, pRawPosition.Z);
CollisionShape shape = (pShape as BulletShapeXNA).shape;
//UpdateSingleAabb(world, shape);
// TODO: Feed Update array into null
SimMotionState motionState = new SimMotionState(this, pLocalID, mat, null);
RigidBody body = new RigidBody(0,motionState,shape,IndexedVector3.Zero);
RigidBodyConstructionInfo constructionInfo = new RigidBodyConstructionInfo(0, motionState, shape, IndexedVector3.Zero)
{
m_mass = 0
};
/*
m_mass = mass;
m_motionState =motionState;
m_collisionShape = collisionShape;
m_localInertia = localInertia;
m_linearDamping = 0f;
m_angularDamping = 0f;
m_friction = 0.5f;
m_restitution = 0f;
m_linearSleepingThreshold = 0.8f;
m_angularSleepingThreshold = 1f;
m_additionalDamping = false;
m_additionalDampingFactor = 0.005f;
m_additionalLinearDampingThresholdSqr = 0.01f;
m_additionalAngularDampingThresholdSqr = 0.01f;
m_additionalAngularDampingFactor = 0.01f;
m_startWorldTransform = IndexedMatrix.Identity;
*/
body.SetUserPointer(pLocalID);
return new BulletBodyXNA(pLocalID, body);
}
public override void AddRigidBody(RigidBody body, CollisionFilterGroups group, CollisionFilterGroups mask)
{
body.SetGravity(ref m_gravity);
if (body.GetCollisionShape() != null)
{
AddCollisionObject(body, group, mask);
}
}
public override void SetRigidBody(RigidBody body)
{
Rigidbody = body;
}
public override void RemoveRigidBody(RigidBody body)
{
base.RemoveCollisionObject(body);
}