// 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));
}
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 Generic6DofSpringConstraint(RigidBody rbA, RigidBody rbB, ref Matrix frameInA, ref Matrix frameInB ,bool useLinearReferenceFrameA) : base(rbA,rbB,ref frameInA,ref frameInB,useLinearReferenceFrameA)
{
for (int i = 0; i < s_degreesOfFreedom; ++i)
{
m_springEnabled[i] = false;
m_equilibriumPoint[i] = 0.0f;
m_springStiffness[i] = 0.0f;
m_springDamping[i] = 1.0f;
}
}
public WheelContactPoint(RigidBody body0,RigidBody body1,ref Vector3 frictionPosWorld,ref Vector3 frictionDirectionWorld, float maxImpulse)
{
m_body0 = body0;
m_body1 = body1;
m_frictionPositionWorld = frictionPosWorld;
m_frictionDirectionWorld = frictionDirectionWorld;
m_maxImpulse = maxImpulse;
float denom0 = body0.ComputeImpulseDenominator(ref frictionPosWorld,ref frictionDirectionWorld);
float denom1 = body1.ComputeImpulseDenominator(ref frictionPosWorld,ref frictionDirectionWorld);
float relaxation = 1f;
m_jacDiagABInv = relaxation/(denom0+denom1);
}
///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;
}
}
public TypedConstraint(TypedConstraintType type, RigidBody rbA) : base((int)type)
{
m_userConstraintType = -1;
m_userConstraintId = -1;
m_constraintType = type;
m_rbA = rbA;
m_rbB = GetFixedBody();
m_appliedImpulse = 0f;
m_dbgDrawSize = DEFAULT_DEBUGDRAW_SIZE;
{
s_fixed.SetMassProps(0f,Vector3.Zero);
}
}
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 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;
}
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 float SolveLinearAxis(
float timeStep,
float jacDiagABInv,
RigidBody body1, ref Vector3 pointInA,
RigidBody body2, ref Vector3 pointInB,
int limit_index,
ref Vector3 axis_normal_on_a,
ref Vector3 anchorPos)
{
///find relative velocity
// Vector3 rel_pos1 = pointInA - body1.getCenterOfMassPosition();
// Vector3 rel_pos2 = pointInB - body2.getCenterOfMassPosition();
Vector3 rel_pos1 = anchorPos - body1.GetCenterOfMassPosition();
Vector3 rel_pos2 = anchorPos - body2.GetCenterOfMassPosition();
Vector3 vel1 = Vector3.Zero;
body1.InternalGetVelocityInLocalPointObsolete(ref rel_pos1,ref vel1);
Vector3 vel2 = Vector3.Zero;;
body2.InternalGetVelocityInLocalPointObsolete(ref rel_pos2,ref vel2);
Vector3 vel = vel1 - vel2;
float rel_vel = Vector3.Dot(axis_normal_on_a,vel);
/// apply displacement correction
//positional error (zeroth order error)
float depth = -Vector3.Dot((pointInA - pointInB),axis_normal_on_a);
float lo = float.MinValue;
float hi = float.MaxValue;
float minLimit = MathUtil.VectorComponent(ref m_lowerLimit,limit_index);
float maxLimit = MathUtil.VectorComponent(ref m_upperLimit,limit_index);
//handle the limits
if (minLimit < maxLimit)
{
{
if (depth > maxLimit)
{
depth -= maxLimit;
lo = 0f;
}
else
{
if (depth < minLimit)
{
depth -= minLimit;
hi = 0f;
}
else
{
return 0.0f;
}
}
}
}
float normalImpulse= m_limitSoftness*(m_restitution*depth/timeStep - m_damping*rel_vel) * jacDiagABInv;
float oldNormalImpulse = MathUtil.VectorComponent(ref m_accumulatedImpulse,limit_index);
float sum = oldNormalImpulse + normalImpulse;
MathUtil.VectorComponent(ref m_accumulatedImpulse,limit_index,(sum > hi ? 0f: sum < lo ? 0f : sum));
normalImpulse = MathUtil.VectorComponent(ref m_accumulatedImpulse,limit_index) - oldNormalImpulse;
Vector3 impulse_vector = axis_normal_on_a * normalImpulse;
//body1.applyImpulse( impulse_vector, rel_pos1);
//body2.applyImpulse(-impulse_vector, rel_pos2);
Vector3 ftorqueAxis1 = Vector3.Cross(rel_pos1,axis_normal_on_a);
Vector3 ftorqueAxis2 = Vector3.Cross(rel_pos2,axis_normal_on_a);
body1.InternalApplyImpulse(axis_normal_on_a*body1.GetInvMass(), Vector3.TransformNormal(ftorqueAxis1,body1.GetInvInertiaTensorWorld()),normalImpulse);
body2.InternalApplyImpulse(axis_normal_on_a * body2.GetInvMass(), Vector3.TransformNormal(ftorqueAxis2,body2.GetInvInertiaTensorWorld()), -normalImpulse);
return normalImpulse;
}
public HingeConstraint(RigidBody rbA, RigidBody rbB, ref Vector3 pivotInA, ref Vector3 pivotInB, ref Vector3 axisInA, ref Vector3 axisInB)
: this(rbA, rbB, ref pivotInA,ref pivotInB, ref axisInA, ref axisInB,false)
{
}
private static void Main(string[] args)
{
var collisionConfig = new DefaultCollisionConfiguration();
var dispatcher = new CollisionDispatcher(collisionConfig);
var pairCache = new DbvtBroadphase();
var solver = new SequentialImpulseConstraintSolver();
var dynamicsWorld = new DiscreteDynamicsWorld(dispatcher, pairCache, solver, collisionConfig);
dynamicsWorld.Gravity = new Vector3(0, -10, 0);
var groundShape = new BoxShape(new Vector3(50, 50, 50));
var groundTransform = Matrix.CreateTranslation(0, -56, 0);
{
float mass = 0;
bool isDynamic = mass != 0;
Vector3 localInertia = Vector3.Zero;
if (isDynamic)
localInertia = groundShape.CalculateLocalInertia(mass);
var motionState = new DefaultMotionState(groundTransform);
var rbinfo = new RigidBodyConstructionInfo(mass, motionState, groundShape, localInertia);
var body = new RigidBody(rbinfo);
dynamicsWorld.AddRigidBody(body);
}
{
var collisionShape = new SphereShape(1);
var transform = Matrix.Identity;
float mass = 1;
bool isDynamic = mass != 0;
Vector3 localInertia = Vector3.Zero;
if (isDynamic)
localInertia = collisionShape.CalculateLocalInertia(mass);
transform.Translation = new Vector3(2, 10, 0);
var motionState = new DefaultMotionState(transform);
var rbinfo = new RigidBodyConstructionInfo(mass, motionState, collisionShape, localInertia);
var body = new RigidBody(rbinfo);
dynamicsWorld.AddRigidBody(body);
}
Console.WriteLine("Starting simulation");
for (int i = 0; i < 100; i++)
{
dynamicsWorld.StepSimulation(1f / 60f, 10);
for (int j = dynamicsWorld.GetNumCollisionObjects() - 1; j >= 0; j--)
{
var obj = dynamicsWorld.GetCollisionObjectArray()[j];
var body = (RigidBody)obj;
if (body != null && body.GetMotionState() != null)
{
Matrix transform = Matrix.Identity;
body.GetMotionState().GetWorldTransform(ref transform);
Console.WriteLine("Object@{0} Position={1}", body.GetHashCode(), transform.Translation);
}
}
}
Console.Read();
}
public ContactConstraint(PersistentManifold contactManifold, RigidBody rbA, RigidBody rbB) : base(TypedConstraintType.CONTACT_CONSTRAINT_TYPE,rbA,rbB)
{
m_contactManifold = contactManifold;
}
public Point2PointConstraint(RigidBody rbA, RigidBody rbB, ref Vector3 pivotInA, ref Vector3 pivotInB)
: base(TypedConstraintType.POINT2POINT_CONSTRAINT_TYPE,rbA,rbB)
{
m_pivotInA = pivotInA;
m_pivotInB = pivotInB;
}
//! apply the correction impulses for two bodies
public float SolveAngularLimits(float timeStep,ref Vector3 axis, float jacDiagABInv,RigidBody body0, RigidBody body1)
{
if (NeedApplyTorques() == false)
{
return 0.0f;
}
float target_velocity = m_targetVelocity;
float maxMotorForce = m_maxMotorForce;
//current error correction
if (m_currentLimit!=0)
{
target_velocity = -m_stopERP*m_currentLimitError/(timeStep);
maxMotorForce = m_maxLimitForce;
}
maxMotorForce *= timeStep;
// current velocity difference
Vector3 angVelA = Vector3.Zero;
body0.InternalGetAngularVelocity(ref angVelA);
Vector3 angVelB = Vector3.Zero;
body1.InternalGetAngularVelocity(ref angVelB);
Vector3 vel_diff = angVelA-angVelB;
float rel_vel = Vector3.Dot(axis,vel_diff);
// correction velocity
float motor_relvel = m_limitSoftness*(target_velocity - m_damping*rel_vel);
if ( motor_relvel < MathUtil.SIMD_EPSILON && motor_relvel > -MathUtil.SIMD_EPSILON )
{
return 0.0f;//no need for applying force
}
// correction impulse
float unclippedMotorImpulse = (1+m_bounce)*motor_relvel*jacDiagABInv;
// clip correction impulse
float clippedMotorImpulse;
///@todo: should clip against accumulated impulse
if (unclippedMotorImpulse>0.0f)
{
clippedMotorImpulse = unclippedMotorImpulse > maxMotorForce? maxMotorForce: unclippedMotorImpulse;
}
else
{
clippedMotorImpulse = unclippedMotorImpulse < -maxMotorForce ? -maxMotorForce: unclippedMotorImpulse;
}
// sort with accumulated impulses
float lo = float.MinValue;
float hi = float.MaxValue;
float oldaccumImpulse = m_accumulatedImpulse;
float sum = oldaccumImpulse + clippedMotorImpulse;
m_accumulatedImpulse = sum > hi ? 0f : sum < lo ? 0f : sum;
clippedMotorImpulse = m_accumulatedImpulse - oldaccumImpulse;
Vector3 motorImp = clippedMotorImpulse * axis;
//body0.applyTorqueImpulse(motorImp);
//body1.applyTorqueImpulse(-motorImp);
body0.InternalApplyImpulse(Vector3.Zero, Vector3.TransformNormal(axis,body0.GetInvInertiaTensorWorld()),clippedMotorImpulse);
body1.InternalApplyImpulse(Vector3.Zero, Vector3.TransformNormal(axis,body1.GetInvInertiaTensorWorld()),-clippedMotorImpulse);
return clippedMotorImpulse;
}
public abstract void RemoveRigidBody(RigidBody body);
//public abstract void addRigidBody(RigidBody body,short mask1,short mask2); public abstract void AddRigidBody(RigidBody body);
public override void RemoveRigidBody(RigidBody body)
{
m_nonStaticRigidBodies.Remove(body);
base.RemoveCollisionObject(body);
}
public virtual void AddRigidBody(RigidBody body, CollisionFilterGroups group, CollisionFilterGroups mask)
{
if (!body.IsStaticOrKinematicObject() && 0 == (body.GetFlags() & RigidBodyFlags.BT_DISABLE_WORLD_GRAVITY))
body.Gravity = m_gravity;
if (body.GetCollisionShape() != null)
{
if (!body.IsStaticObject())
{
m_nonStaticRigidBodies.Add(body);
}
else
{
body.SetActivationState(ActivationState.ISLAND_SLEEPING);
}
AddCollisionObject(body, group, mask);
}
}
public override void AddRigidBody(RigidBody body)
{
if (!body.IsStaticOrKinematicObject() && 0 == (body.GetFlags() & RigidBodyFlags.BT_DISABLE_WORLD_GRAVITY))
body.Gravity = m_gravity;
if (body.GetCollisionShape() != null)
{
if (!body.IsStaticObject())
{
m_nonStaticRigidBodies.Add(body);
}
else
{
body.SetActivationState(ActivationState.ISLAND_SLEEPING);
}
bool isDynamic = !(body.IsStaticObject() || body.IsKinematicObject());
CollisionFilterGroups collisionFilterGroup = isDynamic ? CollisionFilterGroups.DefaultFilter : CollisionFilterGroups.StaticFilter;
CollisionFilterGroups collisionFilterMask = isDynamic ? CollisionFilterGroups.AllFilter : (CollisionFilterGroups.AllFilter ^ CollisionFilterGroups.StaticFilter);
AddCollisionObject(body, collisionFilterGroup, collisionFilterMask);
}
}
///this can be useful to synchronize a single rigid body . graphics object
public void SynchronizeSingleMotionState(RigidBody body)
{
Debug.Assert(body != null);
if (body.GetMotionState() != null && !body.IsStaticOrKinematicObject())
{
//we need to call the update at least once, even for sleeping objects
//otherwise the 'graphics' transform never updates properly
///@todo: add 'dirty' flag
//if (body.getActivationState() != ISLAND_SLEEPING)
{
Matrix interpolatedTransform = Matrix.Identity;
TransformUtil.IntegrateTransform(body.GetInterpolationWorldTransform(),
body.SetInterpolationLinearVelocity(), body.GetInterpolationAngularVelocity(),
m_localTime * body.GetHitFraction(), ref interpolatedTransform);
body.GetMotionState().SetWorldTransform(ref interpolatedTransform);
}
}
}
public void UpdateWheel(RigidBody chassis, ref WheelRaycastInfo raycastInfo)
{
if (m_raycastInfo.m_isInContact)
{
float project= Vector3.Dot(m_raycastInfo.m_contactNormalWS,m_raycastInfo.m_wheelDirectionWS );
Vector3 chassis_velocity_at_contactPoint;
Vector3 relpos = m_raycastInfo.m_contactPointWS - chassis.GetCenterOfMassPosition();
chassis_velocity_at_contactPoint = chassis.GetVelocityInLocalPoint( ref relpos );
float projVel = Vector3.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 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 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, ref Matrix rbAFrame)
: this(rbA, ref rbAFrame, false)
{
}
public override void RemoveRigidBody(RigidBody body)
{
base.RemoveCollisionObject(body);
}
public HingeConstraint(RigidBody rbA, ref Matrix 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.Translation = Vector3.Transform(m_rbAFrame.Translation,m_rbA.GetCenterOfMassTransform());
//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 ConeTwistConstraint(RigidBody rbA, ref Matrix rbAFrame)
: base(TypedConstraintType.CONETWIST_CONSTRAINT_TYPE, rbA)
{
m_rbAFrame = rbAFrame;
m_rbBFrame = rbAFrame;
m_angularOnly = false;
Init();
}
public Generic6DofSpringConstraint(RigidBody rbA, RigidBody rbB, Matrix frameInA, Matrix frameInB, bool useLinearReferenceFrameA)
: this(rbA, rbB, ref frameInA, ref frameInB, useLinearReferenceFrameA)
{
}