public Point2PointConstraint(RigidBody rbA, Vector3 pivotInA)
: base(rbA)
{
_jacobian = new JacobianEntry[3];
_pivotInA = pivotInA;
_pivotInB = MathHelper.MatrixToVector(rbA.CenterOfMassTransform, _pivotInA);
}
public Point2PointConstraint(RigidBody rbA, RigidBody rbB, Vector3 pivotInA, Vector3 pivotInB)
: base(rbA, rbB)
{
_jacobian = new JacobianEntry[3];
_pivotInA = pivotInA;
_pivotInB = pivotInB;
}
public HingeConstraint(RigidBody rbA, RigidBody rbB, Vector3 pivotInA, Vector3 pivotInB, Vector3 axisInA, Vector3 axisInB)
: base(rbA, rbB)
{
_pivotInA = pivotInA;
_pivotInB = pivotInB;
_axisInA = axisInA;
_axisInB = -axisInB;
_angularOnly = false;
}
public HingeConstraint(RigidBody rbA, Vector3 pivotInA, Vector3 axisInA)
: base(rbA)
{
_pivotInA = pivotInA;
_pivotInB = MathHelper.MatrixToVector(rbA.CenterOfMassTransform, pivotInA);
_axisInA = axisInA;
//fixed axis in worldspace
_axisInB = MathHelper.TransformNormal(-axisInA, rbA.CenterOfMassTransform);
_angularOnly = false;
}
public TypedConstraint(RigidBody rbA, RigidBody rbB)
{
_userConstraintType = -1;
_userConstraintId = -1;
_rbA = rbA;
_rbB = rbB;
_appliedImpulse = 0;
_fixed.SetMassProps(0, new Vector3());
}
// solve unilateral constraint (equality, direct method)
public void ResolveUnilateralPairConstraint(
RigidBody body1, RigidBody body2,
Matrix world2A,
Matrix world2B,
Vector3 invInertiaADiag,
float invMassA,
Vector3 linvelA, Vector3 angvelA,
Vector3 rel_posA1,
Vector3 invInertiaBDiag,
float invMassB,
Vector3 linvelB, Vector3 angvelB,
Vector3 rel_posA2,
float depthA, Vector3 normalA,
Vector3 rel_posB1, Vector3 rel_posB2,
float depthB, Vector3 normalB,
out float imp0, out float imp1)
{
imp0 = 0;
imp1 = 0;
float len = Math.Abs(normalA.Length()) - 1f;
if (Math.Abs(len) >= float.Epsilon)
return;
BulletDebug.Assert(len < float.Epsilon);
//this jacobian entry could be re-used for all iterations
JacobianEntry jacA = new JacobianEntry(world2A, world2B, rel_posA1, rel_posA2, normalA, invInertiaADiag, invMassA,
invInertiaBDiag, invMassB);
JacobianEntry jacB = new JacobianEntry(world2A, world2B, rel_posB1, rel_posB2, normalB, invInertiaADiag, invMassA,
invInertiaBDiag, invMassB);
float vel0 = Vector3.Dot(normalA, body1.GetVelocityInLocalPoint(rel_posA1) - body2.GetVelocityInLocalPoint(rel_posA1));
float vel1 = Vector3.Dot(normalB, body1.GetVelocityInLocalPoint(rel_posB1) - body2.GetVelocityInLocalPoint(rel_posB1));
// btScalar penetrationImpulse = (depth*contactTau*timeCorrection) * massTerm;//jacDiagABInv
float massTerm = 1f / (invMassA + invMassB);
// calculate rhs (or error) terms
float dv0 = depthA * _tau * massTerm - vel0 * _damping;
float dv1 = depthB * _tau * massTerm - vel1 * _damping;
float nonDiag = jacA.GetNonDiagonal(jacB, invMassA, invMassB);
float invDet = 1.0f / (jacA.Diagonal * jacB.Diagonal - nonDiag * nonDiag);
imp0 = dv0 * jacA.Diagonal * invDet + dv1 * -nonDiag * invDet;
imp1 = dv1 * jacB.Diagonal * invDet + dv0 * -nonDiag * invDet;
}
public Generic6DofConstraint(RigidBody rbA, RigidBody rbB, Matrix frameInA, Matrix frameInB)
: base(rbA, rbB)
{
_frameInA = frameInA;
_frameInB = frameInB;
//free means upper < lower,
//locked means upper == lower
//limited means upper > lower
//so start all locked
for (int i = 0; i < 6; ++i)
{
_lowerLimit[i] = 0.0f;
_upperLimit[i] = 0.0f;
_accumulatedImpulse[i] = 0.0f;
}
}
public static float ResolveSingleFrictionOriginal(
RigidBody bodyA,
RigidBody bodyB,
ManifoldPoint contactPoint,
ContactSolverInfo solverInfo)
{
Vector3 posA = contactPoint.PositionWorldOnA;
Vector3 posB = contactPoint.PositionWorldOnB;
Vector3 relPosA = posA - bodyA.CenterOfMassPosition;
Vector3 relPosB = posB - bodyB.CenterOfMassPosition;
ConstraintPersistentData cpd = contactPoint.UserPersistentData as ConstraintPersistentData;
if (cpd == null)
throw new BulletException();
float combinedFriction = cpd.Friction;
float limit = cpd.AppliedImpulse * combinedFriction;
//if (contactPoint.m_appliedImpulse>0.f)
//friction
{
//apply friction in the 2 tangential directions
{
// 1st tangent
Vector3 velA = bodyA.GetVelocityInLocalPoint(relPosA);
Vector3 velB = bodyB.GetVelocityInLocalPoint(relPosB);
Vector3 vel = velA - velB;
float vrel = Vector3.Dot(cpd.FrictionWorldTangentialA, vel);
// calculate j that moves us to zero relative velocity
float j = -vrel * cpd.JacDiagABInvTangentA;
float total = cpd.AccumulatedTangentImpulseA + j;
if (limit < total)
total = limit;
if (total < -limit)
total = -limit;
j = total - cpd.AccumulatedTangentImpulseA;
cpd.AccumulatedTangentImpulseA = total;
bodyA.ApplyImpulse(j * cpd.FrictionWorldTangentialA, relPosA);
bodyB.ApplyImpulse(j * -cpd.FrictionWorldTangentialA, relPosB);
}
{
// 2nd tangent
Vector3 velA = bodyA.GetVelocityInLocalPoint(relPosA);
Vector3 velB = bodyB.GetVelocityInLocalPoint(relPosB);
Vector3 vel = velA - velB;
float vrel = Vector3.Dot(cpd.FrictionWorldTangentialB, vel);
// calculate j that moves us to zero relative velocity
float j = -vrel * cpd.JacDiagABInvTangentB;
float total = cpd.AccumulatedTangentImpulseB + j;
if (limit < total)
total = limit;
if (total < -limit)
total = -limit;
j = total - cpd.AccumulatedTangentImpulseB;
cpd.AccumulatedTangentImpulseB = total;
bodyA.ApplyImpulse(j * cpd.FrictionWorldTangentialB, relPosA);
bodyB.ApplyImpulse(j * -cpd.FrictionWorldTangentialB, relPosB);
}
}
return cpd.AppliedImpulse;
}
public override void AddRigidBody(RigidBody body)
{
if (!body.IsStaticOrKinematicObject)
{
body.Gravity = _gravity;
}
if (body.CollisionShape != null)
{
bool isDynamic = !(body.IsStaticObject || body.IsKinematicObject);
BroadphaseProxy.CollisionFilterGroups collisionFilterGroup = isDynamic ? BroadphaseProxy.CollisionFilterGroups.Default : BroadphaseProxy.CollisionFilterGroups.Static;
BroadphaseProxy.CollisionFilterGroups collisionFilterMask = isDynamic ? BroadphaseProxy.CollisionFilterGroups.All : (BroadphaseProxy.CollisionFilterGroups.All ^ BroadphaseProxy.CollisionFilterGroups.Static);
AddCollisionObject(body, collisionFilterGroup, collisionFilterMask);
}
}
// solving 2x2 lcp problem (inequality, direct solution )
public void ResolveBilateralPairConstraint(
RigidBody body1, RigidBody body2,
Matrix world2A, Matrix world2B,
Vector3 invInertiaADiag,
float invMassA,
Vector3 linvelA, Vector3 angvelA,
Vector3 rel_posA1,
Vector3 invInertiaBDiag,
float invMassB,
Vector3 linvelB, Vector3 angvelB,
Vector3 rel_posA2,
float depthA, Vector3 normalA,
Vector3 rel_posB1, Vector3 rel_posB2,
float depthB, Vector3 normalB,
out float imp0, out float imp1)
{
imp0 = 0f;
imp1 = 0f;
float len = Math.Abs(normalA.Length()) - 1f;
if (Math.Abs(len) >= float.Epsilon)
return;
BulletDebug.Assert(len < float.Epsilon);
JacobianEntry jacA = new JacobianEntry(world2A, world2B, rel_posA1, rel_posA2, normalA, invInertiaADiag, invMassA,
invInertiaBDiag, invMassB);
JacobianEntry jacB = new JacobianEntry(world2A, world2B, rel_posB1, rel_posB2, normalB, invInertiaADiag, invMassA,
invInertiaBDiag, invMassB);
float vel0 = Vector3.Dot(normalA, body1.GetVelocityInLocalPoint(rel_posA1) - body2.GetVelocityInLocalPoint(rel_posA1));
float vel1 = Vector3.Dot(normalB, body1.GetVelocityInLocalPoint(rel_posB1) - body2.GetVelocityInLocalPoint(rel_posB1));
// calculate rhs (or error) terms
float dv0 = depthA * _tau - vel0 * _damping;
float dv1 = depthB * _tau - vel1 * _damping;
float nonDiag = jacA.GetNonDiagonal(jacB, invMassA, invMassB);
float invDet = 1.0f / (jacA.Diagonal * jacB.Diagonal - nonDiag * nonDiag);
imp0 = dv0 * jacA.Diagonal * invDet + dv1 * -nonDiag * invDet;
imp1 = dv1 * jacB.Diagonal * invDet + dv0 * -nonDiag * invDet;
if (imp0 > 0.0f)
{
if (imp1 <= 0.0f)
{
imp1 = 0f;
// now imp0>0 imp1<0
imp0 = dv0 / jacA.Diagonal;
if (imp0 < 0.0f)
imp0 = 0f;
}
}
else
{
imp0 = 0f;
imp1 = dv1 / jacB.Diagonal;
if (imp1 <= 0.0f)
{
imp1 = 0f;
// now imp0>0 imp1<0
imp0 = dv0 / jacA.Diagonal;
if (imp0 > 0.0f)
{
}
else
{
imp0 = 0f;
}
}
}
}
//velocity + friction
//response between two dynamic objects with friction
public static float ResolveSingleCollisionCombined(
RigidBody bodyA,
RigidBody bodyB,
ManifoldPoint contactPoint,
ContactSolverInfo solverInfo)
{
Vector3 posA = contactPoint.PositionWorldOnA;
Vector3 posB = contactPoint.PositionWorldOnB;
Vector3 normal = contactPoint.NormalWorldOnB;
Vector3 relPosA = posA - bodyA.CenterOfMassPosition;
Vector3 relPosB = posB - bodyB.CenterOfMassPosition;
Vector3 velA = bodyA.GetVelocityInLocalPoint(relPosA);
Vector3 velB = bodyB.GetVelocityInLocalPoint(relPosB);
Vector3 vel = velA - velB;
float relVel;
relVel = Vector3.Dot(normal, vel);
float Kfps = 1f / solverInfo.TimeStep;
//float damping = solverInfo.m_damping;
float Kerp = solverInfo.Erp;
float Kcor = Kerp * Kfps;
ConstraintPersistentData cpd = contactPoint.UserPersistentData as ConstraintPersistentData;
if (cpd == null)
throw new BulletException();
float distance = cpd.Penetration;
float positionalError = Kcor * -distance;
float velocityError = cpd.Restitution - relVel;// * damping;
float penetrationImpulse = positionalError * cpd.JacDiagABInv;
float velocityImpulse = velocityError * cpd.JacDiagABInv;
float normalImpulse = penetrationImpulse + velocityImpulse;
// See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse
float oldNormalImpulse = cpd.AppliedImpulse;
float sum = oldNormalImpulse + normalImpulse;
cpd.AppliedImpulse = 0 > sum ? 0 : sum;
normalImpulse = cpd.AppliedImpulse - oldNormalImpulse;
if (bodyA.InverseMass != 0)
{
bodyA.InternalApplyImpulse(contactPoint.NormalWorldOnB * bodyA.InverseMass, cpd.AngularComponentA, normalImpulse);
}
if (bodyB.InverseMass != 0)
{
bodyB.InternalApplyImpulse(contactPoint.NormalWorldOnB * bodyB.InverseMass, cpd.AngularComponentB, -normalImpulse);
}
{
//friction
Vector3 vel12 = bodyA.GetVelocityInLocalPoint(relPosA);
Vector3 vel22 = bodyB.GetVelocityInLocalPoint(relPosB);
Vector3 vel3 = vel12 - vel22;
relVel = Vector3.Dot(normal, vel3);
Vector3 latVel = vel3 - normal * relVel;
float lat_rel_vel = latVel.Length();
float combinedFriction = cpd.Friction;
if (cpd.AppliedImpulse > 0)
if (lat_rel_vel > float.Epsilon)
{
latVel /= lat_rel_vel;
Vector3 temp1 = Vector3.TransformNormal(Vector3.Cross(relPosA, latVel), bodyA.InvInertiaTensorWorld);
Vector3 temp2 = Vector3.TransformNormal(Vector3.Cross(relPosB, latVel), bodyB.InvInertiaTensorWorld);
float friction_impulse = lat_rel_vel /
(bodyA.InverseMass + bodyB.InverseMass + Vector3.Dot(latVel, Vector3.Cross(temp1, relPosA) + Vector3.Cross(temp2, relPosB)));
float normal_impulse = cpd.AppliedImpulse * combinedFriction;
MathHelper.SetMin(ref friction_impulse, normal_impulse);
MathHelper.SetMin(ref friction_impulse, -normal_impulse);
bodyA.ApplyImpulse(latVel * -friction_impulse, relPosA);
bodyB.ApplyImpulse(latVel * friction_impulse, relPosB);
}
}
return normalImpulse;
}
public override void RemoveRigidBody(RigidBody body)
{
RemoveCollisionObject(body);
}
public static float ResolveSingleFrictionEmpty(
RigidBody bodyA,
RigidBody bodyB,
ManifoldPoint contactPoint,
ContactSolverInfo solverInfo)
{
return 0;
}
public ConstraintDemo()
{
CollisionShape shape = new BoxShape(new Vector3(HalfExtents, HalfExtents, HalfExtents));
Matrix trans = Matrix.Identity;
trans.Translation = new Vector3(0, 20, 0);
float mass = 1f;
//Point to Point constraint (ball socket)
{
RigidBody bodyA = CreateRigidBody(mass, trans, shape);
trans.Translation = new Vector3(2 * HalfExtents, 20, 0);
mass = 1f;
RigidBody bodyB = null;
//RigidBody bodyB = CreateRigidBody(mass, trans, shape);
//bodyB.ActivationState = ActivationState.DisableDeactivation;
//bodyB.SetDamping(0.3f, 0.3f);
Vector3 pivotInA = new Vector3(HalfExtents, -HalfExtents, -HalfExtents);
Vector3 axisInA = new Vector3(0, 0, 1);
Vector3 pivotInB = bodyB != null ? MathHelper.MatrixToVector(MathHelper.InvertMatrix(bodyB.CenterOfMassTransform), MathHelper.MatrixToVector(bodyA.CenterOfMassTransform, pivotInA)) : pivotInA;
Vector3 axisInB = bodyB != null ?
Vector3.TransformNormal(Vector3.TransformNormal(axisInA, bodyB.CenterOfMassTransform), MathHelper.InvertMatrix(bodyB.CenterOfMassTransform)) :
Vector3.TransformNormal(axisInA, bodyA.CenterOfMassTransform);
//TypedConstraint p2p = new Point2PointConstraint(bodyA, bodyB, pivotInA, pivotInB);
//TypedConstraint hinge = new HingeConstraint(bodyA, bodyB, pivotInA, pivotInB, axisInA, axisInB);
HingeConstraint hinge = new HingeConstraint(bodyA, pivotInA, axisInA);
//use zero targetVelocity and a small maxMotorImpulse to simulate joint friction
//float targetVelocity = 0.0f;
//float maxMotorImpulse = 0.01;
float targetVelocity = 1.0f;
float maxMotorImpulse = 1.0f;
hinge.EnableAngularMotor(true, targetVelocity, maxMotorImpulse);
PhysicsWorld.AddConstraint(hinge);
}
// create a slider, using the generic D6 constraint
{
mass = 1f;
Vector3 sliderWorldPos = new Vector3(0, 10, 0);
Vector3 sliderAxis = new Vector3(1, 0, 0);
float angle = 0;
Matrix sliderOrientation = Matrix.CreateFromQuaternion(new Quaternion(sliderAxis, angle));
trans = Matrix.Identity;
trans.Translation = sliderWorldPos;
//trans.setBasis(sliderOrientation);
_sliderTransform = trans;
_d6BodyA = CreateRigidBody(mass, trans, shape);
_d6BodyA.ActivationState = ActivationState.DisableDeactivation;
RigidBody fixedBody1 = CreateRigidBody(0, trans, null);
Matrix frameInA, frameInB;
frameInA = Matrix.Identity;
frameInB = Matrix.Identity;
Generic6DofConstraint slider = new Generic6DofConstraint(_d6BodyA, fixedBody1, frameInA, frameInB);
slider.SetLinearLowerLimit(_lowerSliderLimit);
slider.SetLinearUpperLimit(_hiSliderLimit);
//range should be small, otherwise singularities will 'explode' the constraint
slider.SetAngularLowerLimit(new Vector3(10, 0, 0));
slider.SetAngularUpperLimit(new Vector3(0, 0, 0));
PhysicsWorld.AddConstraint(slider);
}
}
protected float SolveCombinedContactFriction(RigidBody bodyA, RigidBody bodyB, ManifoldPoint cp, ContactSolverInfo info, int iter, IDebugDraw debugDraw)
{
float maxImpulse = 0;
Vector3 color = new Vector3(0, 1, 0);
if (cp.Distance <= 0)
{
if (iter == 0)
if (debugDraw != null)
debugDraw.DrawContactPoint(cp.PositionWorldOnB, cp.NormalWorldOnB, cp.Distance, cp.LifeTime, color);
float impulse = ContactConstraint.ResolveSingleCollisionCombined(
bodyA, bodyB,
cp,
info);
if (maxImpulse < impulse)
maxImpulse = impulse;
}
return maxImpulse;
}
protected internal void CreateRigidBody(BulletXScene parent_scene, IMesh mesh, OpenMetaverse.Vector3 pos,
OpenMetaverse.Vector3 size)
{
//For RigidBody Constructor. The next values might change
float _linearDamping = 0.0f;
float _angularDamping = 0.0f;
float _friction = 1.0f;
float _restitution = 0.0f;
Matrix _startTransform = Matrix.Identity;
Matrix _centerOfMassOffset = Matrix.Identity;
//added by jed zhu
_mesh = mesh;
lock (BulletXScene.BulletXLock)
{
_startTransform.Translation = BulletXMaths.PhysicsVectorToXnaVector3(pos);
//For now all prims are boxes
CollisionShape _collisionShape;
if (mesh == null)
{
_collisionShape = new BoxShape(BulletXMaths.PhysicsVectorToXnaVector3(size)/2.0f);
}
else
{
int iVertexCount = mesh.getVertexList().Count;
int[] indices = mesh.getIndexListAsInt();
Vector3[] v3Vertices = new Vector3[iVertexCount];
for (int i = 0; i < iVertexCount; i++)
{
OpenMetaverse.Vector3 v = mesh.getVertexList()[i];
if (v != null) // Note, null has special meaning. See meshing code for details
v3Vertices[i] = BulletXMaths.PhysicsVectorToXnaVector3(v);
else
v3Vertices[i] = Vector3.Zero;
}
TriangleIndexVertexArray triMesh = new TriangleIndexVertexArray(indices, v3Vertices);
_collisionShape = new TriangleMeshShape(triMesh);
}
DefaultMotionState _motionState = new DefaultMotionState(_startTransform, _centerOfMassOffset);
Vector3 _localInertia = new Vector3();
if (_physical) _collisionShape.CalculateLocalInertia(Mass, out _localInertia); //Always when mass > 0
rigidBody =
new RigidBody(Mass, _motionState, _collisionShape, _localInertia, _linearDamping, _angularDamping,
_friction, _restitution);
//rigidBody.ActivationState = ActivationState.DisableDeactivation;
//It's seems that there are a bug with rigidBody constructor and its CenterOfMassPosition
Vector3 _vDebugTranslation;
_vDebugTranslation = _startTransform.Translation - rigidBody.CenterOfMassPosition;
rigidBody.Translate(_vDebugTranslation);
//---
parent_scene.ddWorld.AddRigidBody(rigidBody);
}
}
internal void AddForgottenRigidBody(RigidBody forgottenRigidBody)
{
_forgottenRigidBodies.Add(forgottenRigidBody);
}
protected float Solve(RigidBody bodyA, RigidBody bodyB, ManifoldPoint cp, ContactSolverInfo info, int iter, IDebugDraw debugDraw)
{
float maxImpulse = 0;
Vector3 color = new Vector3(0, 1, 0);
if (cp.Distance <= 0)
{
if (iter == 0)
if(debugDraw != null)
debugDraw.DrawContactPoint(cp.PositionWorldOnB, cp.NormalWorldOnB, cp.Distance, cp.LifeTime, color);
ConstraintPersistentData cpd = cp.UserPersistentData as ConstraintPersistentData;
float impulse = cpd.ContactSolverFunc(
bodyA, bodyB,
cp,
info);
if (maxImpulse < impulse)
maxImpulse = impulse;
}
return maxImpulse;
}
private void InitSolverBody(out SolverBody solverBody, RigidBody rigidBody)
{
solverBody = new SolverBody();
solverBody.AngularVelocity = rigidBody.AngularVelocity;
solverBody.CenterOfMassPosition = rigidBody.CenterOfMassPosition;
solverBody.Friction = rigidBody.Friction;
solverBody.InvMass = rigidBody.InverseMass;
solverBody.LinearVelocity = rigidBody.LinearVelocity;
solverBody.OriginalBody = rigidBody;
solverBody.AngularFactor = rigidBody.AngularFactor;
}
protected float SolveFriction(RigidBody bodyA, RigidBody bodyB, ManifoldPoint cp, ContactSolverInfo info, int iter, IDebugDraw debugDraw)
{
Vector3 color = new Vector3(0, 1, 0);
if (cp.Distance <= 0)
{
ConstraintPersistentData cpd = cp.UserPersistentData as ConstraintPersistentData;
cpd.FrictionSolverFunc(
bodyA, bodyB,
cp,
info);
}
return 0;
}
public override void RemoveRigidBody(RigidBody body)
{
RemoveCollisionObject(body);
}
public abstract void AddRigidBody(RigidBody body);
public BulletXCharacter(String avName, BulletXScene parent_scene, OpenMetaverse.Vector3 pos, OpenMetaverse.Vector3 velocity,
OpenMetaverse.Vector3 size, OpenMetaverse.Vector3 acceleration, OpenMetaverse.Quaternion orientation)
: base(avName)
{
//This fields will be removed. They're temporal
float _sizeX = 0.5f;
float _sizeY = 0.5f;
float _sizeZ = 1.6f;
//.
_position = pos;
_velocity = velocity;
_size = size;
//---
_size.X = _sizeX;
_size.Y = _sizeY;
_size.Z = _sizeZ;
//.
_acceleration = acceleration;
_orientation = orientation;
_physical = true;
float _mass = 50.0f; //This depends of avatar's dimensions
//For RigidBody Constructor. The next values might change
float _linearDamping = 0.0f;
float _angularDamping = 0.0f;
float _friction = 0.5f;
float _restitution = 0.0f;
Matrix _startTransform = Matrix.Identity;
Matrix _centerOfMassOffset = Matrix.Identity;
lock (BulletXScene.BulletXLock)
{
_startTransform.Translation = BulletXMaths.PhysicsVectorToXnaVector3(pos);
//CollisionShape _collisionShape = new BoxShape(new MonoXnaCompactMaths.Vector3(1.0f, 1.0f, 1.60f));
//For now, like ODE, collisionShape = sphere of radious = 1.0
CollisionShape _collisionShape = new SphereShape(1.0f);
DefaultMotionState _motionState = new DefaultMotionState(_startTransform, _centerOfMassOffset);
Vector3 _localInertia = new Vector3();
_collisionShape.CalculateLocalInertia(_mass, out _localInertia); //Always when mass > 0
rigidBody =
new RigidBody(_mass, _motionState, _collisionShape, _localInertia, _linearDamping, _angularDamping,
_friction, _restitution);
//rigidBody.ActivationState = ActivationState.DisableDeactivation;
//It's seems that there are a bug with rigidBody constructor and its CenterOfMassPosition
Vector3 _vDebugTranslation;
_vDebugTranslation = _startTransform.Translation - rigidBody.CenterOfMassPosition;
rigidBody.Translate(_vDebugTranslation);
parent_scene.ddWorld.AddRigidBody(rigidBody);
}
}
protected float Solve(RigidBody bodyA, RigidBody bodyB, ManifoldPoint cp, ContactSolverInfo info, int iter)
{
return Solve(bodyA, bodyB, cp, info, iter, null);
}
internal BulletXPlanet(BulletXScene parent_scene, float[] heightField)
{
_staticPosition = new OpenMetaverse.Vector3(BulletXScene.MaxXY / 2, BulletXScene.MaxXY / 2, 0);
// _staticVelocity = new PhysicsVector();
// _staticOrientation = OpenMetaverse.Quaternion.Identity;
_mass = 0; //No active
// _parentscene = parent_scene;
_heightField = heightField;
float _linearDamping = 0.0f;
float _angularDamping = 0.0f;
float _friction = 0.5f;
float _restitution = 0.0f;
Matrix _startTransform = Matrix.Identity;
Matrix _centerOfMassOffset = Matrix.Identity;
lock (BulletXScene.BulletXLock)
{
try
{
_startTransform.Translation = BulletXMaths.PhysicsVectorToXnaVector3(_staticPosition);
CollisionShape _collisionShape =
new HeightfieldTerrainShape(BulletXScene.MaxXY, BulletXScene.MaxXY, _heightField,
(float) BulletXScene.MaxZ, 2, true, false);
DefaultMotionState _motionState = new DefaultMotionState(_startTransform, _centerOfMassOffset);
Vector3 _localInertia = new Vector3();
//_collisionShape.CalculateLocalInertia(_mass, out _localInertia); //Always when mass > 0
_flatPlanet =
new RigidBody(_mass, _motionState, _collisionShape, _localInertia, _linearDamping,
_angularDamping, _friction, _restitution);
//It's seems that there are a bug with rigidBody constructor and its CenterOfMassPosition
Vector3 _vDebugTranslation;
_vDebugTranslation = _startTransform.Translation - _flatPlanet.CenterOfMassPosition;
_flatPlanet.Translate(_vDebugTranslation);
parent_scene.ddWorld.AddRigidBody(_flatPlanet);
}
catch (Exception ex)
{
BulletXScene.BulletXMessage(ex.Message, true);
}
}
BulletXScene.BulletXMessage("BulletXPlanet created.", false);
}
/// <summary>
/// Creates new rigid body and adds it to the world.
/// </summary>
/// <param name="mass">Body mass, if 0 body is static.</param>
/// <param name="startTransform">Starting body transform.</param>
/// <param name="shape">Body shape.</param>
/// <returns>Created rigid body.</returns>
public RigidBody CreateRigidBody(float mass, Matrix startTransform, CollisionShape shape)
{
//rigidbody is dynamic if and only if mass is non zero, otherwise static
bool isDynamic = (mass != 0.0f);
Vector3 localInertia = new Vector3();
if (isDynamic)
shape.CalculateLocalInertia(mass, out localInertia);
//using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
DefaultMotionState myMotionState = new DefaultMotionState(startTransform, Matrix.Identity);
RigidBody body = new RigidBody(mass, myMotionState, shape, localInertia, 0.0f, 0.0f, 0.5f, 0.0f);
_world.AddRigidBody(body);
return body;
}
public TypedConstraint(RigidBody rbA)
: this(rbA, _fixed) { }
/// <summary>
/// Called when the game has determined that game logic needs to be processed.
/// </summary>
/// <param name="gameTime">Time passed since the last call to this function.</param>
protected override void Update(GameTime gameTime)
{
MouseState mouseState = Mouse.GetState();
Vector3 rayTo = getRayTo(mouseState.X, mouseState.Y);
if (mouseState.LeftButton == ButtonState.Pressed && _prevMouseState.LeftButton == ButtonState.Released)
{
shootBox(rayTo);
}
if (mouseState.MiddleButton == ButtonState.Pressed && _prevMouseState.MiddleButton == ButtonState.Released)
{
if (_world != null)
{
CollisionWorld.ClosestRayResultCallback rayCallback = new CollisionWorld.ClosestRayResultCallback(_camera.Position, rayTo);
_world.RayTest(_camera.Position, rayTo, rayCallback);
if (rayCallback.HasHit)
{
RigidBody body = RigidBody.Upcast(rayCallback.CollisionObject);
if (body != null)
{
//other exclusions?
if (!(body.IsStaticObject || body.IsKinematicObject))
{
_pickedBody = body;
_pickedBody.ActivationState = ActivationState.DisableDeactivation;
Vector3 pickPos = rayCallback.HitPointWorld;
Vector3 localPivot = Vector3.Transform(pickPos, XnaDevRu.BulletX.MathHelper.InvertMatrix(body.CenterOfMassTransform));
Point2PointConstraint p2p = new Point2PointConstraint(body, localPivot);
_world.AddConstraint(p2p);
_pickConstraint = p2p;
//save mouse position for dragging
_oldPickingPos = rayTo;
Vector3 eyePos = new Vector3(_camera.Position.X, _camera.Position.Y, _camera.Position.Z);
_oldPickingDist = (eyePos - pickPos).Length();
//very weak constraint for picking
p2p.Settings.Tau = 1.1f;
}
}
}
}
}
else if (mouseState.MiddleButton == ButtonState.Released && _prevMouseState.MiddleButton == ButtonState.Pressed)
{
if (_pickConstraint != null && _world != null)
{
_world.RemoveConstraint(_pickConstraint);
_pickConstraint = null;
_pickedBody.ForceActivationState(ActivationState.Active);
_pickedBody.DeactivationTime = 0f;
_pickedBody = null;
}
}
if (_pickConstraint != null)
{
//move the constraint pivot
Point2PointConstraint p2p = _pickConstraint as Point2PointConstraint;
if (p2p != null)
{
//keep it at the same picking distance
Vector3 dir = rayTo - _camera.Position;
dir.Normalize();
dir *= _oldPickingDist;
Vector3 newPos = _camera.Position + dir;
p2p.PivotInB = newPos;
}
}
_prevMouseState = mouseState;
if (Keyboard.GetState().IsKeyDown(Keys.Space))
{
//world.stepSimulation(1.0f/60.0f,0);
int numObjects = _world.CollisionObjectsCount;
for (int i = 0; i < numObjects; i++)
{
CollisionObject colObj = _world.CollisionObjects[i];
RigidBody body = RigidBody.Upcast(colObj);
if (body != null)
{
if (body.MotionState != null)
{
DefaultMotionState myMotionState = (DefaultMotionState)body.MotionState;
myMotionState.GraphicsWorldTransform = myMotionState.StartWorldTransform;
colObj.WorldTransform = myMotionState.GraphicsWorldTransform;
colObj.InterpolationWorldTransform = myMotionState.StartWorldTransform;
colObj.Activate();
}
//removed cached contact points
_world.Broadphase.CleanProxyFromPairs(colObj.Broadphase);
if (body != null && !body.IsStaticObject)
{
RigidBody.Upcast(colObj).LinearVelocity = new Vector3(0, 0, 0);
RigidBody.Upcast(colObj).AngularVelocity = new Vector3(0, 0, 0);
}
}
}
}
else if (Keyboard.GetState().IsKeyDown(Keys.Escape) || GamePad.GetState(PlayerIndex.One).Buttons.Back == ButtonState.Pressed)
{
Exit();
}
else
{
//world.stepSimulation(1.0f / 60.0f, 1);
}
base.Update(gameTime);
}
/// <summary>
///
/// </summary>
/// <param name="chassis"></param>
/// <param name="paramRaycastInfo">Not used!</param>
public void UpdateWheel(RigidBody chassis, RaycastInfo paramRaycastInfo)
{
if (_raycastInfo.IsInContact)
{
float project = Vector3.Dot(_raycastInfo.ContactNormalWS, _raycastInfo.WheelDirectionWS);
Vector3 chassisVelocityAtContactPoint = new Vector3();
Vector3 relpos = _raycastInfo.ContactPointWS - chassis.CenterOfMassPosition;
chassisVelocityAtContactPoint = chassis.GetVelocityInLocalPoint(relpos);
float projVel = Vector3.Dot(_raycastInfo.ContactNormalWS, chassisVelocityAtContactPoint);
if (project >= -0.1f)
{
_suspensionRelativeVelocity = 0;
_clippedInvContactDotSuspension = 1.0f / 0.1f;
}
else
{
float inv = -1 / project;
_suspensionRelativeVelocity = projVel * inv;
_clippedInvContactDotSuspension = inv;
}
}
else
{
_raycastInfo.SuspensionLength = _suspensionRestLength;
_suspensionRelativeVelocity = 0.0f;
_raycastInfo.ContactNormalWS = -_raycastInfo.WheelDirectionWS;
_clippedInvContactDotSuspension = 1.0f;
}
}
public override void AddRigidBody(RigidBody body)
{
body.Gravity = _gravity;
if (body.CollisionShape != null)
{
AddCollisionObject(body);
}
}
public abstract void RemoveRigidBody(RigidBody body);
protected void SynchronizeMotionStates()
{
//debug vehicle wheels
{
//todo: iterate over awake simulation islands!
for (int i = 0; i < CollisionObjects.Count; i++)
{
CollisionObject colObj = CollisionObjects[i];
if (DebugDrawer != null && (DebugDrawer.DebugMode & DebugDrawModes.DrawWireframe) != 0)
{
Vector3 color = new Vector3(255f, 255f, 255f);
switch (colObj.ActivationState)
{
case ActivationState.Active:
color = new Vector3(255f, 255f, 255f); break;
case ActivationState.IslandSleeping:
color = new Vector3(0f, 255f, 0f); break;
case ActivationState.WantsDeactivation:
color = new Vector3(0f, 255f, 255f); break;
case ActivationState.DisableDeactivation:
color = new Vector3(255f, 0f, 0f); break;
case ActivationState.DisableSimulation:
color = new Vector3(255f, 255f, 0f); break;
default:
color = new Vector3(255f, 0f, 0f); break;
}
DebugDrawObject(colObj.WorldTransform, colObj.CollisionShape, color);
}
RigidBody body = RigidBody.Upcast(colObj);
if (body != null && body.MotionState != null && !body.IsStaticOrKinematicObject)
{
//if (body.ActivationState != ActivationState.IslandSleeping)
{
Matrix interpolatedTransform = new Matrix();
TransformUtil.IntegrateTransform(body.InterpolationWorldTransform,
body.InterpolationLinearVelocity, body.InterpolationAngularVelocity, _localTime, ref interpolatedTransform);
body.MotionState.SetWorldTransform(interpolatedTransform);
}
}
}
}
if (DebugDrawer != null && (DebugDrawer.DebugMode & DebugDrawModes.DrawWireframe) != 0)
{
for (int i = 0; i < _vehicles.Count; i++)
{
for (int v = 0; v < _vehicles[i].getNumWheels(); v++)
{
Vector3 wheelColor = new Vector3(0, 255, 255);
if (_vehicles[i].getWheelInfo(v).RaycastInfo.IsInContact)
{
wheelColor = new Vector3(0, 0, 255);
}
else
{
wheelColor = new Vector3(255, 0, 255);
}
//synchronize the wheels with the (interpolated) chassis worldtransform
_vehicles[i].updateWheelTransform(v, true);
Vector3 wheelPosWS = _vehicles[i].getWheelInfo(v).WorldTransform.Translation;
Vector3 axle = new Vector3(
MathHelper.GetElement(_vehicles[i].getWheelInfo(v).WorldTransform, 0, _vehicles[i].getRightAxis()),
MathHelper.GetElement(_vehicles[i].getWheelInfo(v).WorldTransform, 1, _vehicles[i].getRightAxis()),
MathHelper.GetElement(_vehicles[i].getWheelInfo(v).WorldTransform, 2, _vehicles[i].getRightAxis()));
//m_vehicles[i]->getWheelInfo(v).m_raycastInfo.m_wheelAxleWS
//debug wheels (cylinders)
_debugDrawer.DrawLine(wheelPosWS, wheelPosWS + axle, wheelColor);
_debugDrawer.DrawLine(wheelPosWS, _vehicles[i].getWheelInfo(v).RaycastInfo.ContactPointWS, wheelColor);
}
}
}
}
