public override void InitializeDemo()
{
base.InitializeDemo();
SetCameraDistance(SCALING * 50f);
//string filename = @"C:\users\man\bullett\xna-largemesh-output.txt";
//FileStream filestream = File.Open(filename, FileMode.Create, FileAccess.Write, FileShare.Read);
//BulletGlobals.g_streamWriter = new StreamWriter(filestream);
///collision configuration contains default setup for memory, collision setup
m_collisionConfiguration = new DefaultCollisionConfiguration();
///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded)
m_dispatcher = new CollisionDispatcher(m_collisionConfiguration);
m_broadphase = new DbvtBroadphase();
IOverlappingPairCache pairCache = null;
//pairCache = new SortedOverlappingPairCache();
//m_broadphase = new SimpleBroadphase(10000, pairCache);
///the default constraint solver. For parallel processing you can use a different solver (see Extras/BulletMultiThreaded)
SequentialImpulseConstraintSolver sol = new SequentialImpulseConstraintSolver();
m_constraintSolver = sol;
m_dynamicsWorld = new DiscreteDynamicsWorld(m_dispatcher, m_broadphase, m_constraintSolver, m_collisionConfiguration);
IndexedVector3 gravity = new IndexedVector3(0, -10, 0);
m_dynamicsWorld.SetGravity(ref gravity);
m_profileManager = new BasicProfileManager();
BulletGlobals.g_profileManager = m_profileManager;
m_profileIterator = m_profileManager.getIterator();
///create a few basic rigid bodies
IndexedVector3 halfExtents = new IndexedVector3(50, 50, 50);
//IndexedVector3 halfExtents = new IndexedVector3(10, 10, 10);
//CollisionShape groundShape = new BoxShape(ref halfExtents);
CollisionShape groundShape = new StaticPlaneShape(new IndexedVector3(0, 1, 0), 0);
LocalCreateRigidBody(0f, IndexedMatrix.Identity, groundShape);
//CollisionShape groundShape = BuildLargeMesh();
m_collisionShapes.Add(groundShape);
CollisionShape sphereShape = new SphereShape(0.2f);
int size = 16;// 5; // 16
for (int i = 0; i < size; ++i)
{
for (int j = 0; j < size; ++j)
{
IndexedMatrix m = IndexedMatrix.CreateTranslation(new IndexedVector3(i, 1, j));
RigidBody rb = LocalCreateRigidBody(1f, m, sphereShape);
rb.SetActivationState(ActivationState.ISLAND_SLEEPING);
}
}
ClientResetScene();
}
//----------------------------------------------------------------------------------------------
public void SetupWorldObjects()
{
IndexedVector3 halfExtents = new IndexedVector3(10, 5, 10);
CollisionShape groundShape = new BoxShape(ref halfExtents);
IndexedVector3 world1Center = new IndexedVector3(-20, -5, 0);
IndexedVector3 world2Center = new IndexedVector3(20, -5, 0);
IndexedMatrix groundTransform1 = IndexedMatrix.CreateTranslation(world1Center);
IndexedMatrix groundTransform2 = IndexedMatrix.CreateTranslation(world2Center);
IndexedVector3 halfExtents2 = new IndexedVector3(0.5f);
CollisionShape smallBoxShape = new BoxShape(ref halfExtents2);
//IndexedMatrix groundTransform = IndexedMatrix.CreateTranslation(new IndexedVector3(0,-10,0));
float mass = 0f;
LocalCreateRigidBodyMultiWorld(mass, ref groundTransform1, groundShape, m_worlds[0]);
LocalCreateRigidBodyMultiWorld(mass, ref groundTransform2, groundShape, m_worlds[1]);
mass = 1f;
for (int i = 0; i < 5; ++i)
{
IndexedVector3 offset = new IndexedVector3(0, halfExtents.Y + halfExtents2.Y + (2 * i), 0);
IndexedMatrix boxTransform1 = IndexedMatrix.CreateTranslation(world1Center + offset);
IndexedMatrix boxTransform2 = IndexedMatrix.CreateTranslation(world2Center + offset);
LocalCreateRigidBodyMultiWorld(mass, ref boxTransform1, smallBoxShape, m_worlds[0]);
LocalCreateRigidBodyMultiWorld(mass, ref boxTransform2, smallBoxShape, m_worlds[1]);
}
}
public override void InitializeDemo()
{
//string filename = @"C:\users\man\bullet\xna-motor-output.txt";
//FileStream filestream = File.Open(filename, FileMode.Create, FileAccess.Write, FileShare.Read);
//BulletGlobals.g_streamWriter = new StreamWriter(filestream);
m_Time = 0;
m_fCyclePeriod = 2000.0f; // in milliseconds
// new SIMD solver for joints clips accumulated impulse, so the new limits for the motor
// should be (numberOfsolverIterations * oldLimits)
// currently solver uses 10 iterations, so:
m_fMuscleStrength = 0.5f;
SetCameraDistance(5.0f);
m_collisionConfiguration = new DefaultCollisionConfiguration();
m_dispatcher = new CollisionDispatcher(m_collisionConfiguration);
m_broadphase = new DbvtBroadphase();
IOverlappingPairCache pairCache = null;
m_broadphase = new SimpleBroadphase(1000, pairCache);
SequentialImpulseConstraintSolver sol = new SequentialImpulseConstraintSolver();
m_constraintSolver = sol;
m_dynamicsWorld = new DiscreteDynamicsWorld(m_dispatcher, m_broadphase, m_constraintSolver, m_collisionConfiguration);
IndexedVector3 gravity = new IndexedVector3(0, -10, 0);
m_dynamicsWorld.SetGravity(ref gravity);
m_dynamicsWorld.SetInternalTickCallback(new MotorPreTickCallback(), this, true);
// Setup a big ground box
{
CollisionShape groundShape = new BoxShape(new IndexedVector3(200.0f, 10.0f, 200.0f));
m_collisionShapes.Add(groundShape);
IndexedMatrix groundTransform = IndexedMatrix.CreateTranslation(0, -10, 0);
LocalCreateRigidBody(0f, ref groundTransform, groundShape);
}
// Spawn one ragdoll
IndexedVector3 startOffset = new IndexedVector3(1, 0.5f, 0);
SpawnTestRig(ref startOffset, false);
startOffset = new IndexedVector3(-2, 0.5f, 0);
SpawnTestRig(ref startOffset, true);
ClientResetScene();
}
public virtual void LoadPlayerController(Entity playerEntity, SceneNode characterNode, object userData, Vector3 mobNodePositionUpdate)
{
//if (!initialized)
//{
// characterToLoad = characterNode;
// characterEntityToLoad = playerEntity;
// JumpHandlerToLoad = jumpHandler;
// MobNodePositionUpdateToLoad = mobNodePositionUpdate;
// userDataToLoad = userData;
// return;
//}
if (playerController != null)
{
return;
}
float modelHeight = 2f;// (playerEntity.BoundingBox.Max.Y) / scaleFactor; // AJ: used to subtract minimum from maximum- playerEntity.BoundingBox.Minimum.y
System.Console.WriteLine("Player capsule info: modelheight '{0}', boundingbox max '{1}', bounding box min '{2}' and playerPosition '{3}'",
modelHeight, playerEntity.BoundingBox.Max.Y, playerEntity.BoundingBox.Min.Y, characterNode.Position);
float radius = 1.75f;
float height = 1.75f;
ConvexShape capsule = new CapsuleShape(radius, height);
//ConvexShape capsule = new SphereShape(radius);
ghostObject = new PairCachingGhostObject();
Vector3 position = new Vector3(0, 0, 0);//new Vector3(characterNode.Position.X / scaleFactor, (characterNode.Position.Y + 1500) / scaleFactor, characterNode.Position.Z / scaleFactor);
//IndexedMatrix worldTransform = IndexedMatrix.CreateTranslation(characterNode.Position.X / scaleFactor,
// (characterNode.Position.Y + 1500) / scaleFactor, characterNode.Position.Z / scaleFactor);
IndexedMatrix worldTransform = IndexedMatrix.CreateTranslation(position);
ghostObject.SetWorldTransform(worldTransform);
//broadphase.OverlappingPairCache.SetInternalGhostPairCallback(new GhostPairCallback());
ghostObject.SetCollisionShape(capsule);
ghostObject.SetCollisionFlags(CollisionFlags.CF_CHARACTER_OBJECT);
float stepHeight = 0.35f;
playerController = new KinematicCharacterController(ghostObject, capsule, stepHeight, 1);
//characterToLoad = null;
BulletMobState mobMovementState = new BulletMobState(playerController, mobNodePositionUpdate);
//mobMovementState.JumpEvent += jumpHandler;
mobControllers.Add(characterNode, mobMovementState);
//m_dynamicsWorld.AddCollisionObject(ghostObject, CollisionFilterGroups.CharacterFilter, CollisionFilterGroups.StaticFilter | CollisionFilterGroups.DefaultFilter);
m_dynamicsWorld.AddCollisionObject(ghostObject, CollisionFilterGroups.CharacterFilter, CollisionFilterGroups.StaticFilter | CollisionFilterGroups.DefaultFilter);
//m_dynamicsWorld.AddCollisionObject(ghostObject, CollisionFilterGroups.DefaultFilter, CollisionFilterGroups.AllFilter);
m_dynamicsWorld.AddAction(playerController);
//collisionShapes.Add(capsule);
//frozenTime = 0;
}
////////////////////////////////////////////////////////////////////////////////
//
// TerrainDemo -- private helper methods
//
////////////////////////////////////////////////////////////////////////////////
/// called whenever key terrain attribute is changed
public override void ClientResetScene()
{
base.ClientResetScene();
// remove old heightfield
m_rawHeightfieldData = null;
// reset gravity to point in appropriate direction
//m_dynamicsWorld.setGravity(getUpVector(m_upAxis, 0.0f, -s_gravity));
m_dynamicsWorld.SetGravity(ref m_defaultGravity);
// get new heightfield of appropriate type
m_rawHeightfieldData =
GetRawHeightfieldData(m_model, m_type, ref m_minHeight, ref m_maxHeight);
Debug.Assert(m_rawHeightfieldData != null, "failed to create raw heightfield");
if (m_terrainRigidBody != null)
{
m_dynamicsWorld.RemoveCollisionObject(m_terrainRigidBody);
m_terrainRigidBody = null;
m_terrainShape = null;
m_collisionShapes.Remove(m_terrainShape);
}
bool flipQuadEdges = false;
m_terrainShape =
new HeightfieldTerrainShape(s_gridSize, s_gridSize,
m_rawHeightfieldData,
s_gridHeightScale,
m_minHeight, m_maxHeight,
m_upAxis, m_type, flipQuadEdges);
Debug.Assert(m_terrainShape != null, "null heightfield");
// scale the shape
IndexedVector3 localScaling = GetUpVector(m_upAxis, s_gridSpacing, 1.0f);
m_terrainShape.SetLocalScaling(ref localScaling);
// stash this shape away
m_collisionShapes.Add(m_terrainShape);
// set origin to middle of heightfield
IndexedMatrix tr = IndexedMatrix.CreateTranslation(new IndexedVector3(0, -20, 0));
// create ground object
float mass = 0.0f;
m_terrainRigidBody = LocalCreateRigidBody(mass, ref tr, m_terrainShape);
CollisionShape sphere = new SphereShape(0.5f);
tr = IndexedMatrix.CreateTranslation(new IndexedVector3(0, 0, 0));
LocalCreateRigidBody(1f, ref tr, sphere);
}
public override void InitializeDemo()
{
// Setup the basic world
SetTexturing(true);
SetShadows(true);
SetCameraDistance(5.0f);
m_collisionConfiguration = new DefaultCollisionConfiguration();
m_dispatcher = new CollisionDispatcher(m_collisionConfiguration);
IndexedVector3 worldAabbMin = new IndexedVector3(-10000, -10000, -10000);
IndexedVector3 worldAabbMax = new IndexedVector3(10000, 10000, 10000);
m_broadphase = new AxisSweep3Internal(ref worldAabbMin, ref worldAabbMax, 0xfffe, 0xffff, 16384, null, true);
//m_broadphase = new SimpleBroadphase(1000, null);
m_constraintSolver = new SequentialImpulseConstraintSolver();
m_dynamicsWorld = new DiscreteDynamicsWorld(m_dispatcher, m_broadphase, m_constraintSolver, m_collisionConfiguration);
//m_dynamicsWorld.getDispatchInfo().m_useConvexConservativeDistanceUtil = true;
//m_dynamicsWorld.getDispatchInfo().m_convexConservativeDistanceThreshold = 0.01f;
// Setup a big ground box
{
CollisionShape groundShape = new BoxShape(new IndexedVector3(200.0f, 10.0f, 200.0f));
m_collisionShapes.Add(groundShape);
IndexedMatrix groundTransform = IndexedMatrix.CreateTranslation(0, -10, 0);
CollisionObject fixedGround = new CollisionObject();
fixedGround.SetCollisionShape(groundShape);
fixedGround.SetWorldTransform(ref groundTransform);
fixedGround.SetUserPointer("Ground");
m_dynamicsWorld.AddCollisionObject(fixedGround);
}
// Spawn one ragdoll
IndexedVector3 startOffset = new IndexedVector3(1, 0.5f, 0);
//string filename = @"c:\users\man\bullet\xna-ragdoll-constraints-output.txt";
//FileStream filestream = File.Open(filename, FileMode.Create, FileAccess.Write, FileShare.Read);
//BulletGlobals.g_streamWriter = new StreamWriter(filestream);
SpawnRagdoll(ref startOffset, BulletGlobals.g_streamWriter);
//startOffset = new IndexedVector3(-1,0.5f,0);
//spawnRagdoll(ref startOffset);
ClientResetScene();
}
//----------------------------------------------------------------------------------------------------------------
public override void ClientResetScene()
{
base.ClientResetScene();
IndexedMatrix ident = IndexedMatrix.Identity;
m_carChassis.SetCenterOfMassTransform(ref ident);
IndexedVector3 zero = IndexedVector3.Zero;
m_carChassis.SetLinearVelocity(ref zero);
m_carChassis.SetAngularVelocity(ref zero);
m_dynamicsWorld.GetBroadphase().GetOverlappingPairCache().CleanProxyFromPairs(m_carChassis.GetBroadphaseHandle(), GetDynamicsWorld().GetDispatcher());
if (m_liftBody != null)
{
IndexedMatrix liftTrans = IndexedMatrix.CreateTranslation(m_liftStartPos);
m_liftBody.Activate();
m_liftBody.SetCenterOfMassTransform(ref liftTrans);
m_liftBody.SetLinearVelocity(ref zero);
m_liftBody.SetAngularVelocity(ref zero);
}
if (m_forkBody != null)
{
IndexedMatrix forkTrans = IndexedMatrix.CreateTranslation(m_forkStartPos);
m_forkBody.Activate();
m_forkBody.SetCenterOfMassTransform(ref forkTrans);
m_forkBody.SetLinearVelocity(ref zero);
m_forkBody.SetAngularVelocity(ref zero);
}
if (m_liftHinge != null)
{
// m_liftHinge.setLimit(-LIFT_EPS, LIFT_EPS);
m_liftHinge.SetLimit(0.0f, 0.0f);
m_liftHinge.EnableAngularMotor(false, 0, 0);
}
if (m_forkSlider != null)
{
m_forkSlider.SetLowerLinLimit(0.1f);
m_forkSlider.SetUpperLinLimit(0.1f);
m_forkSlider.SetPoweredLinMotor(false);
}
if (m_loadBody != null)
{
IndexedMatrix loadTrans = IndexedMatrix.CreateTranslation(m_loadStartPos);
m_loadBody.Activate();
m_loadBody.SetCenterOfMassTransform(ref loadTrans);
m_loadBody.SetLinearVelocity(ref zero);
m_loadBody.SetAngularVelocity(ref zero);
}
}
public override void InitializeDemo()
{
SetCameraDistance(50f);
m_collisionConfiguration = new DefaultCollisionConfiguration();
///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded)
m_dispatcher = new CollisionDispatcher(m_collisionConfiguration);
IndexedVector3 worldMin = new IndexedVector3(-1000, -1000, -1000);
IndexedVector3 worldMax = -worldMin;
//m_broadphase = new AxisSweep3Internal(ref worldMin, ref worldMax, 0xfffe, 0xffff, 16384, null, false);
//m_broadphase = new DbvtBroadphase();
IOverlappingPairCache pairCache = null;
//pairCache = new SortedOverlappingPairCache();
m_broadphase = new SimpleBroadphase(1000, pairCache);
///the default constraint solver. For parallel processing you can use a different solver (see Extras/BulletMultiThreaded)
SequentialImpulseConstraintSolver sol = new SequentialImpulseConstraintSolver();
m_constraintSolver = sol;
m_dynamicsWorld = new DiscreteDynamicsWorld(m_dispatcher, m_broadphase, m_constraintSolver, m_collisionConfiguration);
IndexedVector3 gravity = new IndexedVector3(0, -10, 0);
m_dynamicsWorld.SetGravity(ref gravity);
///create a few basic rigid bodies
IndexedVector3 halfExtents = new IndexedVector3(50, 50, 50);
//IndexedVector3 halfExtents = new IndexedVector3(10, 10, 10);
CollisionShape groundShape = new BoxShape(ref halfExtents);
//CollisionShape groundShape = new StaticPlaneShape(new IndexedVector3(0,1,0), 50);
m_collisionShapes.Add(groundShape);
IndexedMatrix groundTransform = IndexedMatrix.CreateTranslation(new IndexedVector3(0, -50, 0));
//IndexedMatrix groundTransform = IndexedMatrix.CreateTranslation(new IndexedVector3(0,-10,0));
float mass = 0f;
LocalCreateRigidBody(mass, ref groundTransform, groundShape);
CollisionShape shape = SetupShape();
IndexedMatrix objTransform = IndexedMatrix.CreateTranslation(new IndexedVector3(0, 2, 0));
LocalCreateRigidBody(mass, ref objTransform, shape);
//ClientResetScene();
}
public override void InitializeDemo()
{
m_maxIterations = 500;
SetCameraDistance(SCALING * 50f);
//string filename = @"E:\users\man\bullet\xna-basic-output-1.txt";
//FileStream filestream = File.Open(filename, FileMode.Create, FileAccess.Write, FileShare.Read);
//BulletGlobals.g_streamWriter = new StreamWriter(filestream);
///collision configuration contains default setup for memory, collision setup
m_collisionConfiguration = new DefaultCollisionConfiguration();
///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded)
m_dispatcher = new CollisionDispatcher(m_collisionConfiguration);
m_broadphase = new DbvtBroadphase();
IOverlappingPairCache pairCache = null;
//pairCache = new SortedOverlappingPairCache();
//m_broadphase = new SimpleBroadphase(1000, pairCache);
///the default constraint solver. For parallel processing you can use a different solver (see Extras/BulletMultiThreaded)
SequentialImpulseConstraintSolver sol = new SequentialImpulseConstraintSolver();
m_constraintSolver = sol;
m_dynamicsWorld = new DiscreteDynamicsWorld(m_dispatcher, m_broadphase, m_constraintSolver, m_collisionConfiguration);
IndexedVector3 gravity = new IndexedVector3(0, -10, 0);
m_dynamicsWorld.SetGravity(ref gravity);
///create a few basic rigid bodies
IndexedVector3 halfExtents = new IndexedVector3(50, 50, 50);
//IndexedVector3 halfExtents = new IndexedVector3(10, 10, 10);
CollisionShape groundShape = new BoxShape(ref halfExtents);
//CollisionShape groundShape = new StaticPlaneShape(new IndexedVector3(0,1,0), 50);
m_collisionShapes.Add(groundShape);
IndexedMatrix groundTransform = IndexedMatrix.CreateTranslation(new IndexedVector3(0, -52, 0));
//IndexedMatrix groundTransform = IndexedMatrix.CreateTranslation(new IndexedVector3(0,-10,0));
float mass = 0f;
LocalCreateRigidBody(mass, ref groundTransform, groundShape);
CreateScene5(20);
ClientResetScene();
}
private void CreateScene5(int dim)
{
BoxShape boxShape = new BoxShape(new IndexedVector3(0.5f));
float mass = 1.0f;
for (int x = 0; x < dim; x++)
{
for (int e = x; e < dim; e++)
{
//IndexedVector3 pos = new IndexedVector3(e - 0.5f * x, x * 1.01f - 14, 25);
IndexedVector3 pos = new IndexedVector3(e - 0.5f * x, x * 1.0f, 0);
RigidBody rb = LocalCreateRigidBody(mass, IndexedMatrix.CreateTranslation(pos), boxShape);
//m_dynamicsWorld.AddRigidBody(rb);
}
}
}
public static void IntegrateTransform(ref IndexedMatrix curTrans,ref IndexedVector3 linvel,ref IndexedVector3 angvel,float timeStep,out IndexedMatrix predictedTransform)
{
predictedTransform = IndexedMatrix.CreateTranslation(curTrans._origin + linvel * timeStep);
// #define QUATERNION_DERIVATIVE
#if QUATERNION_DERIVATIVE
IndexedVector3 pos;
IndexedQuaternion predictedOrn;
IndexedVector3 scale;
curTrans.Decompose(ref scale, ref predictedOrn, ref pos);
predictedOrn += (angvel * predictedOrn) * (timeStep * .5f));
predictedOrn.Normalize();
#else
//Exponential map
//google for "Practical Parameterization of Rotations Using the Exponential Map", F. Sebastian Grassia
IndexedVector3 axis;
float fAngle = angvel.Length();
//limit the angular motion
if (fAngle*timeStep > ANGULAR_MOTION_THRESHOLD)
{
fAngle = ANGULAR_MOTION_THRESHOLD / timeStep;
}
if ( fAngle < 0.001f )
{
// use Taylor's expansions of sync function
axis = angvel*( 0.5f*timeStep-(timeStep*timeStep*timeStep)*(0.020833333333f)*fAngle*fAngle );
}
else
{
// sync(fAngle) = sin(c*fAngle)/t
axis = angvel*( (float)Math.Sin(0.5f*fAngle*timeStep)/fAngle );
}
IndexedQuaternion dorn = new IndexedQuaternion(axis.X,axis.Y,axis.Z,(float)Math.Cos( fAngle*timeStep*.5f) );
IndexedQuaternion orn0 = curTrans.GetRotation();
IndexedQuaternion predictedOrn = dorn * orn0;
predictedOrn.Normalize();
#endif
IndexedMatrix newMatrix = IndexedMatrix.CreateFromQuaternion(predictedOrn);
predictedTransform._basis = newMatrix._basis;
}
public override void InitializeDemo()
{
base.InitializeDemo();
SetCameraDistance(SCALING * 50f);
//string filename = @"e:\users\man\bullet\xna-largemesh-output.txt";
//FileStream filestream = File.Open(filename, FileMode.Create, FileAccess.Write, FileShare.Read);
//BulletGlobals.g_streamWriter = new StreamWriter(filestream);
///collision configuration contains default setup for memory, collision setup
m_collisionConfiguration = new DefaultCollisionConfiguration();
///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded)
m_dispatcher = new CollisionDispatcher(m_collisionConfiguration);
m_broadphase = new DbvtBroadphase();
IOverlappingPairCache pairCache = null;
//pairCache = new SortedOverlappingPairCache();
m_broadphase = new SimpleBroadphase(1000, pairCache);
///the default constraint solver. For parallel processing you can use a different solver (see Extras/BulletMultiThreaded)
SequentialImpulseConstraintSolver sol = new SequentialImpulseConstraintSolver();
m_constraintSolver = sol;
m_dynamicsWorld = new DiscreteDynamicsWorld(m_dispatcher, m_broadphase, m_constraintSolver, m_collisionConfiguration);
IndexedVector3 gravity = new IndexedVector3(0, -10, 0);
m_dynamicsWorld.SetGravity(ref gravity);
///create a few basic rigid bodies
IndexedVector3 halfExtents = new IndexedVector3(50, 50, 50);
//IndexedVector3 halfExtents = new IndexedVector3(10, 10, 10);
//CollisionShape groundShape = new BoxShape(ref halfExtents);
//CollisionShape groundShape = new StaticPlaneShape(IndexedVector3.Up, 50);
CollisionShape groundShape = BuildLargeMesh();
m_collisionShapes.Add(groundShape);
IndexedMatrix groundTransform = IndexedMatrix.CreateTranslation(new IndexedVector3(0, 0, 0));
//IndexedMatrix groundTransform = IndexedMatrix.CreateTranslation(new IndexedVector3(0,-10,0));
//IndexedMatrix rotateMatrix = IndexedMatrix.CreateFromYawPitchRoll(0, MathUtil.SIMD_PI / 2.0f, 0);
//IndexedMatrix rotateMatrix = IndexedMatrix.Identity;
IndexedMatrix rotateMatrix = IndexedMatrix.Identity;
rotateMatrix._basis.SetEulerZYX(0, 0, MathUtil.SIMD_PI * 0.7f);
rotateMatrix._origin = IndexedVector3.Zero;
float mass = 0f;
LocalCreateRigidBody(mass, ref rotateMatrix, groundShape);
CollisionShape boxShape = new BoxShape(new IndexedVector3(0.2f, 0.2f, 0.2f));
//CollisionShape boxShape = new SphereShape(0.2f);
//CollisionShape boxShape = new CylinderShapeX(new IndexedVector3(0.2f, 0.4f, 0.2f));
//CollisionShape boxShape = new CapsuleShape(0.2f, 0.4f);
IndexedMatrix boxTransform = IndexedMatrix.Identity;
boxTransform._basis.SetEulerZYX(MathUtil.SIMD_PI * 0.2f, MathUtil.SIMD_PI * 0.4f, MathUtil.SIMD_PI * 0.7f);
boxTransform._origin = new IndexedVector3(0.0f, 5.0f, 0.0f);
LocalCreateRigidBody(1.25f, boxTransform, boxShape);
ClientResetScene();
}
public override void InitializeDemo()
{
base.InitializeDemo();
SetCameraDistance(50f);
//string filename = @"E:\users\man\bullet\xna-basic-output-1.txt";
//FileStream filestream = File.Open(filename, FileMode.Create, FileAccess.Write, FileShare.Read);
//BulletGlobals.g_streamWriter = new StreamWriter(filestream);
///collision configuration contains default setup for memory, collision setup
m_collisionConfiguration = new DefaultCollisionConfiguration();
///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded)
m_dispatcher = new CollisionDispatcher(m_collisionConfiguration);
IndexedVector3 worldMin = new IndexedVector3(-1000, -1000, -1000);
IndexedVector3 worldMax = -worldMin;
m_broadphase = new AxisSweep3Internal(ref worldMin, ref worldMax, 0xfffe, 0xffff, 16384, null, false);
//pairCache = new SortedOverlappingPairCache();
//m_broadphase = new SimpleBroadphase(1000, pairCache);
///the default constraint solver. For parallel processing you can use a different solver (see Extras/BulletMultiThreaded)
SequentialImpulseConstraintSolver sol = new SequentialImpulseConstraintSolver();
m_constraintSolver = sol;
m_dynamicsWorld = new DiscreteDynamicsWorld(m_dispatcher, m_broadphase, m_constraintSolver, m_collisionConfiguration);
m_dynamicsWorld.GetDispatchInfo().SetAllowedCcdPenetration(0.0001f);
IndexedVector3 gravity = new IndexedVector3(0, -10, 0);
m_dynamicsWorld.SetGravity(ref gravity);
///create a few basic rigid bodies
IndexedVector3 halfExtents = new IndexedVector3(50, 50, 50);
//IndexedVector3 halfExtents = new IndexedVector3(10, 10, 10);
CollisionShape groundShape = new BoxShape(ref halfExtents);
//CollisionShape groundShape = new StaticPlaneShape(new IndexedVector3(0,1,0), 50);
m_collisionShapes.Add(groundShape);
IndexedMatrix groundTransform = IndexedMatrix.CreateTranslation(new IndexedVector3(0, -50, 0));
//IndexedMatrix groundTransform = IndexedMatrix.CreateTranslation(new IndexedVector3(0,-10,0));
float mass = 0f;
LocalCreateRigidBody(mass, ref groundTransform, groundShape);
#region CharacterController
IndexedMatrix startTransform = IndexedMatrix.Identity;
//startTransform.setOrigin (btVector3(0.0, 4.0, 0.0));
startTransform._origin = new IndexedVector3(10.210098f, -1.6433364f, 16.453260f);
m_ghostObject = new PairCachingGhostObject();
m_ghostObject.SetWorldTransform(startTransform);
m_broadphase.GetOverlappingPairCache().SetInternalGhostPairCallback(new GhostPairCallback());
float characterHeight = 1.75f;
float characterWidth = 1.75f;
ConvexShape capsule = new CapsuleShape(characterWidth, characterHeight);
m_ghostObject.SetCollisionShape(capsule);
m_ghostObject.SetCollisionFlags(CollisionFlags.CF_CHARACTER_OBJECT);
float stepHeight = 0.35f;
int upAxis = 1;
m_character = new KinematicCharacterController(m_ghostObject, capsule, stepHeight, upAxis);
m_dynamicsWorld.AddCollisionObject(m_ghostObject, CollisionFilterGroups.CharacterFilter, CollisionFilterGroups.StaticFilter | CollisionFilterGroups.DefaultFilter);
m_dynamicsWorld.AddAction(m_character);
#endregion
}
public virtual void LoadHeightField(float[,] heights, float heightRange, uint vertsX, uint vertsZ, Vector3 loc, int metersPerSample)
{
//if (worldLoaded == false || metersPerSample > 2)
// return;
loc = loc / scaleFactor;
//heightRange = heightRange * 1000;
//loc.X += heightFieldOffset;
//loc.z += heightFieldOffset; // these axes are out by about 1m?
if (heightFields.ContainsKey(loc))
{
int oldMetersPerSample = heightFields[loc];
if (oldMetersPerSample == metersPerSample)
{
return; // no need to update
}
else
{
// we need to delete the old one to rebuild this one
foreach (CollisionObject a in m_dynamicsWorld.GetCollisionObjectArray())
{
if (a.GetCollisionShape() != null && a.GetCollisionShape().GetShapeType() == BroadphaseNativeTypes.TERRAIN_SHAPE_PROXYTYPE)
{
string terrainName = (string)a.GetUserPointer();
if (terrainName == "TestHeightField_" + loc)
{
a.Cleanup();
heightFields.Remove(loc);
System.Console.WriteLine("Removed heightmap at position '{0}' with metersPerSample: '{1}' and old: '{2}'",
loc, metersPerSample, oldMetersPerSample);
break;
}
}
}
}
}
byte[] terr = new byte[vertsX * vertsZ * 4];
MemoryStream file = new MemoryStream(terr);
BinaryWriter writer = new BinaryWriter(file);
for (int i = 0; i < vertsX; i++)
{
for (int j = 0; j < vertsZ; j++)
{
writer.Write((float)((heightRange / 2) + 4 * Math.Sin(j * 0.5f) * Math.Cos(i)));
//writer.Write(0f);
}
}
writer.Flush();
file.Position = 0;
float heightScale = heightRange / 32767f / scaleFactor;
int upAxis = 1;
CollisionShape terrainShape = new HeightfieldTerrainShape((int)vertsX, (int)vertsZ, terr, heightScale, 0, heightRange, upAxis, PHY_ScalarType.PHY_FLOAT, true);
IndexedMatrix worldTransform = IndexedMatrix.CreateTranslation(loc);
DefaultMotionState objectMotionState = new DefaultMotionState(worldTransform, IndexedMatrix.Identity);
//terrainShape = new StaticPlaneShape(Vector3.Up, 0f);
//IndexedVector3 halfExtents = new IndexedVector3(50, 50, 50);
//terrainShape = new BoxShape(ref halfExtents);
RigidBodyConstructionInfo rbInfo = new RigidBodyConstructionInfo(0, objectMotionState, terrainShape);
RigidBody terrain = new RigidBody(rbInfo);
//IndexedMatrix groundTransform = IndexedMatrix.CreateTranslation(new IndexedVector3(0, -50, 0));
IndexedMatrix groundTransform = IndexedMatrix.CreateTranslation(new IndexedVector3(0, -5, 0));
LocalCreateRigidBody(0f, groundTransform, terrainShape);
terrain.SetUserPointer("TestHeightField_" + loc.ToString());
//terrain.SetCollisionFlags(CollisionFlags.CF_KINEMATIC_OBJECT);
////m_dynamicsWorld.AddCollisionObject(terrain, CollisionFilterGroups.DefaultFilter, CollisionFilterGroups.AllFilter);
//m_dynamicsWorld.AddCollisionObject(terrain);
System.Console.WriteLine("Added heightmap at position: '{0}' with metersPerSample: '{1}'", loc, metersPerSample);
heightFields.Add(loc, metersPerSample);
}
//-----------------------------------------------------------------------------------------------
public override void InitializeDemo()
{
//string filename = @"C:\users\man\bullett\xna-concave-output.txt";
//FileStream filestream = File.Open(filename, FileMode.Create, FileAccess.Write, FileShare.Read);
//BulletGlobals.g_streamWriter = new StreamWriter(filestream);
m_animatedMesh = true;
base.InitializeDemo();
int totalTriangles = 2 * (NUM_VERTS_X - 1) * (NUM_VERTS_Y - 1);
gVertices = new ObjectArray <IndexedVector3>(totalVerts);
int indicesTotal = totalTriangles * 3;
gIndices = new ObjectArray <int>(indicesTotal);
//BulletGlobals.gContactAddedCallback = new CustomMaterialCombinerCallback();
SetVertexPositions(waveheight, 0f);
int vertStride = 1;
int indexStride = 3;
int index = 0;
for (int i = 0; i < NUM_VERTS_X - 1; i++)
{
for (int j = 0; j < NUM_VERTS_Y - 1; j++)
{
gIndices[index++] = j * NUM_VERTS_X + i;
gIndices[index++] = j * NUM_VERTS_X + i + 1;
gIndices[index++] = (j + 1) * NUM_VERTS_X + i + 1;
gIndices[index++] = j * NUM_VERTS_X + i;
gIndices[index++] = (j + 1) * NUM_VERTS_X + i + 1;
gIndices[index++] = (j + 1) * NUM_VERTS_X + i;
}
}
if (BulletGlobals.g_streamWriter != null)
{
index = 0;
BulletGlobals.g_streamWriter.WriteLine("setIndexPositions");
for (int i = 0; i < gIndices.Count; i++)
{
BulletGlobals.g_streamWriter.WriteLine(String.Format("{0} {1}", i, gIndices[i]));
}
}
TriangleIndexVertexArray indexVertexArrays = new TriangleIndexVertexArray(totalTriangles,
gIndices, indexStride, totalVerts, gVertices, vertStride);
bool useQuantizedAabbCompression = true;
OptimizedBvh bvh = new OptimizedBvh();
IndexedVector3 aabbMin = new IndexedVector3(-1000, -1000, -1000);
IndexedVector3 aabbMax = new IndexedVector3(1000, 1000, 1000);
m_trimeshShape = new BvhTriangleMeshShape(indexVertexArrays, useQuantizedAabbCompression, ref aabbMin, ref aabbMax, true);
//CollisionShape trimeshShape = new TriangleMeshShape(indexVertexArrays);
IndexedVector3 scaling = IndexedVector3.One;
//m_trimeshShape.SetOptimizedBvh(bvh, ref scaling);
//BulletWorldImporter import = new BulletWorldImporter(0);//don't store info into the world
//if (import.loadFile("myShape.bullet"))
//{
// int numBvh = import.getNumBvhs();
// if (numBvh != 0)
// {
// OptimizedBvh bvh = import.getBvhByIndex(0);
// IndexedVector3 aabbMin = new IndexedVector3(-1000,-1000,-1000);
// IndexedVector3 aabbMax = new IndexedVector3(1000,1000,1000);
// trimeshShape = new indexVertexArrays,useQuantizedAabbCompression,ref aabbMin,ref aabbMax,false);
// IndexedVector3 scaling = IndexedVector3.One;
// trimeshShape.setOptimizedBvh(bvh, ref scaling);
// //trimeshShape = new btBvhTriangleMeshShape(m_indexVertexArrays,useQuantizedAabbCompression,aabbMin,aabbMax);
// //trimeshShape.setOptimizedBvh(bvh);
// }
// int numShape = import.getNumCollisionShapes();
// if (numShape != 0)
// {
// trimeshShape = (BvhTriangleMeshShape)import.getCollisionShapeByIndex(0);
// //if you know the name, you can also try to get the shape by name:
// String meshName = import.getNameForPointer(trimeshShape);
// if (meshName != null)
// {
// trimeshShape = (BvhTriangleMeshShape)import.getCollisionShapeByName(meshName);
// }
// }
//}
//CollisionShape groundShape = trimeshShape;//m_trimeshShape;
CollisionShape groundShape = m_trimeshShape;//m_trimeshShape;
//groundShape = new TriangleShape(new IndexedVector3(0,` 0, 100), new IndexedVector3(100, 0, 0),new IndexedVector3(-100, 0, -100));
//groundShape = new StaticPlaneShape(IndexedVector3.Up, 0f);
//groundShape = new BoxShape(new IndexedVector3(100f, 0.1f, 100f));
IndexedVector3 up = new IndexedVector3(0.4f, 1, 0);
up.Normalize();
//groundShape = new StaticPlaneShape(up, 0f);
//groundShape = new TriangleMeshShape(indexVertexArrays);
m_collisionConfiguration = new DefaultCollisionConfiguration();
m_dispatcher = new CollisionDispatcher(m_collisionConfiguration);
IndexedVector3 worldMin = new IndexedVector3(-1000, -1000, -1000);
IndexedVector3 worldMax = new IndexedVector3(1000, 1000, 1000);
//m_broadphase = new AxisSweep3Internal(ref worldMin, ref worldMax, 0xfffe, 0xffff, 16384, null, false);
m_broadphase = new DbvtBroadphase();
m_constraintSolver = new SequentialImpulseConstraintSolver();
m_dynamicsWorld = new DiscreteDynamicsWorld(m_dispatcher, m_broadphase, m_constraintSolver, m_collisionConfiguration);
float mass = 0f;
IndexedMatrix startTransform = IndexedMatrix.CreateTranslation(new IndexedVector3(2, -2, 0));
//CompoundShape colShape = new CompoundShape();
//IndexedVector3 halfExtents = new IndexedVector3(4, 1, 1);
//CollisionShape cylinderShape = new CylinderShapeX(ref halfExtents);
//CollisionShape boxShape = new BoxShape(new IndexedVector3(4, 1, 1));
//IndexedMatrix localTransform = IndexedMatrix.Identity;
//colShape.addChildShape(ref localTransform, boxShape);
//Quaternion orn = Quaternion.CreateFromYawPitchRoll(MathUtil.SIMD_HALF_PI, 0f, 0f);
//localTransform = IndexedMatrix.CreateFromQuaternion(orn);
//colShape.addChildShape(ref localTransform, cylinderShape);
////BoxShape colShape = new BoxShape(new IndexedVector3(1, 1, 1));
//int numCollideObjects = 1;
//m_collisionShapes.Add(colShape);
//{
// for (int i = 0; i < numCollideObjects; i++)
// {
// startTransform._origin = new IndexedVector3(4,10+i*2,1);
// localCreateRigidBody(1, ref startTransform,colShape);
// }
//}
CollisionShape boxShape = new BoxShape(new IndexedVector3(1, 1, 1));
//CollisionShape boxShape = new SphereShape(1);
//CollisionShape boxShape = new SphereShape(1);
//CollisionShape boxShape = new CapsuleShapeZ(0.5f, 1);
m_collisionShapes.Add(boxShape);
for (int i = 0; i < 1; i++)
{
startTransform._origin = new IndexedVector3(2f * i, 5, 1);
LocalCreateRigidBody(1, ref startTransform, boxShape);
}
startTransform = IndexedMatrix.Identity;
staticBody = LocalCreateRigidBody(mass, ref startTransform, groundShape);
staticBody.SetCollisionFlags(staticBody.GetCollisionFlags() | CollisionFlags.CF_KINEMATIC_OBJECT); //STATIC_OBJECT);
//enable custom material callback
staticBody.SetCollisionFlags(staticBody.GetCollisionFlags() | CollisionFlags.CF_CUSTOM_MATERIAL_CALLBACK);
//clientResetScene();
}
public void DropBall()
{
RigidBody rb = LocalCreateRigidBody(1f, IndexedMatrix.CreateTranslation(m_ballDropSpot), m_dropSphereShape);
rb.SetLinearFactor(new IndexedVector3(1, 1, 0));
}
public void Warp(ref IndexedVector3 origin)
{
IndexedMatrix m = IndexedMatrix.CreateTranslation(origin);
m_ghostObject.SetWorldTransform(ref m);
}
public RagDoll(RagDollDemo ragDollDemo, DynamicsWorld ownerWorld, ref IndexedVector3 positionOffset, StreamWriter streamWriter)
{
m_ownerWorld = ownerWorld;
m_ragDollDemo = ragDollDemo;
// Setup the geometry
m_shapes[(int)BODYPART.PELVIS] = new CapsuleShape(0.15f, 0.20f);
m_shapes[(int)BODYPART.SPINE] = new CapsuleShape(0.15f, 0.28f);
m_shapes[(int)BODYPART.HEAD] = new CapsuleShape(0.10f, 0.05f);
m_shapes[(int)BODYPART.LEFT_UPPER_LEG] = new CapsuleShape(0.07f, 0.45f);
m_shapes[(int)BODYPART.LEFT_LOWER_LEG] = new CapsuleShape(0.05f, 0.37f);
m_shapes[(int)BODYPART.RIGHT_UPPER_LEG] = new CapsuleShape(0.07f, 0.45f);
m_shapes[(int)BODYPART.RIGHT_LOWER_LEG] = new CapsuleShape(0.05f, 0.37f);
m_shapes[(int)BODYPART.LEFT_UPPER_ARM] = new CapsuleShape(0.05f, 0.33f);
m_shapes[(int)BODYPART.LEFT_LOWER_ARM] = new CapsuleShape(0.04f, 0.25f);
m_shapes[(int)BODYPART.RIGHT_UPPER_ARM] = new CapsuleShape(0.05f, 0.33f);
m_shapes[(int)BODYPART.RIGHT_LOWER_ARM] = new CapsuleShape(0.04f, 0.25f);
// Setup all the rigid bodies
IndexedMatrix offset = IndexedMatrix.CreateTranslation(positionOffset);
IndexedMatrix transform = IndexedMatrix.CreateTranslation(new IndexedVector3(0, 1, 0));
IndexedMatrix adjusted = offset * transform;
m_bodies[(int)BODYPART.PELVIS] = m_ragDollDemo.LocalCreateRigidBody(1f, adjusted, m_shapes[(int)BODYPART.PELVIS], true);
m_bodies[(int)BODYPART.PELVIS].SetUserPointer("PELVIS");
transform = IndexedMatrix.CreateTranslation(new IndexedVector3(0, 1.2f, 0));
adjusted = offset * transform;
m_bodies[(int)BODYPART.SPINE] = m_ragDollDemo.LocalCreateRigidBody(1f, adjusted, m_shapes[(int)BODYPART.SPINE], true);
m_bodies[(int)BODYPART.SPINE].SetUserPointer("SPINE");
transform = IndexedMatrix.CreateTranslation(new IndexedVector3(0, 1.6f, 0));
adjusted = offset * transform;
m_bodies[(int)BODYPART.HEAD] = m_ragDollDemo.LocalCreateRigidBody(1f, adjusted, m_shapes[(int)BODYPART.HEAD], true);
m_bodies[(int)BODYPART.HEAD].SetUserPointer("HEAD");
transform = IndexedMatrix.CreateTranslation(new IndexedVector3(-0.18f, 0.65f, 0));
adjusted = offset * transform
;
m_bodies[(int)BODYPART.LEFT_UPPER_LEG] = m_ragDollDemo.LocalCreateRigidBody(1f, adjusted, m_shapes[(int)BODYPART.LEFT_UPPER_LEG], true);
m_bodies[(int)BODYPART.LEFT_UPPER_LEG].SetUserPointer("LEFTUPPERLEG");
transform = IndexedMatrix.CreateTranslation(new IndexedVector3(-0.18f, 0.2f, 0));
adjusted = offset * transform;
m_bodies[(int)BODYPART.LEFT_LOWER_LEG] = m_ragDollDemo.LocalCreateRigidBody(1f, adjusted, m_shapes[(int)BODYPART.LEFT_LOWER_LEG], true);
m_bodies[(int)BODYPART.LEFT_LOWER_LEG].SetUserPointer("LEFTLOWERLEG");
transform = IndexedMatrix.CreateTranslation(new IndexedVector3(0.18f, 0.65f, 0));
adjusted = offset * transform;
m_bodies[(int)BODYPART.RIGHT_UPPER_LEG] = m_ragDollDemo.LocalCreateRigidBody(1f, adjusted, m_shapes[(int)BODYPART.RIGHT_UPPER_LEG], true);
m_bodies[(int)BODYPART.RIGHT_UPPER_LEG].SetUserPointer("RIGHTUPPERLEG");
transform = IndexedMatrix.CreateTranslation(new IndexedVector3(0.18f, 0.2f, 0));
adjusted = offset * transform;
m_bodies[(int)BODYPART.RIGHT_LOWER_LEG] = m_ragDollDemo.LocalCreateRigidBody(1f, adjusted, m_shapes[(int)BODYPART.RIGHT_LOWER_LEG], true);
m_bodies[(int)BODYPART.RIGHT_LOWER_LEG].SetUserPointer("RIGHTLOWERLEG");
transform = MathUtil.SetEulerZYX(0, 0, MathUtil.SIMD_HALF_PI);
transform._origin = new IndexedVector3(-0.35f, 1.45f, 0);
adjusted = offset * transform;
m_bodies[(int)BODYPART.LEFT_UPPER_ARM] = m_ragDollDemo.LocalCreateRigidBody(1f, adjusted, m_shapes[(int)BODYPART.LEFT_UPPER_ARM], true);
m_bodies[(int)BODYPART.LEFT_UPPER_ARM].SetUserPointer("LEFTUPPERARM");
transform = MathUtil.SetEulerZYX(0, 0, MathUtil.SIMD_HALF_PI);
transform._origin = new IndexedVector3(-0.7f, 1.45f, 0);
adjusted = offset * transform;
m_bodies[(int)BODYPART.LEFT_LOWER_ARM] = m_ragDollDemo.LocalCreateRigidBody(1f, adjusted, m_shapes[(int)BODYPART.LEFT_LOWER_ARM], true);
m_bodies[(int)BODYPART.LEFT_LOWER_ARM].SetUserPointer("LEFTLOWERARM");
transform = MathUtil.SetEulerZYX(0, 0, -MathUtil.SIMD_HALF_PI);
transform._origin = new IndexedVector3(0.35f, 1.45f, 0);
adjusted = offset * transform;
m_bodies[(int)BODYPART.RIGHT_UPPER_ARM] = m_ragDollDemo.LocalCreateRigidBody(1f, adjusted, m_shapes[(int)BODYPART.RIGHT_UPPER_ARM], true);
m_bodies[(int)BODYPART.RIGHT_UPPER_ARM].SetUserPointer("RIGHTUPPERARM");
transform = MathUtil.SetEulerZYX(0, 0, -MathUtil.SIMD_HALF_PI);
transform._origin = new IndexedVector3(0.7f, 1.45f, 0);
adjusted = offset * transform;
m_bodies[(int)BODYPART.RIGHT_LOWER_ARM] = m_ragDollDemo.LocalCreateRigidBody(1f, adjusted, m_shapes[(int)BODYPART.RIGHT_LOWER_ARM], true);
m_bodies[(int)BODYPART.RIGHT_LOWER_ARM].SetUserPointer("RIGHTLOWERARM");
// Setup some damping on the m_bodies
for (int i = 0; i < (int)BODYPART.COUNT; ++i)
{
if (m_bodies[i] != null)
{
//m_bodies[i].SetDamping(0.05f, 0.85f);
m_bodies[i].SetDamping(0.5f, 0.85f);
m_bodies[i].SetDeactivationTime(0.8f);
m_bodies[i].SetSleepingThresholds(1.6f, 2.5f);
}
}
// Now setup the constraints
HingeConstraint hingeC;
ConeTwistConstraint coneC;
IndexedMatrix localA = IndexedMatrix.Identity;
IndexedMatrix localB = IndexedMatrix.Identity;
localA = MathUtil.SetEulerZYX(0, MathUtil.SIMD_HALF_PI, 0);
localA._origin = new IndexedVector3(0.0f, 0.15f, 0.0f);
localB = MathUtil.SetEulerZYX(0, MathUtil.SIMD_HALF_PI, 0);
localB._origin = new IndexedVector3(0.0f, -0.15f, 0.0f);
hingeC = new HingeConstraint(m_bodies[(int)BODYPART.PELVIS], m_bodies[(int)BODYPART.SPINE], ref localA, ref localB);
hingeC.SetLimit(-MathUtil.SIMD_QUARTER_PI, MathUtil.SIMD_HALF_PI);
m_joints[(int)JOINT.PELVIS_SPINE] = hingeC;
m_joints[(int)JOINT.PELVIS_SPINE].m_debugName = "PELVIS_SPINE";
hingeC.SetDbgDrawSize(CONSTRAINT_DEBUG_SIZE);
m_ownerWorld.AddConstraint(m_joints[(int)JOINT.PELVIS_SPINE], true);
localA = MathUtil.SetEulerZYX(0, 0, MathUtil.SIMD_HALF_PI);
localA._origin = new IndexedVector3(0.0f, 0.30f, 0.0f);
localB = MathUtil.SetEulerZYX(0, 0, MathUtil.SIMD_HALF_PI);
localB._origin = new IndexedVector3(0.0f, -0.14f, 0.0f);
coneC = new ConeTwistConstraint(m_bodies[(int)BODYPART.SPINE], m_bodies[(int)BODYPART.HEAD], ref localA, ref localB);
coneC.SetLimit(MathUtil.SIMD_QUARTER_PI, MathUtil.SIMD_QUARTER_PI, MathUtil.SIMD_HALF_PI);
m_joints[(int)JOINT.SPINE_HEAD] = coneC;
m_joints[(int)JOINT.SPINE_HEAD].m_debugName = "SPINE_HEAD";
coneC.SetDbgDrawSize(CONSTRAINT_DEBUG_SIZE);
m_ownerWorld.AddConstraint(m_joints[(int)JOINT.SPINE_HEAD], true);
localA = IndexedMatrix.Identity;
localB = IndexedMatrix.Identity;
localA = MathUtil.SetEulerZYX(0, 0, -MathUtil.SIMD_QUARTER_PI * 5);
localA._origin = new IndexedVector3(-0.18f, -0.10f, 0.0f);
localB = MathUtil.SetEulerZYX(0, 0, -MathUtil.SIMD_QUARTER_PI * 5);
localB._origin = new IndexedVector3(0.0f, 0.225f, 0.0f);
coneC = new ConeTwistConstraint(m_bodies[(int)BODYPART.PELVIS], m_bodies[(int)BODYPART.LEFT_UPPER_LEG], ref localA, ref localB);
coneC.SetLimit(MathUtil.SIMD_QUARTER_PI, MathUtil.SIMD_QUARTER_PI, 0);
m_joints[(int)JOINT.LEFT_HIP] = coneC;
m_joints[(int)JOINT.LEFT_HIP].m_debugName = "LEFT_HIP";
coneC.SetDbgDrawSize(CONSTRAINT_DEBUG_SIZE);
m_ownerWorld.AddConstraint(m_joints[(int)JOINT.LEFT_HIP], true);
localA = MathUtil.SetEulerZYX(0f, MathUtil.SIMD_HALF_PI, 0f);
localA._origin = new IndexedVector3(0.0f, -0.225f, 0.0f);
localB = MathUtil.SetEulerZYX(0, MathUtil.SIMD_HALF_PI, 0);
localB._origin = new IndexedVector3(0.0f, 0.185f, 0.0f);
hingeC = new HingeConstraint(m_bodies[(int)BODYPART.LEFT_UPPER_LEG], m_bodies[(int)BODYPART.LEFT_LOWER_LEG], ref localA, ref localB);
hingeC.SetLimit(0, MathUtil.SIMD_HALF_PI);
m_joints[(int)JOINT.LEFT_KNEE] = hingeC;
m_joints[(int)JOINT.LEFT_KNEE].m_debugName = "LEFT_KNEE";
hingeC.SetDbgDrawSize(CONSTRAINT_DEBUG_SIZE);
m_ownerWorld.AddConstraint(m_joints[(int)JOINT.LEFT_KNEE], true);
localA = MathUtil.SetEulerZYX(0, 0, MathUtil.SIMD_QUARTER_PI);
localA._origin = new IndexedVector3(0.18f, -0.10f, 0.0f);
localB = MathUtil.SetEulerZYX(0, 0, MathUtil.SIMD_QUARTER_PI);
localB._origin = new IndexedVector3(0.0f, 0.225f, 0.0f);
coneC = new ConeTwistConstraint(m_bodies[(int)BODYPART.PELVIS], m_bodies[(int)BODYPART.RIGHT_UPPER_LEG], ref localA, ref localB);
coneC.SetLimit(MathUtil.SIMD_QUARTER_PI, MathUtil.SIMD_QUARTER_PI, 0);
m_joints[(int)JOINT.RIGHT_HIP] = coneC;
m_joints[(int)JOINT.RIGHT_HIP].m_debugName = "RIGHT_HIP";
coneC.SetDbgDrawSize(CONSTRAINT_DEBUG_SIZE);
m_ownerWorld.AddConstraint(m_joints[(int)JOINT.RIGHT_HIP], true);
localA = MathUtil.SetEulerZYX(0, MathUtil.SIMD_HALF_PI, 0);
localA._origin = new IndexedVector3(0.0f, -0.225f, 0.0f);
localB = MathUtil.SetEulerZYX(0, MathUtil.SIMD_HALF_PI, 0);
localB._origin = new IndexedVector3(0.0f, 0.185f, 0.0f);
hingeC = new HingeConstraint(m_bodies[(int)BODYPART.RIGHT_UPPER_LEG], m_bodies[(int)BODYPART.RIGHT_LOWER_LEG], ref localA, ref localB);
hingeC.SetLimit(0, MathUtil.SIMD_HALF_PI);
m_joints[(int)JOINT.RIGHT_KNEE] = hingeC;
m_joints[(int)JOINT.RIGHT_KNEE].m_debugName = "RIGHT_KNEE";
hingeC.SetDbgDrawSize(CONSTRAINT_DEBUG_SIZE);
m_ownerWorld.AddConstraint(m_joints[(int)JOINT.RIGHT_KNEE], true);
localA = MathUtil.SetEulerZYX(0, 0, MathUtil.SIMD_PI);
localA._origin = new IndexedVector3(-0.2f, 0.15f, 0.0f);
localB = MathUtil.SetEulerZYX(0, 0, MathUtil.SIMD_HALF_PI);
localB._origin = new IndexedVector3(0.0f, -0.18f, 0.0f);
coneC = new ConeTwistConstraint(m_bodies[(int)BODYPART.SPINE], m_bodies[(int)BODYPART.LEFT_UPPER_ARM], ref localA, ref localB);
coneC.SetLimit(MathUtil.SIMD_HALF_PI, MathUtil.SIMD_HALF_PI, 0);
coneC.SetDbgDrawSize(CONSTRAINT_DEBUG_SIZE);
m_joints[(int)JOINT.LEFT_SHOULDER] = coneC;
m_joints[(int)JOINT.LEFT_SHOULDER].m_debugName = "LEFT_SHOULDER";
m_ownerWorld.AddConstraint(m_joints[(int)JOINT.LEFT_SHOULDER], true);
localA = MathUtil.SetEulerZYX(0, MathUtil.SIMD_HALF_PI, 0);
localA._origin = new IndexedVector3(0.0f, 0.18f, 0.0f);
localB = MathUtil.SetEulerZYX(0, MathUtil.SIMD_HALF_PI, 0);
localB._origin = new IndexedVector3(0.0f, -0.14f, 0.0f);
hingeC = new HingeConstraint(m_bodies[(int)BODYPART.LEFT_UPPER_ARM], m_bodies[(int)BODYPART.LEFT_LOWER_ARM], ref localA, ref localB);
// hingeC.setLimit(-MathUtil.SIMD_HALF_PI), 0));
hingeC.SetLimit(0, MathUtil.SIMD_HALF_PI);
m_joints[(int)JOINT.LEFT_ELBOW] = hingeC;
m_joints[(int)JOINT.LEFT_ELBOW].m_debugName = "LEFT_ELBOW";
hingeC.SetDbgDrawSize(CONSTRAINT_DEBUG_SIZE);
m_ownerWorld.AddConstraint(m_joints[(int)JOINT.LEFT_ELBOW], true);
localA = MathUtil.SetEulerZYX(0, 0, 0);
localA._origin = new IndexedVector3(0.2f, 0.15f, 0.0f);
localB = MathUtil.SetEulerZYX(0, 0, MathUtil.SIMD_HALF_PI);
localB._origin = new IndexedVector3(0.0f, -0.18f, 0.0f);
coneC = new ConeTwistConstraint(m_bodies[(int)BODYPART.SPINE], m_bodies[(int)BODYPART.RIGHT_UPPER_ARM], ref localA, ref localB);
coneC.SetLimit(MathUtil.SIMD_HALF_PI, MathUtil.SIMD_HALF_PI, 0);
m_joints[(int)JOINT.RIGHT_SHOULDER] = coneC;
m_joints[(int)JOINT.RIGHT_SHOULDER].m_debugName = "RIGHT_SHOULDER";
coneC.SetDbgDrawSize(CONSTRAINT_DEBUG_SIZE);
m_ownerWorld.AddConstraint(m_joints[(int)JOINT.RIGHT_SHOULDER], true);
localA = MathUtil.SetEulerZYX(0, MathUtil.SIMD_HALF_PI, 0);
localA._origin = new IndexedVector3(0.0f, 0.18f, 0.0f);
localB = MathUtil.SetEulerZYX(0, MathUtil.SIMD_HALF_PI, 0);
localB._origin = new IndexedVector3(0.0f, -0.14f, 0.0f);
hingeC = new HingeConstraint(m_bodies[(int)BODYPART.RIGHT_UPPER_ARM], m_bodies[(int)BODYPART.RIGHT_LOWER_ARM], ref localA, ref localB);
// hingeC.setLimit(-MathUtil.SIMD_HALF_PI), 0));
hingeC.SetLimit(0, MathUtil.SIMD_HALF_PI);
m_joints[(int)JOINT.RIGHT_ELBOW] = hingeC;
m_joints[(int)JOINT.RIGHT_ELBOW].m_debugName = "RIGHT_ELBOW";
hingeC.SetDbgDrawSize(CONSTRAINT_DEBUG_SIZE);
m_ownerWorld.AddConstraint(m_joints[(int)JOINT.RIGHT_ELBOW], true);
}
public override void InitializeDemo()
{
//maxiterations = 10;
SetCameraDistance(SCALING * 50f);
//string filename = @"E:\users\man\bullet\xna-basic-output-1.txt";
//FileStream filestream = File.Open(filename, FileMode.Create, FileAccess.Write, FileShare.Read);
//BulletGlobals.g_streamWriter = new StreamWriter(filestream);
///collision configuration contains default setup for memory, collision setup
m_collisionConfiguration = new DefaultCollisionConfiguration();
///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded)
m_dispatcher = new CollisionDispatcher(m_collisionConfiguration);
IndexedVector3 worldMin = new IndexedVector3(-1000, -1000, -1000);
IndexedVector3 worldMax = -worldMin;
m_broadphase = new AxisSweep3Internal(ref worldMin, ref worldMax, 0xfffe, 0xffff, 16384, null, false);
IOverlappingPairCache pairCache = null;
//pairCache = new SortedOverlappingPairCache();
pairCache = new HashedOverlappingPairCache();
m_broadphase = new DbvtBroadphase(pairCache);
BulletGlobals.gDebugDraw = Game1.Instance.shape_drawer;
//m_broadphase = new SimpleBroadphase(1000, pairCache);
///the default constraint solver. For parallel processing you can use a different solver (see Extras/BulletMultiThreaded)
SequentialImpulseConstraintSolver sol = new SequentialImpulseConstraintSolver();
m_constraintSolver = sol;
m_dynamicsWorld = new DiscreteDynamicsWorld(m_dispatcher, m_broadphase, m_constraintSolver, m_collisionConfiguration);
IndexedVector3 gravity = new IndexedVector3(0, -10, 0);
m_dynamicsWorld.SetGravity(ref gravity);
///create a few basic rigid bodies
IndexedVector3 halfExtents = new IndexedVector3(50, 50, 50);
//IndexedVector3 halfExtents = new IndexedVector3(10, 10, 10);
CollisionShape groundShape = new BoxShape(ref halfExtents);
//CollisionShape groundShape = new StaticPlaneShape(new IndexedVector3(0,1,0), 50);
m_collisionShapes.Add(groundShape);
IndexedMatrix groundTransform = IndexedMatrix.CreateTranslation(new IndexedVector3(0, -50, 0));
float mass = 0f;
float topY = 2.5f;
float bottomY = 0.5f;
float diff = 10f;
float left = -(diff / 2f);
float right = -left;
LocalCreateRigidBody(mass, ref groundTransform, groundShape);
{
/// Create Dynamic Objects
IndexedMatrix startTransform = IndexedMatrix.Identity;
mass = 0f;
//rigidbody is dynamic if and only if mass is non zero, otherwise static
bool isDynamic = mass != 0f;
RigidBody rb = null;
//startTransform._origin = new IndexedVector3(left, topY, 0);
//collisionTopLeftCorner = BuildCorner(vertices, topLeft);
//rb = LocalCreateRigidBody(0f, startTransform, collisionTopLeftCorner);
//rb.SetUserPointer("TopLeftCorner");
//startTransform._origin = new IndexedVector3(right, topY, 0);
//collisionTopRightCorner = BuildCorner(vertices, topRight);
//rb = LocalCreateRigidBody(0f, startTransform, collisionTopRightCorner);
//rb.SetUserPointer("TopRightCorner");
startTransform._origin = new IndexedVector3(left, bottomY, 0);
collisionBottomLeftCorner = BuildCorner(vertices, bottomLeft);
rb = LocalCreateRigidBody(0f, startTransform, collisionBottomLeftCorner);
rb.SetUserPointer("BottomLeftCorner");
startTransform._origin = new IndexedVector3(right, bottomY, 0);
collisionBottomRightCorner = BuildCorner(vertices, bottomRight);
rb = LocalCreateRigidBody(0f, startTransform, collisionBottomRightCorner);
rb.SetUserPointer("BottomRightCorner");
startTransform._origin = IndexedVector3.Zero;
m_playerSphere = LocalCreateRigidBody(1f, startTransform, new SphereShape(0.25f));
m_playerSphere.SetActivationState(ActivationState.DISABLE_DEACTIVATION);
}
BulletGlobals.gDebugDraw.SetDebugMode(BulletMonogame.LinearMath.DebugDrawModes.DBG_DrawAabb | BulletMonogame.LinearMath.DebugDrawModes.DBG_DrawNormals | BulletMonogame.LinearMath.DebugDrawModes.DBG_DrawContactPoints);
m_dynamicsWorld.SetDebugDrawer(BulletGlobals.gDebugDraw);
//ClientResetScene();
}
public override void InitializeDemo()
{
SetTexturing(true);
SetShadows(true);
SetCameraDistance(SCALING * 50.0f);
m_cameraTargetPosition = IndexedVector3.Zero;
//string filename = @"E:\users\man\bullet\xna-box2d-output.txt";
//FileStream filestream = File.Open(filename, FileMode.Create, FileAccess.Write, FileShare.Read);
//BulletGlobals.g_streamWriter = new StreamWriter(filestream);
///collision configuration contains default setup for memory, collision setup
m_collisionConfiguration = new DefaultCollisionConfiguration();
//m_collisionConfiguration.setConvexConvexMultipointIterations();
///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded)
m_dispatcher = new CollisionDispatcher(m_collisionConfiguration);
VoronoiSimplexSolver simplex = new VoronoiSimplexSolver();
MinkowskiPenetrationDepthSolver pdSolver = new MinkowskiPenetrationDepthSolver();
CollisionAlgorithmCreateFunc convexAlgo2d = new Convex2dConvex2dCreateFunc(simplex, pdSolver);
m_dispatcher.RegisterCollisionCreateFunc((int)BroadphaseNativeTypes.CONVEX_2D_SHAPE_PROXYTYPE, (int)BroadphaseNativeTypes.CONVEX_2D_SHAPE_PROXYTYPE, convexAlgo2d);
m_dispatcher.RegisterCollisionCreateFunc((int)BroadphaseNativeTypes.BOX_2D_SHAPE_PROXYTYPE, (int)BroadphaseNativeTypes.CONVEX_2D_SHAPE_PROXYTYPE, convexAlgo2d);
m_dispatcher.RegisterCollisionCreateFunc((int)BroadphaseNativeTypes.CONVEX_2D_SHAPE_PROXYTYPE, (int)BroadphaseNativeTypes.BOX_2D_SHAPE_PROXYTYPE, convexAlgo2d);
m_dispatcher.RegisterCollisionCreateFunc((int)BroadphaseNativeTypes.BOX_2D_SHAPE_PROXYTYPE, (int)BroadphaseNativeTypes.BOX_2D_SHAPE_PROXYTYPE, new Box2dBox2dCreateFunc());
//m_broadphase = new DbvtBroadphase();
m_broadphase = new SimpleBroadphase(1000, null);
///the default constraint solver. For parallel processing you can use a different solver (see Extras/BulletMultiThreaded)
m_constraintSolver = new SequentialImpulseConstraintSolver();
m_dynamicsWorld = new DiscreteDynamicsWorld(m_dispatcher, m_broadphase, m_constraintSolver, m_collisionConfiguration);
//m_dynamicsWorld.getSolverInfo().m_erp = 1.f;
//m_dynamicsWorld.getSolverInfo().m_numIterations = 4;
//m_dynamicsWorld.setGravity(new IndexedVector3(0,-10,0));
///create a few basic rigid bodies
CollisionShape groundShape = new BoxShape(new IndexedVector3(150, 50, 150));
// btCollisionShape* groundShape = new btStaticPlaneShape(IndexedVector3(0,1,0),50);
m_collisionShapes.Add(groundShape);
IndexedMatrix groundTransform = IndexedMatrix.CreateTranslation(0, -43, 0);
LocalCreateRigidBody(0, groundTransform, groundShape);
{
//create a few dynamic rigidbodies
// Re-using the same collision is better for memory usage and performance
float u = 1 * SCALING - 0.04f;
IList <IndexedVector3> points = new List <IndexedVector3>();
points.Add(new IndexedVector3(0, u, 0));
points.Add(new IndexedVector3(-u, -u, 0));
points.Add(new IndexedVector3(u, -u, 0));
ConvexShape boxShape = new Convex2dShape(new BoxShape(new IndexedVector3(SCALING, SCALING, 0.04f)));
//btCollisionShape* colShape = new btBox2dShape(IndexedVector3(SCALING*1,SCALING*1,0.04));
ConvexShape triangleShape = new Convex2dShape(new ConvexHullShape(points, 3));
IndexedVector3 extents = new IndexedVector3(SCALING, SCALING, 0.04f);
ConvexShape cylinderShape = new Convex2dShape(new CylinderShapeZ(ref extents));
//btUniformScalingShape* colShape = new btUniformScalingShape(convexColShape,1.f);
boxShape.SetMargin(0.03f);
//btCollisionShape* colShape = new btSphereShape(float(1.));
m_collisionShapes.Add(boxShape);
m_collisionShapes.Add(triangleShape);
/// Create Dynamic Objects
IndexedMatrix startTransform = IndexedMatrix.Identity;
float mass = 1.0f;;
//rigidbody is dynamic if and only if mass is non zero, otherwise static
bool isDynamic = (mass != 0.9f);
IndexedVector3 localInertia = IndexedVector3.Zero;
if (isDynamic)
{
boxShape.CalculateLocalInertia(mass, out localInertia);
}
// float start_x = START_POS_X - ARRAY_SIZE_X/2;
// float start_y = START_POS_Y;
// float start_z = START_POS_Z - ARRAY_SIZE_Z/2;
IndexedVector3 x = new IndexedVector3(-ARRAY_SIZE_X, 8f, -20f);
//IndexedVector3 y = IndexedVector3.Zero;
IndexedVector3 y = new IndexedVector3(0, 0, 0);
IndexedVector3 deltaX = new IndexedVector3(SCALING * 1, SCALING * 2, 0f);
IndexedVector3 deltaY = new IndexedVector3(SCALING * 2, 0.0f, 0f);
for (int i = 0; i < ARRAY_SIZE_X; ++i)
{
y = x;
for (int j = i; j < ARRAY_SIZE_Y; ++j)
{
startTransform._origin = (y - new IndexedVector3(-10, 0, 0));
//using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
DefaultMotionState myMotionState = new DefaultMotionState(startTransform, IndexedMatrix.Identity);
RigidBodyConstructionInfo rbInfo;
switch (j % 3)
{
#if true
case 0:
rbInfo = new RigidBodyConstructionInfo(mass, myMotionState, boxShape, localInertia);
//rbInfo = new RigidBodyConstructionInfo(mass, myMotionState, triangleShape, localInertia);
break;
case 1:
rbInfo = new RigidBodyConstructionInfo(mass, myMotionState, triangleShape, localInertia);
break;
#endif
default:
rbInfo = new RigidBodyConstructionInfo(mass, myMotionState, cylinderShape, localInertia);
break;
}
RigidBody body = new RigidBody(rbInfo);
//body.setContactProcessingThreshold(colShape.getContactBreakingThreshold());
body.SetActivationState(ActivationState.ISLAND_SLEEPING);
body.SetLinearFactor(new IndexedVector3(1, 1, 0));
body.SetAngularFactor(new IndexedVector3(0, 0, 1));
m_dynamicsWorld.AddRigidBody(body);
body.SetActivationState(ActivationState.ISLAND_SLEEPING);
//if (BulletGlobals.g_streamWriter != null)
//{
// BulletGlobals.g_streamWriter.WriteLine("localCreateRigidBody [{0}] startTransform", body.m_debugBodyId);
// MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, startTransform);
// BulletGlobals.g_streamWriter.WriteLine("");
//}
// y += -0.8*deltaY;
y += deltaY;
}
x += deltaX;
}
}
ClientResetScene();
}
public override void InitializeDemo()
{
SetCameraDistance(50);
int totalTriangles = 2 * (NUM_VERTS_X - 1) * (NUM_VERTS_Y - 1);
int vertStride = 1;
int indexStride = 3;
BulletGlobals.gContactAddedCallback = new CustomMaterialCombinerCallback();
gVertices = new ObjectArray <IndexedVector3>(totalVerts);
gIndices = new ObjectArray <int>(totalTriangles * 3);
SetVertexPositions(waveheight, 0.0f);
gVertices.GetRawArray()[1].Y = 0.1f;
int index = 0;
int i, j;
for (i = 0; i < NUM_VERTS_X - 1; i++)
{
for (j = 0; j < NUM_VERTS_Y - 1; j++)
{
#if SWAP_WINDING
#if SHIFT_INDICES
gIndices[index++] = j * NUM_VERTS_X + i;
gIndices[index++] = (j + 1) * NUM_VERTS_X + i + 1;
gIndices[index++] = j * NUM_VERTS_X + i + 1;
gIndices[index++] = j * NUM_VERTS_X + i;
gIndices[index++] = (j + 1) * NUM_VERTS_X + i;
gIndices[index++] = (j + 1) * NUM_VERTS_X + i + 1;
#else
gIndices[index++] = (j + 1) * NUM_VERTS_X + i + 1;
gIndices[index++] = j * NUM_VERTS_X + i + 1;
gIndices[index++] = j * NUM_VERTS_X + i;
gIndices[index++] = (j + 1) * NUM_VERTS_X + i;
gIndices[index++] = (j + 1) * NUM_VERTS_X + i + 1;
gIndices[index++] = j * NUM_VERTS_X + i;
#endif //SHIFT_INDICES
#else //SWAP_WINDING
#if SHIFT_INDICES
gIndices[index++] = (j + 1) * NUM_VERTS_X + i + 1;
gIndices[index++] = j * NUM_VERTS_X + i;
gIndices[index++] = j * NUM_VERTS_X + i + 1;
#if TEST_INCONSISTENT_WINDING
gIndices[index++] = j * NUM_VERTS_X + i;
gIndices[index++] = (j + 1) * NUM_VERTS_X + i;
gIndices[index++] = (j + 1) * NUM_VERTS_X + i + 1;
#else //TEST_INCONSISTENT_WINDING
gIndices[index++] = (j + 1) * NUM_VERTS_X + i;
gIndices[index++] = j * NUM_VERTS_X + i;
gIndices[index++] = (j + 1) * NUM_VERTS_X + i + 1;
#endif //TEST_INCONSISTENT_WINDING
#else //SHIFT_INDICES
gIndices[index++] = j * NUM_VERTS_X + i;
gIndices[index++] = j * NUM_VERTS_X + i + 1;
gIndices[index++] = (j + 1) * NUM_VERTS_X + i + 1;
gIndices[index++] = j * NUM_VERTS_X + i;
gIndices[index++] = (j + 1) * NUM_VERTS_X + i + 1;
gIndices[index++] = (j + 1) * NUM_VERTS_X + i;
#endif //SHIFT_INDICES
#endif //SWAP_WINDING
}
}
m_indexVertexArrays = new TriangleIndexVertexArray(totalTriangles,
gIndices,
indexStride,
totalVerts, gVertices, vertStride);
bool useQuantizedAabbCompression = true;
IndexedVector3 aabbMin = new IndexedVector3(-1000, -1000, -1000);
IndexedVector3 aabbMax = new IndexedVector3(1000, 1000, 1000);
trimeshShape = new BvhTriangleMeshShape(m_indexVertexArrays, useQuantizedAabbCompression, ref aabbMin, ref aabbMax, true);
CollisionShape groundShape = trimeshShape;
TriangleInfoMap triangleInfoMap = new TriangleInfoMap();
InternalEdgeUtility.GenerateInternalEdgeInfo(trimeshShape, triangleInfoMap);
m_collisionConfiguration = new DefaultCollisionConfiguration();
m_dispatcher = new CollisionDispatcher(m_collisionConfiguration);
m_broadphase = new DbvtBroadphase();
m_constraintSolver = new SequentialImpulseConstraintSolver();
m_dynamicsWorld = new DiscreteDynamicsWorld(m_dispatcher, m_broadphase, m_constraintSolver, m_collisionConfiguration);
m_dynamicsWorld.SetDebugDrawer(m_debugDraw);
IndexedVector3 gravity = new IndexedVector3(0, -10, 0);
m_dynamicsWorld.SetGravity(ref gravity);
float mass = 0.0f;
IndexedMatrix startTransform = IndexedMatrix.CreateTranslation(new IndexedVector3(0, -2, 0));
ConvexHullShape colShape = new ConvexHullShape(new List <IndexedVector3>(), 0);
for (int k = 0; k < DemoMeshes.TaruVtxCount; k++)
{
IndexedVector3 vtx = DemoMeshes.TaruVtx[k];
colShape.AddPoint(ref vtx);
}
//this will enable polyhedral contact clipping, better quality, slightly slower
//colShape.InitializePolyhedralFeatures();
//the polyhedral contact clipping can use either GJK or SAT test to find the separating axis
m_dynamicsWorld.GetDispatchInfo().m_enableSatConvex = false;
{
//for (int i2 = 0; i2 < 1; i2++)
//{
// startTransform._origin = new IndexedVector3(-10.0f + i2 * 3.0f, 2.2f + i2 * 0.1f, -1.3f);
// RigidBody body = LocalCreateRigidBody(10, startTransform, colShape);
// body.SetActivationState(ActivationState.DISABLE_DEACTIVATION);
// body.SetLinearVelocity(new IndexedVector3(0, 0, -1));
// //body->setContactProcessingThreshold(0.f);
//}
}
{
BoxShape colShape2 = new BoxShape(new IndexedVector3(1, 1, 1));
//colShape.InitializePolyhedralFeatures();
m_collisionShapes.Add(colShape2);
startTransform._origin = new IndexedVector3(-16.0f + i * 3.0f, 1.0f + i * 0.1f, -1.3f);
RigidBody body = LocalCreateRigidBody(10, startTransform, colShape2);
body.SetActivationState(ActivationState.DISABLE_DEACTIVATION);
body.SetLinearVelocity(new IndexedVector3(0, 0, -1));
}
startTransform = IndexedMatrix.Identity;
staticBody = LocalCreateRigidBody(mass, startTransform, groundShape);
//staticBody->setContactProcessingThreshold(-0.031f);
staticBody.SetCollisionFlags(staticBody.GetCollisionFlags() | CollisionFlags.CF_KINEMATIC_OBJECT); //STATIC_OBJECT);
//enable custom material callback
staticBody.SetCollisionFlags(staticBody.GetCollisionFlags() | CollisionFlags.CF_CUSTOM_MATERIAL_CALLBACK);
m_debugDraw.SetDebugMode(DebugDrawModes.DBG_DrawText | DebugDrawModes.DBG_NoHelpText | DebugDrawModes.DBG_DrawWireframe | DebugDrawModes.DBG_DrawContactPoints);
//base.InitializeDemo();
//ClientResetScene();
}
public override void InitializeDemo()
{
m_collisionConfiguration = new DefaultCollisionConfiguration();
m_dispatcher = new CollisionDispatcher(m_collisionConfiguration);
IndexedVector3 worldMin = new IndexedVector3(-1000, -1000, -1000);
IndexedVector3 worldMax = new IndexedVector3(1000, 1000, 1000);
m_broadphase = new AxisSweep3Internal(ref worldMin, ref worldMax, 0xfffe, 0xffff, 16384, null, false);
m_constraintSolver = new SequentialImpulseConstraintSolver();
m_dynamicsWorld = new DiscreteDynamicsWorld(m_dispatcher, m_broadphase, m_constraintSolver, m_collisionConfiguration);
m_dynamicsWorld.SetDebugDrawer(m_debugDraw);
SetCameraDistance(26f);
//CollisionShape groundShape = new BoxShape(new IndexedVector3(50f, 40f, 50f));
CollisionShape groundShape = new StaticPlaneShape(new IndexedVector3(0, 1, 0), 40);
m_collisionShapes.Add(groundShape);
IndexedMatrix groundTransform = IndexedMatrix.Identity;
groundTransform._origin = new IndexedVector3(0, -56, 0);
RigidBody groundBody = LocalCreateRigidBody(0, ref groundTransform, groundShape);
CollisionShape shape = new BoxShape(new IndexedVector3(CUBE_HALF_EXTENTS, CUBE_HALF_EXTENTS, CUBE_HALF_EXTENTS));
m_collisionShapes.Add(shape);
IndexedMatrix trans = IndexedMatrix.Identity;
trans._origin = new IndexedVector3(0, 20, 0);
float mass = 1f;
#if true
//point to point constraint (ball socket)
//SEEMS OK
{
RigidBody body0 = LocalCreateRigidBody(mass, ref trans, shape);
trans._origin = new IndexedVector3(2 * CUBE_HALF_EXTENTS, 20, 0);
mass = 1f;
RigidBody body1 = null; //localCreateRigidBody( mass,trans,shape);
IndexedVector3 pivotInA = new IndexedVector3(CUBE_HALF_EXTENTS, -CUBE_HALF_EXTENTS, -CUBE_HALF_EXTENTS);
IndexedVector3 axisInA = new IndexedVector3(0, 0, 1);
IndexedVector3 pivotInB = body1 != null?body1.GetCenterOfMassTransform().Inverse() * (body0.GetCenterOfMassTransform() * (pivotInA)) : pivotInA;
IndexedVector3 axisInB = body1 != null ? (body1.GetCenterOfMassTransform()._basis.Inverse() * (body1.GetCenterOfMassTransform()._basis *axisInA)) :
body0.GetCenterOfMassTransform()._basis *axisInA;
HingeConstraint hinge = new HingeConstraint(body0, ref pivotInA, ref axisInA, false);
float targetVelocity = 1f;
float maxMotorImpulse = 1.0f;
hinge.EnableAngularMotor(true, targetVelocity, maxMotorImpulse);
m_dynamicsWorld.AddConstraint(hinge); //p2p);
hinge.SetDbgDrawSize(5f);
}
#endif
#if true
//create a slider, using the generic D6 constraint
// SEEMS OK
{
mass = 1f;
IndexedVector3 sliderWorldPos = new IndexedVector3(0, 10, 0);
IndexedVector3 sliderAxis = new IndexedVector3(1, 0, 0);
float angle = 0f;//SIMD_RADS_PER_DEG * 10.f;
IndexedBasisMatrix sliderOrientation = new IndexedBasisMatrix(Quaternion.CreateFromAxisAngle(sliderAxis.ToVector3(), angle));
trans = IndexedMatrix.Identity;
trans._origin = sliderWorldPos;
//trans.setBasis(sliderOrientation);
sliderTransform = trans;
d6body0 = LocalCreateRigidBody(mass, ref trans, shape);
d6body0.SetActivationState(ActivationState.DISABLE_DEACTIVATION);
RigidBody fixedBody1 = LocalCreateRigidBody(0, ref trans, null);
m_dynamicsWorld.AddRigidBody(fixedBody1);
IndexedMatrix frameInA, frameInB;
frameInA = IndexedMatrix.Identity;
frameInB = IndexedMatrix.Identity;
frameInA._origin = new IndexedVector3(0, 5, 0);
frameInB._origin = new IndexedVector3(0, 5, 0);
// bool useLinearReferenceFrameA = false;//use fixed frame B for linear llimits
bool useLinearReferenceFrameA = true; //use fixed frame A for linear llimits
spSlider6Dof = new Generic6DofConstraint(fixedBody1, d6body0, ref frameInA, ref frameInB, useLinearReferenceFrameA);
spSlider6Dof.SetLinearLowerLimit(ref lowerSliderLimit);
spSlider6Dof.SetLinearUpperLimit(ref hiSliderLimit);
//range should be small, otherwise singularities will 'explode' the constraint
IndexedVector3 angularLower = new IndexedVector3(-1.5f, 0, 0);
IndexedVector3 angularUpper = -angularLower;
spSlider6Dof.SetAngularLowerLimit(ref angularLower);
spSlider6Dof.SetAngularUpperLimit(ref angularUpper);
// slider.setAngularLowerLimit(IndexedVector3(0,0,0));
// slider.setAngularUpperLimit(IndexedVector3(0,0,0));
spSlider6Dof.GetTranslationalLimitMotor().m_enableMotor[0] = true;
spSlider6Dof.GetTranslationalLimitMotor().m_targetVelocity.X = -5.0f;
spSlider6Dof.GetTranslationalLimitMotor().m_maxMotorForce.X = 0.1f;
m_dynamicsWorld.AddConstraint(spSlider6Dof);
spSlider6Dof.SetDbgDrawSize(5f);
}
#endif
#if true
{ // create a door using hinge constraint attached to the world
CollisionShape pDoorShape = new BoxShape(new IndexedVector3(2.0f, 5.0f, 0.2f));
m_collisionShapes.Add(pDoorShape);
IndexedMatrix doorTrans = IndexedMatrix.Identity;
doorTrans._origin = new IndexedVector3(-5.0f, -2.0f, 0.0f);
RigidBody pDoorBody = LocalCreateRigidBody(1.0f, ref doorTrans, pDoorShape);
pDoorBody.SetActivationState(ActivationState.DISABLE_DEACTIVATION);
IndexedVector3 btPivotA = new IndexedVector3(10f + 2.1f, -2.0f, 0.0f); // right next to the door slightly outside
IndexedVector3 btAxisA = new IndexedVector3(0.0f, 1.0f, 0.0f); // pointing upwards, aka Y-axis
spDoorHinge = new HingeConstraint(pDoorBody, ref btPivotA, ref btAxisA, false);
spDoorHinge.SetLimit(-MathUtil.SIMD_PI * 0.25f, MathUtil.SIMD_PI * 0.25f);
m_dynamicsWorld.AddConstraint(spDoorHinge);
spDoorHinge.SetDbgDrawSize(5.0f);
}
#endif
#if true
{ // create a generic 6DOF constraint
// SEEMS OK - But debug draw a bit wrong?
IndexedMatrix tr = IndexedMatrix.Identity;
tr._origin = new IndexedVector3(10f, 6f, 0f);
//tr.getBasis().setEulerZYX(0,0,0);
// RigidBody pBodyA = localCreateRigidBody( mass, tr, shape);
RigidBody pBodyA = LocalCreateRigidBody(0.0f, ref tr, shape);
// RigidBody pBodyA = localCreateRigidBody( 0.0, tr, 0);
pBodyA.SetActivationState(ActivationState.DISABLE_DEACTIVATION);
tr = IndexedMatrix.Identity;
tr._origin = new IndexedVector3(0f, 6f, 0f);
//tr.getBasis().setEulerZYX(0,0,0);
RigidBody pBodyB = LocalCreateRigidBody(mass, ref tr, shape);
pBodyB.SetActivationState(ActivationState.DISABLE_DEACTIVATION);
IndexedMatrix frameInA, frameInB;
frameInA = IndexedMatrix.CreateTranslation(-5, 0, 0);
frameInB = IndexedMatrix.CreateTranslation(5, 0, 0);
Generic6DofConstraint pGen6DOF = new Generic6DofConstraint(pBodyA, pBodyB, ref frameInA, ref frameInB, true);
// btGeneric6DofConstraint* pGen6DOF = new btGeneric6DofConstraint(*pBodyA, *pBodyB, frameInA, frameInB, false);
IndexedVector3 linearLower = new IndexedVector3(-10, -2, -1);
pGen6DOF.SetLinearLowerLimit(ref linearLower);
IndexedVector3 linearUpper = new IndexedVector3(10, 2, 1);
pGen6DOF.SetLinearUpperLimit(ref linearUpper);
// ? why again?
//linearLower = new IndexedVector3(-10,0,0);
//pGen6DOF.setLinearLowerLimit(ref linearLower);
// pGen6DOF.setLinearUpperLimit(IndexedVector3(10., 0., 0.));
// pGen6DOF.setLinearLowerLimit(IndexedVector3(0., 0., 0.));
// pGen6DOF.setLinearUpperLimit(IndexedVector3(0., 0., 0.));
// pGen6DOF.getTranslationalLimitMotor().m_enableMotor[0] = true;
// pGen6DOF.getTranslationalLimitMotor().m_targetVelocity[0] = 5.0f;
// pGen6DOF.getTranslationalLimitMotor().m_maxMotorForce[0] = 0.1f;
// pGen6DOF.setAngularLowerLimit(IndexedVector3(0., SIMD_HALF_PI*0.9, 0.));
// pGen6DOF.setAngularUpperLimit(IndexedVector3(0., -SIMD_HALF_PI*0.9, 0.));
// pGen6DOF.setAngularLowerLimit(IndexedVector3(0., 0., -SIMD_HALF_PI));
// pGen6DOF.setAngularUpperLimit(IndexedVector3(0., 0., SIMD_HALF_PI));
IndexedVector3 angularLower = new IndexedVector3(-MathUtil.SIMD_HALF_PI * 0.5f, -0.75f, -MathUtil.SIMD_HALF_PI * 0.8f);
IndexedVector3 angularUpper = -angularLower;
pGen6DOF.SetAngularLowerLimit(ref angularLower);
pGen6DOF.SetAngularUpperLimit(ref angularUpper);
// pGen6DOF.setAngularLowerLimit(IndexedVector3(0.f, -0.75, SIMD_HALF_PI * 0.8f));
// pGen6DOF.setAngularUpperLimit(IndexedVector3(0.f, 0.75, -SIMD_HALF_PI * 0.8f));
// pGen6DOF.setAngularLowerLimit(IndexedVector3(0.f, -SIMD_HALF_PI * 0.8f, SIMD_HALF_PI * 1.98f));
// pGen6DOF.setAngularUpperLimit(IndexedVector3(0.f, SIMD_HALF_PI * 0.8f, -SIMD_HALF_PI * 1.98f));
// pGen6DOF.setAngularLowerLimit(IndexedVector3(-0.75,-0.5, -0.5));
// pGen6DOF.setAngularUpperLimit(IndexedVector3(0.75,0.5, 0.5));
// pGen6DOF.setAngularLowerLimit(IndexedVector3(-0.75,0., 0.));
// pGen6DOF.setAngularUpperLimit(IndexedVector3(0.75,0., 0.));
m_dynamicsWorld.AddConstraint(pGen6DOF, true);
pGen6DOF.SetDbgDrawSize(5.0f);
}
#endif
#if true
{ // create a ConeTwist constraint
IndexedMatrix tr = IndexedMatrix.CreateTranslation(-10, 5, 0);
RigidBody pBodyA = LocalCreateRigidBody(1.0f, ref tr, shape);
pBodyA.SetActivationState(ActivationState.DISABLE_DEACTIVATION);
tr = IndexedMatrix.CreateTranslation(-10, -5, 0);
RigidBody pBodyB = LocalCreateRigidBody(0.0f, ref tr, shape);
IndexedMatrix frameInA, frameInB;
frameInA = MathUtil.SetEulerZYX(0, 0, MathUtil.SIMD_HALF_PI);
frameInA._origin = new IndexedVector3(0, -5, 0);
frameInB = MathUtil.SetEulerZYX(0, 0, MathUtil.SIMD_HALF_PI);
frameInB._origin = new IndexedVector3(0, 5, 0);
ConeTwistConstraint pCT = new ConeTwistConstraint(pBodyA, pBodyB, ref frameInA, ref frameInB);
pCT.SetLimit(MathUtil.SIMD_QUARTER_PI, MathUtil.SIMD_QUARTER_PI, MathUtil.SIMD_PI * 0.8f, 1.0f, 0.3f, 1.0f); // soft limit == hard limit
m_dynamicsWorld.AddConstraint(pCT, true);
pCT.SetDbgDrawSize(5.0f);
}
#endif
#if true
{ // Hinge connected to the world, with motor (to hinge motor with new and old constraint solver)
// WORKS OK
IndexedMatrix tr = IndexedMatrix.Identity;
RigidBody pBody = LocalCreateRigidBody(1.0f, ref tr, shape);
pBody.SetActivationState(ActivationState.DISABLE_DEACTIVATION);
IndexedVector3 btPivotA = new IndexedVector3(10.0f, 0.0f, 0.0f);
IndexedVector3 btAxisA = new IndexedVector3(0.0f, 0.0f, 1.0f);
HingeConstraint pHinge = new HingeConstraint(pBody, ref btPivotA, ref btAxisA, false);
// pHinge.enableAngularMotor(true, -1.0, 0.165); // use for the old solver
pHinge.EnableAngularMotor(true, -1.0f, 1.65f); // use for the new SIMD solver
m_dynamicsWorld.AddConstraint(pHinge);
pHinge.SetDbgDrawSize(5.0f);
}
#endif
#if true
{
// WORKS OK
// create a universal joint using generic 6DOF constraint
// create two rigid bodies
// static bodyA (parent) on top:
IndexedMatrix tr = IndexedMatrix.CreateTranslation(20, 4, 0);
RigidBody pBodyA = LocalCreateRigidBody(0.0f, ref tr, shape);
pBodyA.SetActivationState(ActivationState.DISABLE_DEACTIVATION);
// dynamic bodyB (child) below it :
tr = IndexedMatrix.CreateTranslation(20, 0, 0);
RigidBody pBodyB = LocalCreateRigidBody(1.0f, ref tr, shape);
pBodyB.SetActivationState(ActivationState.DISABLE_DEACTIVATION);
// add some (arbitrary) data to build constraint frames
IndexedVector3 parentAxis = new IndexedVector3(1.0f, 0.0f, 0.0f);
IndexedVector3 childAxis = new IndexedVector3(0.0f, 0.0f, 1.0f);
IndexedVector3 anchor = new IndexedVector3(20.0f, 2.0f, 0.0f);
UniversalConstraint pUniv = new UniversalConstraint(pBodyA, pBodyB, ref anchor, ref parentAxis, ref childAxis);
pUniv.SetLowerLimit(-MathUtil.SIMD_HALF_PI * 0.5f, -MathUtil.SIMD_HALF_PI * 0.5f);
pUniv.SetUpperLimit(MathUtil.SIMD_HALF_PI * 0.5f, MathUtil.SIMD_HALF_PI * 0.5f);
// add constraint to world
m_dynamicsWorld.AddConstraint(pUniv, true);
// draw constraint frames and limits for debugging
pUniv.SetDbgDrawSize(5.0f);
}
#endif
#if true
// WORKS OK
{ // create a generic 6DOF constraint with springs
IndexedMatrix tr = IndexedMatrix.CreateTranslation(-20f, 16f, 0f);
//tr.setIdentity();
//tr.setOrigin(btVector3(btScalar(-20.), btScalar(16.), btScalar(0.)));
//tr.getBasis().setEulerZYX(0,0,0);
RigidBody pBodyA = LocalCreateRigidBody(0.0f, ref tr, shape);
pBodyA.SetActivationState(ActivationState.DISABLE_DEACTIVATION);
//tr.setIdentity();
//tr.setOrigin(btVector3(btScalar(-10.), btScalar(16.), btScalar(0.)));
//tr.getBasis().setEulerZYX(0,0,0);
tr = IndexedMatrix.CreateTranslation(-10, 16, 0);
RigidBody pBodyB = LocalCreateRigidBody(1.0f, ref tr, shape);
pBodyB.SetActivationState(ActivationState.DISABLE_DEACTIVATION);
IndexedMatrix frameInA = IndexedMatrix.CreateTranslation(10f, 0f, 0f);
IndexedMatrix frameInB = IndexedMatrix.CreateTranslation(0f, 0f, 0f);
Generic6DofSpringConstraint pGen6DOFSpring = new Generic6DofSpringConstraint(pBodyA, pBodyB, ref frameInA, ref frameInB, true);
pGen6DOFSpring.SetLinearUpperLimit(new IndexedVector3(5f, 0f, 0f));
pGen6DOFSpring.SetLinearLowerLimit(new IndexedVector3(-5f, 0f, 0f));
pGen6DOFSpring.SetAngularLowerLimit(new IndexedVector3(0f, 0f, -1.5f));
pGen6DOFSpring.SetAngularUpperLimit(new IndexedVector3(0f, 0f, 1.5f));
m_dynamicsWorld.AddConstraint(pGen6DOFSpring, true);
pGen6DOFSpring.SetDbgDrawSize(5.0f);
pGen6DOFSpring.EnableSpring(0, true);
pGen6DOFSpring.SetStiffness(0, 39.478f);
pGen6DOFSpring.SetDamping(0, 0.5f);
pGen6DOFSpring.EnableSpring(5, true);
pGen6DOFSpring.SetStiffness(5, 39.478f);
pGen6DOFSpring.SetDamping(0, 0.3f);
pGen6DOFSpring.SetEquilibriumPoint();
}
#endif
#if true
{
// WORKS OK
// create a Hinge2 joint
// create two rigid bodies
// static bodyA (parent) on top:
IndexedMatrix tr = IndexedMatrix.CreateTranslation(-20f, 4f, 0f);
RigidBody pBodyA = LocalCreateRigidBody(0.0f, ref tr, shape);
pBodyA.SetActivationState(ActivationState.DISABLE_DEACTIVATION);
// dynamic bodyB (child) below it :
tr = IndexedMatrix.CreateTranslation(-20f, 0f, 0f);
RigidBody pBodyB = LocalCreateRigidBody(1.0f, ref tr, shape);
pBodyB.SetActivationState(ActivationState.DISABLE_DEACTIVATION);
// add some data to build constraint frames
IndexedVector3 parentAxis = new IndexedVector3(0.0f, 1.0f, 0.0f);
IndexedVector3 childAxis = new IndexedVector3(1.0f, 0.0f, 0.0f);
IndexedVector3 anchor = new IndexedVector3(-20.0f, 0.0f, 0.0f);
Hinge2Constraint pHinge2 = new Hinge2Constraint(pBodyA, pBodyB, ref anchor, ref parentAxis, ref childAxis);
pHinge2.SetLowerLimit(-MathUtil.SIMD_HALF_PI * 0.5f);
pHinge2.SetUpperLimit(MathUtil.SIMD_HALF_PI * 0.5f);
// add constraint to world
m_dynamicsWorld.AddConstraint(pHinge2, true);
// draw constraint frames and limits for debugging
pHinge2.SetDbgDrawSize(5.0f);
}
#endif
#if true
{
// WORKS OK
// create a Hinge joint between two dynamic bodies
// create two rigid bodies
// static bodyA (parent) on top:
IndexedMatrix tr = IndexedMatrix.CreateTranslation(-20f, -2f, 0f);
RigidBody pBodyA = LocalCreateRigidBody(1.0f, ref tr, shape);
pBodyA.SetActivationState(ActivationState.DISABLE_DEACTIVATION);
// dynamic bodyB:
tr = IndexedMatrix.CreateTranslation(-30f, -2f, 0f);
RigidBody pBodyB = LocalCreateRigidBody(10.0f, ref tr, shape);
pBodyB.SetActivationState(ActivationState.DISABLE_DEACTIVATION);
// add some data to build constraint frames
IndexedVector3 axisA = new IndexedVector3(0.0f, 1.0f, 0.0f);
IndexedVector3 axisB = new IndexedVector3(0.0f, 1.0f, 0.0f);
IndexedVector3 pivotA = new IndexedVector3(-5.0f, 0.0f, 0.0f);
IndexedVector3 pivotB = new IndexedVector3(5.0f, 0.0f, 0.0f);
spHingeDynAB = new HingeConstraint(pBodyA, pBodyB, ref pivotA, ref pivotB, ref axisA, ref axisB);
spHingeDynAB.SetLimit(-MathUtil.SIMD_HALF_PI * 0.5f, MathUtil.SIMD_HALF_PI * 0.5f);
// add constraint to world
m_dynamicsWorld.AddConstraint(spHingeDynAB, true);
// draw constraint frames and limits for debugging
spHingeDynAB.SetDbgDrawSize(5.0f);
}
#endif
}
public void DrawXNA(ref IndexedMatrix m, CollisionShape shape, ref IndexedVector3 color, DebugDrawModes debugMode, ref IndexedVector3 worldBoundsMin, ref IndexedVector3 worldBoundsMax, ref IndexedMatrix view, ref IndexedMatrix projection)
{
//btglMultMatrix(m);
if (shape == null)
{
return;
}
if (shape.GetShapeType() == BroadphaseNativeTypes.UNIFORM_SCALING_SHAPE_PROXYTYPE)
{
UniformScalingShape scalingShape = (UniformScalingShape)shape;
ConvexShape convexShape = scalingShape.GetChildShape();
float scalingFactor = scalingShape.GetUniformScalingFactor();
IndexedMatrix scaleMatrix = IndexedMatrix.CreateScale(scalingFactor);
IndexedMatrix finalMatrix = scaleMatrix * m;
DrawXNA(ref finalMatrix, convexShape, ref color, debugMode, ref worldBoundsMin, ref worldBoundsMax, ref view, ref projection);
return;
}
if (shape.GetShapeType() == BroadphaseNativeTypes.COMPOUND_SHAPE_PROXYTYPE)
{
CompoundShape compoundShape = (CompoundShape)shape;
for (int i = compoundShape.GetNumChildShapes() - 1; i >= 0; i--)
{
IndexedMatrix childTrans = compoundShape.GetChildTransform(i);
CollisionShape colShape = compoundShape.GetChildShape(i);
IndexedMatrix childMat = childTrans;
//childMat = MathUtil.bulletMatrixMultiply(m, childMat);
//childMat = childMat * m;
childMat = m * childMat;
DrawXNA(ref childMat, colShape, ref color, debugMode, ref worldBoundsMin, ref worldBoundsMax, ref view, ref projection);
}
}
else
{
bool useWireframeFallback = true;
if ((debugMode & DebugDrawModes.DBG_DrawWireframe) == 0)
{
///you can comment out any of the specific cases, and use the default
///the benefit of 'default' is that it approximates the actual collision shape including collision margin
//BroadphaseNativeTypes shapetype = m_textureEnabled ? BroadphaseNativeTypes.MAX_BROADPHASE_COLLISION_TYPES : shape.getShapeType();
BroadphaseNativeTypes shapetype = shape.GetShapeType();
switch (shapetype)
{
case BroadphaseNativeTypes.BOX_SHAPE_PROXYTYPE:
{
BoxShape boxShape = shape as BoxShape;
IndexedVector3 halfExtents = boxShape.GetHalfExtentsWithMargin();
DrawSolidCube(ref halfExtents, ref m, ref view, ref projection, ref color);
//drawSolidSphere(halfExtents.X, 10, 10, ref m, ref view, ref projection);
//drawCylinder(halfExtents.X, halfExtents.Y, 1, ref m, ref view, ref projection);
//drawSolidCone(halfExtents.Y, halfExtents.X, ref m, ref view, ref projection);
//DrawText("Hello World", new IndexedVector3(20, 20, 0), new IndexedVector3(255, 255, 255));
useWireframeFallback = false;
break;
}
case BroadphaseNativeTypes.SPHERE_SHAPE_PROXYTYPE:
{
SphereShape sphereShape = shape as SphereShape;
float radius = sphereShape.GetMargin();//radius doesn't include the margin, so draw with margin
DrawSolidSphere(radius, 10, 10, ref m, ref view, ref projection, ref color);
//glutSolidSphere(radius,10,10);
useWireframeFallback = false;
break;
}
case BroadphaseNativeTypes.CAPSULE_SHAPE_PROXYTYPE:
{
CapsuleShape capsuleShape = shape as CapsuleShape;
float radius = capsuleShape.GetRadius();
float halfHeight = capsuleShape.GetHalfHeight();
int upAxis = capsuleShape.GetUpAxis();
IndexedVector3 capStart = IndexedVector3.Zero;
capStart[upAxis] = -halfHeight;
IndexedVector3 capEnd = IndexedVector3.Zero;
capEnd[upAxis] = halfHeight;
// Draw the ends
{
IndexedMatrix childTransform = IndexedMatrix.Identity;
childTransform._origin = m * capStart;
DrawSolidSphere(radius, 5, 5, ref childTransform, ref view, ref projection, ref color);
}
{
IndexedMatrix childTransform = IndexedMatrix.Identity;
childTransform._origin = m * capEnd;
DrawSolidSphere(radius, 5, 5, ref childTransform, ref view, ref projection, ref color);
}
DrawCylinder(radius, halfHeight, upAxis, ref m, ref view, ref projection, ref color);
break;
}
case BroadphaseNativeTypes.CONE_SHAPE_PROXYTYPE:
{
ConeShape coneShape = (ConeShape)(shape);
int upIndex = coneShape.GetConeUpIndex();
float radius = coneShape.GetRadius(); //+coneShape.getMargin();
float height = coneShape.GetHeight(); //+coneShape.getMargin();
IndexedMatrix rotateMatrix = IndexedMatrix.Identity;
switch (upIndex)
{
case 0:
rotateMatrix = IndexedMatrix.CreateRotationX(-MathUtil.SIMD_HALF_PI);
break;
case 1:
break;
case 2:
rotateMatrix = IndexedMatrix.CreateRotationX(MathUtil.SIMD_HALF_PI);
break;
default:
{
break;
}
}
;
IndexedMatrix translationMatrix = IndexedMatrix.CreateTranslation(0f, 0f, -0.5f * height);
IndexedMatrix resultant = m * rotateMatrix * translationMatrix;
DrawSolidCone(height, radius, ref resultant, ref view, ref projection, ref color);
useWireframeFallback = false;
break;
}
case BroadphaseNativeTypes.STATIC_PLANE_PROXYTYPE:
{
StaticPlaneShape staticPlaneShape = shape as StaticPlaneShape;
float planeConst = staticPlaneShape.GetPlaneConstant();
IndexedVector3 planeNormal = staticPlaneShape.GetPlaneNormal();
IndexedVector3 planeOrigin = planeNormal * planeConst;
IndexedVector3 vec0, vec1;
TransformUtil.PlaneSpace1(ref planeNormal, out vec0, out vec1);
float vecLen = 100f;
IndexedVector3 pt0 = planeOrigin + vec0 * vecLen;
IndexedVector3 pt1 = planeOrigin - vec0 * vecLen;
IndexedVector3 pt2 = planeOrigin + vec1 * vecLen;
IndexedVector3 pt3 = planeOrigin - vec1 * vecLen;
// Fallback to debug draw - needs tidying
IndexedVector3 colour = new IndexedVector3(255, 255, 255);
DrawLine(ref pt0, ref pt1, ref colour);
DrawLine(ref pt1, ref pt2, ref colour);
DrawLine(ref pt2, ref pt3, ref colour);
DrawLine(ref pt3, ref pt1, ref colour);
break;
}
case BroadphaseNativeTypes.CYLINDER_SHAPE_PROXYTYPE:
{
CylinderShape cylinder = (CylinderShape)(shape);
int upAxis = cylinder.GetUpAxis();
float radius = cylinder.GetRadius();
float halfHeight = cylinder.GetHalfExtentsWithMargin()[upAxis];
DrawCylinder(radius, halfHeight, upAxis, ref m, ref view, ref projection, ref color);
break;
}
default:
{
if (shape.IsConvex())
{
ShapeCache sc = Cache(shape as ConvexShape);
//if (shape.getUserPointer())
{
//glutSolidCube(1.0);
ShapeHull hull = sc.m_shapehull /*(btShapeHull*)shape.getUserPointer()*/;
int numTriangles = hull.NumTriangles();
int numIndices = hull.NumIndices();
int numVertices = hull.NumVertices();
if (numTriangles > 0)
{
int index = 0;
IList <int> idx = hull.m_indices;
IList <IndexedVector3> vtx = hull.m_vertices;
for (int i = 0; i < numTriangles; i++)
{
int i1 = index++;
int i2 = index++;
int i3 = index++;
Debug.Assert(i1 < numIndices &&
i2 < numIndices &&
i3 < numIndices);
int index1 = idx[i1];
int index2 = idx[i2];
int index3 = idx[i3];
Debug.Assert(index1 < numVertices &&
index2 < numVertices &&
index3 < numVertices);
IndexedVector3 v1 = m * vtx[index1];
IndexedVector3 v2 = m * vtx[index2];
IndexedVector3 v3 = m * vtx[index3];
IndexedVector3 normal = IndexedVector3.Cross((v3 - v1), (v2 - v1));
normal.Normalize();
Vector2 tex = new Vector2(0, 0);
AddVertex(ref v1, ref normal, ref tex);
AddVertex(ref v2, ref normal, ref tex);
AddVertex(ref v3, ref normal, ref tex);
}
}
}
}
break;
}
}
}
/// for polyhedral shapes
if (debugMode == DebugDrawModes.DBG_DrawFeaturesText && (shape.IsPolyhedral()))
{
PolyhedralConvexShape polyshape = (PolyhedralConvexShape)shape;
{
//BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),polyshape.getExtraDebugInfo());
IndexedVector3 colour = new IndexedVector3(255, 255, 255);
for (int i = 0; i < polyshape.GetNumVertices(); i++)
{
IndexedVector3 vtx;
polyshape.GetVertex(i, out vtx);
String buf = " " + i;
DrawText(buf, ref vtx, ref colour);
}
for (int i = 0; i < polyshape.GetNumPlanes(); i++)
{
IndexedVector3 normal;
IndexedVector3 vtx;
polyshape.GetPlane(out normal, out vtx, i);
float d = IndexedVector3.Dot(vtx, normal);
vtx *= d;
String buf = " plane " + i;
DrawText(buf, ref vtx, ref colour);
}
}
}
if (shape.IsConcave() && !shape.IsInfinite()) //>getShapeType() == TRIANGLE_MESH_SHAPE_PROXYTYPE||shape.getShapeType() == GIMPACT_SHAPE_PROXYTYPE)
// if (shape.getShapeType() == TRIANGLE_MESH_SHAPE_PROXYTYPE)
{
ConcaveShape concaveMesh = shape as ConcaveShape;
XNADrawcallback drawCallback = new XNADrawcallback(this, ref m);
drawCallback.m_wireframe = (debugMode & DebugDrawModes.DBG_DrawWireframe) != 0;
concaveMesh.ProcessAllTriangles(drawCallback, ref worldBoundsMin, ref worldBoundsMax);
}
//glDisable(GL_DEPTH_TEST);
//glRasterPos3f(0,0,0);//mvtx.x(), vtx.y(), vtx.z());
if ((debugMode & DebugDrawModes.DBG_DrawText) != 0)
{
IndexedVector3 position = IndexedVector3.Zero;
IndexedVector3 colour = new IndexedVector3(255, 255, 255);
DrawText(shape.GetName(), ref position, ref colour);
}
if ((debugMode & DebugDrawModes.DBG_DrawFeaturesText) != 0)
{
//drawText(shape.getEx]
//BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),shape.getExtraDebugInfo());
}
//glEnable(GL_DEPTH_TEST);
//// glPopMatrix();
//if(m_textureenabled) glDisable(GL_TEXTURE_2D);
// }
// glPopMatrix();
}
}
//----------------------------------------------------------------------------------------------------------------
public override void InitializeDemo()
{
CollisionShape groundShape = new BoxShape(new IndexedVector3(50, 3, 50));
//CollisionShape groundShape = new StaticPlaneShape(IndexedVector3.Up, 0f);
m_collisionShapes.Add(groundShape);
m_collisionConfiguration = new DefaultCollisionConfiguration();
m_dispatcher = new CollisionDispatcher(m_collisionConfiguration);
IndexedVector3 worldMin = new IndexedVector3(-1000, -1000, -1000);
IndexedVector3 worldMax = new IndexedVector3(1000, 1000, 1000);
//m_broadphase = new AxisSweep3Internal(ref worldMin, ref worldMax, 0xfffe, 0xffff, 16384, null, false);
m_broadphase = new SimpleBroadphase(100, null);
m_constraintSolver = new SequentialImpulseConstraintSolver();
m_dynamicsWorld = new DiscreteDynamicsWorld(m_dispatcher, m_broadphase, m_constraintSolver, m_collisionConfiguration);
//m_dynamicsWorld.setGravity(new IndexedVector3(0,0,0));
IndexedMatrix tr = IndexedMatrix.CreateTranslation(0, -10, 0);
//either use heightfield or triangle mesh
//create ground object
LocalCreateRigidBody(0f, ref tr, groundShape);
CollisionShape chassisShape = new BoxShape(new IndexedVector3(1.0f, 0.5f, 2.0f));
m_collisionShapes.Add(chassisShape);
CompoundShape compound = new CompoundShape();
m_collisionShapes.Add(compound);
//localTrans effectively shifts the center of mass with respect to the chassis
IndexedMatrix localTrans = IndexedMatrix.CreateTranslation(0, 1, 0);
compound.AddChildShape(ref localTrans, chassisShape);
{
CollisionShape suppShape = new BoxShape(new IndexedVector3(0.5f, 0.1f, 0.5f));
//localTrans effectively shifts the center of mass with respect to the chassis
IndexedMatrix suppLocalTrans = IndexedMatrix.CreateTranslation(0f, 1.0f, 2.5f);
compound.AddChildShape(ref suppLocalTrans, suppShape);
}
tr._origin = IndexedVector3.Zero;
m_carChassis = LocalCreateRigidBody(800f, ref tr, compound);//chassisShape);
//m_carChassis = LocalCreateRigidBody(800f, ref tr, chassisShape);//chassisShape);
//CollisionShape liftShape = new BoxShape(new IndexedVector3(0.5f, 2.0f, 0.05f));
//m_collisionShapes.Add(liftShape);
//m_liftStartPos = new IndexedVector3(0.0f, 2.5f, 3.05f);
//IndexedMatrix liftTrans = IndexedMatrix.CreateTranslation(m_liftStartPos);
//m_liftBody = LocalCreateRigidBody(10f, ref liftTrans, liftShape);
//IndexedMatrix localA = MathUtil.SetEulerZYX(0f, MathUtil.SIMD_HALF_PI, 0f);
//localA._origin = new IndexedVector3(0f, 1.0f, 3.05f);
//IndexedMatrix localB = MathUtil.SetEulerZYX(0f, MathUtil.SIMD_HALF_PI, 0f);
//localB._origin = new IndexedVector3(0f, -1.5f, -0.05f);
//m_liftHinge = new HingeConstraint(m_carChassis, m_liftBody, ref localA, ref localB);
//// m_liftHinge.setLimit(-LIFT_EPS, LIFT_EPS);
//m_liftHinge.SetLimit(0.0f, 0.0f);
//m_dynamicsWorld.AddConstraint(m_liftHinge, true);
//CompoundShape forkCompound = new CompoundShape();
//m_collisionShapes.Add(forkCompound);
//IndexedMatrix forkLocalTrans = IndexedMatrix.Identity;
//CollisionShape forkShapeA = new BoxShape(new IndexedVector3(1.0f, 0.1f, 0.1f));
//m_collisionShapes.Add(forkShapeA);
//forkCompound.AddChildShape(ref forkLocalTrans, forkShapeA);
//CollisionShape forkShapeB = new BoxShape(new IndexedVector3(0.1f, 0.02f, 0.6f));
//m_collisionShapes.Add(forkShapeB);
//forkLocalTrans = IndexedMatrix.CreateTranslation(-0.9f, -0.08f, 0.7f);
//forkCompound.AddChildShape(ref forkLocalTrans, forkShapeB);
//CollisionShape forkShapeC = new BoxShape(new IndexedVector3(0.1f, 0.02f, 0.6f));
//m_collisionShapes.Add(forkShapeC);
//forkLocalTrans = IndexedMatrix.CreateTranslation(0.9f, -0.08f, 0.7f);
//forkCompound.AddChildShape(ref forkLocalTrans, forkShapeC);
//m_forkStartPos = new IndexedVector3(0.0f, 0.6f, 3.2f);
//IndexedMatrix forkTrans = IndexedMatrix.CreateTranslation(m_forkStartPos);
//m_forkBody = LocalCreateRigidBody(5f, ref forkTrans, forkCompound);
//localA = MathUtil.SetEulerZYX(0f, 0f, MathUtil.SIMD_HALF_PI);
//localA._origin = new IndexedVector3(0.0f, -1.9f, 0.05f);
//IndexedVector3 col0 = MathUtil.matrixColumn(ref localA, 0);
//IndexedVector3 col1 = MathUtil.matrixColumn(ref localA, 1);
//IndexedVector3 col2 = MathUtil.matrixColumn(ref localA, 2);
////localB = MathUtil.setEulerZYX(0f, 0f, MathUtil.SIMD_HALF_PI);
//localB = MathUtil.SetEulerZYX(0f, 0f, MathUtil.SIMD_HALF_PI);
//localB._origin = new IndexedVector3(0.0f, 0.0f, -0.1f);
//m_forkSlider = new SliderConstraint(m_liftBody, m_forkBody, ref localA, ref localB, true);
//m_forkSlider.SetLowerLinLimit(0.1f);
//m_forkSlider.SetUpperLinLimit(0.1f);
//// m_forkSlider.setLowerAngLimit(-LIFT_EPS);
//// m_forkSlider.setUpperAngLimit(LIFT_EPS);
//m_forkSlider.SetLowerAngLimit(0.0f);
//m_forkSlider.SetUpperAngLimit(0.0f);
//IndexedMatrix localAVec = IndexedMatrix.Identity;
//IndexedMatrix localBVec = IndexedMatrix.Identity;
//m_forkSlider2 = new HingeConstraint(m_liftBody, m_forkBody, ref localAVec, ref localBVec);
//m_dynamicsWorld.AddConstraint(m_forkSlider, true);
//m_dynamicsWorld.addConstraint(m_forkSlider2, true);
CompoundShape loadCompound = new CompoundShape(true);
m_collisionShapes.Add(loadCompound);
CollisionShape loadShapeA = new BoxShape(new IndexedVector3(2.0f, 0.5f, 0.5f));
m_collisionShapes.Add(loadShapeA);
IndexedMatrix loadTrans = IndexedMatrix.Identity;
loadCompound.AddChildShape(ref loadTrans, loadShapeA);
CollisionShape loadShapeB = new BoxShape(new IndexedVector3(0.1f, 1.0f, 1.0f));
m_collisionShapes.Add(loadShapeB);
loadTrans = IndexedMatrix.CreateTranslation(2.1f, 0.0f, 0.0f);
loadCompound.AddChildShape(ref loadTrans, loadShapeB);
CollisionShape loadShapeC = new BoxShape(new IndexedVector3(0.1f, 1.0f, 1.0f));
m_collisionShapes.Add(loadShapeC);
loadTrans = IndexedMatrix.CreateTranslation(-2.1f, 0.0f, 0.0f);
loadCompound.AddChildShape(ref loadTrans, loadShapeC);
m_loadStartPos = new IndexedVector3(0.0f, -3.5f, 7.0f);
loadTrans = IndexedMatrix.CreateTranslation(m_loadStartPos);
m_loadBody = LocalCreateRigidBody(4f, ref loadTrans, loadCompound);
#if false
{
CollisionShape liftShape = new BoxShape(new IndexedVector3(0.5f, 2.0f, 0.05f));
m_collisionShapes.Add(liftShape);
IndexedMatrix liftTrans = IndexedMatrix.CreateTranslation(m_liftStartPos);
m_liftBody = localCreateRigidBody(10f, ref liftTrans, liftShape);
IndexedMatrix localA = MathUtil.setEulerZYX(0f, MathUtil.SIMD_HALF_PI, 0f);
localA._origin = new IndexedVector3(0f, 1.0f, 3.05f);
IndexedMatrix localB = MathUtil.setEulerZYX(0f, MathUtil.SIMD_HALF_PI, 0f);
localB._origin = new IndexedVector3(0f, -1.5f, -0.05f);
m_liftHinge = new HingeConstraint(m_carChassis, m_liftBody, ref localA, ref localB);
// m_liftHinge.setLimit(-LIFT_EPS, LIFT_EPS);
m_liftHinge.setLimit(0.0f, 0.0f);
m_dynamicsWorld.addConstraint(m_liftHinge, true);
CollisionShape forkShapeA = new BoxShape(new IndexedVector3(1.0f, 0.1f, 0.1f));
m_collisionShapes.Add(forkShapeA);
CompoundShape forkCompound = new CompoundShape();
m_collisionShapes.Add(forkCompound);
IndexedMatrix forkLocalTrans = IndexedMatrix.Identity;
forkCompound.addChildShape(ref forkLocalTrans, forkShapeA);
CollisionShape forkShapeB = new BoxShape(new IndexedVector3(0.1f, 0.02f, 0.6f));
m_collisionShapes.Add(forkShapeB);
forkLocalTrans = IndexedMatrix.CreateTranslation(-0.9f, -0.08f, 0.7f);
forkCompound.addChildShape(ref forkLocalTrans, forkShapeB);
CollisionShape forkShapeC = new BoxShape(new IndexedVector3(0.1f, 0.02f, 0.6f));
m_collisionShapes.Add(forkShapeC);
forkLocalTrans = IndexedMatrix.CreateTranslation(0.9f, -0.08f, 0.7f);
forkCompound.addChildShape(ref forkLocalTrans, forkShapeC);
m_forkStartPos = new IndexedVector3(0.0f, 0.6f, 3.2f);
IndexedMatrix forkTrans = IndexedMatrix.CreateTranslation(m_forkStartPos);
m_forkBody = localCreateRigidBody(5f, ref forkTrans, forkCompound);
localA = MathUtil.setEulerZYX(0f, 0f, MathUtil.SIMD_HALF_PI);
localA._origin = new IndexedVector3(0.0f, -1.9f, 0.05f);
localB = MathUtil.setEulerZYX(0f, 0f, MathUtil.SIMD_HALF_PI);
localB._origin = new IndexedVector3(0.0f, 0.0f, -0.1f);
m_forkSlider = new SliderConstraint(m_liftBody, m_forkBody, ref localA, ref localB, true);
m_forkSlider.setLowerLinLimit(0.1f);
m_forkSlider.setUpperLinLimit(0.1f);
// m_forkSlider.setLowerAngLimit(-LIFT_EPS);
// m_forkSlider.setUpperAngLimit(LIFT_EPS);
m_forkSlider.setLowerAngLimit(0.0f);
m_forkSlider.setUpperAngLimit(0.0f);
m_dynamicsWorld.addConstraint(m_forkSlider, true);
CompoundShape loadCompound = new CompoundShape();
m_collisionShapes.Add(loadCompound);
CollisionShape loadShapeA = new BoxShape(new IndexedVector3(2.0f, 0.5f, 0.5f));
m_collisionShapes.Add(loadShapeA);
IndexedMatrix loadTrans = IndexedMatrix.Identity;
loadCompound.addChildShape(ref loadTrans, loadShapeA);
CollisionShape loadShapeB = new BoxShape(new IndexedVector3(0.1f, 1.0f, 1.0f));
m_collisionShapes.Add(loadShapeB);
loadTrans = IndexedMatrix.CreateTranslation(2.1f, 0.0f, 0.0f);
loadCompound.addChildShape(ref loadTrans, loadShapeB);
CollisionShape loadShapeC = new BoxShape(new IndexedVector3(0.1f, 1.0f, 1.0f));
m_collisionShapes.Add(loadShapeC);
loadTrans = IndexedMatrix.CreateTranslation(-2.1f, 0.0f, 0.0f);
loadCompound.addChildShape(ref loadTrans, loadShapeC);
m_loadStartPos = new IndexedVector3(0.0f, -3.5f, 7.0f);
loadTrans = IndexedMatrix.CreateTranslation(m_loadStartPos);
m_loadBody = localCreateRigidBody(4f, ref loadTrans, loadCompound);
}
#endif
//m_carChassis.setDamping(0.2f, 0.2f);
ClientResetScene();
/// create vehicle
SetCameraDistance(26.0f);
SetTexturing(true);
SetShadows(true);
}
public override void InitializeDemo()
{
SetCameraDistance(30f);
//string filename = @"E:\users\man\bullet\xna-basic-output-1.txt";
//FileStream filestream = File.Open(filename, FileMode.Create, FileAccess.Write, FileShare.Read);
//BulletGlobals.g_streamWriter = new StreamWriter(filestream);
///collision configuration contains default setup for memory, collision setup
m_collisionConfiguration = new DefaultCollisionConfiguration();
///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded)
m_dispatcher = new CollisionDispatcher(m_collisionConfiguration);
m_broadphase = new DbvtBroadphase();
IOverlappingPairCache pairCache = null;
//pairCache = new SortedOverlappingPairCache();
m_broadphase = new SimpleBroadphase(1000, pairCache);
///the default constraint solver. For parallel processing you can use a different solver (see Extras/BulletMultiThreaded)
SequentialImpulseConstraintSolver sol = new SequentialImpulseConstraintSolver();
m_constraintSolver = sol;
m_dynamicsWorld = new DiscreteDynamicsWorld(m_dispatcher, m_broadphase, m_constraintSolver, m_collisionConfiguration);
IndexedVector3 gravity = new IndexedVector3(0, -10, 0);
m_dynamicsWorld.SetGravity(ref gravity);
///create a few basic rigid bodies
IndexedVector3 halfExtents = new IndexedVector3(50, 50, 50);
//IndexedVector3 halfExtents = new IndexedVector3(10, 10, 10);
CollisionShape groundShape = new BoxShape(ref halfExtents);
//CollisionShape groundShape = new StaticPlaneShape(new IndexedVector3(0,1,0), 50);
m_collisionShapes.Add(groundShape);
IndexedMatrix groundTransform = IndexedMatrix.CreateTranslation(new IndexedVector3(0, -50, 0));
//IndexedMatrix groundTransform = IndexedMatrix.CreateTranslation(new IndexedVector3(0,-10,0));
float mass = 0f;
LocalCreateRigidBody(mass, ref groundTransform, groundShape);
{
int numBlocksTall = 10; //18; //How many 'stories' tall.
float blockWidth = 6f; //Total width/length of the tower.
float blockHeight = 2f;
//create a few dynamic rigidbodies
IndexedVector3 extents = new IndexedVector3(blockWidth / 3, blockHeight, blockWidth);
IndexedVector3 boxHalfExtents = extents * 0.5f;
CollisionShape colShape = new BoxShape(boxHalfExtents);
//btCollisionShape* colShape = new btSphereShape(btScalar(1.));
//CollisionShape colShape = new CylinderShape(new IndexedVector3(1f, 1, 1f));
//m_collisionShapes.Add(colShape);
/// Create Dynamic Objects
IndexedMatrix startTransform = IndexedMatrix.Identity;
mass = 1f;
//rigidbody is dynamic if and only if mass is non zero, otherwise static
bool isDynamic = mass != 0f;
IndexedVector3 localInertia = IndexedVector3.Zero;
if (isDynamic)
{
colShape.CalculateLocalInertia(mass, out localInertia);
}
//The default number of iterations is 10, which works fine, but this demo
//is all about stability (it's jenga!). Increase the iterations a bit.
//Even though it's using twice as many iterations, it will early-out
//before reaching the limit MOST of the time.
//It's still pretty playable at around 7-8 max iterations, though.
IndexedMatrix transform = IndexedMatrix.Identity;
for (int i = 0; i < numBlocksTall; i++)
{
if (i % 2 == 0)
{
for (int j = 0; j < 3; j++)
{
transform = IndexedMatrix.Identity;
IndexedVector3 position = new IndexedVector3(j * (blockWidth / 3) - blockWidth / 3,
blockHeight / 2 + i * (blockHeight),
0);
//position += boxHalfExtents;
transform._origin = position;
RigidBody rb = LocalCreateRigidBody(mass, transform, colShape);
rb.SetActivationState(ActivationState.ISLAND_SLEEPING);
}
}
else
{
transform = IndexedMatrix.CreateRotationY(MathUtil.SIMD_HALF_PI);
for (int j = 0; j < 3; j++)
{
IndexedVector3 position = new IndexedVector3(0,
blockHeight / 2 + (i) * (blockHeight),
j * (blockWidth / 3) - blockWidth / 3f);
transform._origin = position;
//position += boxHalfExtents;
transform._origin = position;
RigidBody rb = LocalCreateRigidBody(mass, transform, colShape);
rb.SetActivationState(ActivationState.ISLAND_SLEEPING);
}
}
}
//game.Camera.Position = new Vector3(0, 5, 15);
}
ClientResetScene();
}
public override void InitializeDemo()
{
//maxiterations = 10;
SetCameraDistance(SCALING * 50f);
//string filename = @"E:\users\man\bullet\xna-basic-output-1.txt";
//FileStream filestream = File.Open(filename, FileMode.Create, FileAccess.Write, FileShare.Read);
//BulletGlobals.g_streamWriter = new StreamWriter(filestream);
///collision configuration contains default setup for memory, collision setup
m_collisionConfiguration = new DefaultCollisionConfiguration();
///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded)
m_dispatcher = new CollisionDispatcher(m_collisionConfiguration);
IndexedVector3 worldMin = new IndexedVector3(-1000, -1000, -1000);
IndexedVector3 worldMax = -worldMin;
m_broadphase = new AxisSweep3Internal(ref worldMin, ref worldMax, 0xfffe, 0xffff, 16384, null, false);
//m_broadphase = new DbvtBroadphase();
IOverlappingPairCache pairCache = null;
//pairCache = new SortedOverlappingPairCache();
//m_broadphase = new SimpleBroadphase(1000, pairCache);
///the default constraint solver. For parallel processing you can use a different solver (see Extras/BulletMultiThreaded)
SequentialImpulseConstraintSolver sol = new SequentialImpulseConstraintSolver();
m_constraintSolver = sol;
m_dynamicsWorld = new DiscreteDynamicsWorld(m_dispatcher, m_broadphase, m_constraintSolver, m_collisionConfiguration);
IndexedVector3 gravity = new IndexedVector3(0, -10, 0);
m_dynamicsWorld.SetGravity(ref gravity);
///create a few basic rigid bodies
IndexedVector3 halfExtents = new IndexedVector3(50, 50, 50);
//IndexedVector3 halfExtents = new IndexedVector3(10, 10, 10);
CollisionShape groundShape = new BoxShape(ref halfExtents);
//CollisionShape groundShape = new StaticPlaneShape(new IndexedVector3(0,1,0), 50);
m_collisionShapes.Add(groundShape);
IndexedMatrix groundTransform = IndexedMatrix.CreateTranslation(new IndexedVector3(0, -50, 0));
//IndexedMatrix groundTransform = IndexedMatrix.CreateTranslation(new IndexedVector3(0,-10,0));
float mass = 0f;
LocalCreateRigidBody(mass, ref groundTransform, groundShape);
{
//create a few dynamic rigidbodies
CollisionShape colShape = new BoxShape(new IndexedVector3(SCALING, SCALING, SCALING));
//CollisionShape colShape = BuildCorner();
//btCollisionShape* colShape = new btSphereShape(btScalar(1.));
//CollisionShape colShape = new CylinderShape(new IndexedVector3(1f, 1, 1f));
m_collisionShapes.Add(colShape);
/// Create Dynamic Objects
IndexedMatrix startTransform = IndexedMatrix.Identity;
mass = 1f;
//rigidbody is dynamic if and only if mass is non zero, otherwise static
bool isDynamic = mass != 0f;
IndexedVector3 localInertia = IndexedVector3.Zero;
if (isDynamic)
{
colShape.CalculateLocalInertia(mass, out localInertia);
}
float start_x = START_POS_X - ARRAY_SIZE_X / 2;
float start_y = START_POS_Y;
float start_z = START_POS_Z - ARRAY_SIZE_Z / 2;
for (int k = 0; k < ARRAY_SIZE_Y; k++)
{
for (int i = 0; i < ARRAY_SIZE_X; i++)
{
for (int j = 0; j < ARRAY_SIZE_Z; j++)
{
startTransform._origin = (new IndexedVector3(2.0f * i + start_x, 20 + 2.0f * k + start_y, 2.0f * j + start_z) * SCALING);
//using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
DefaultMotionState myMotionState = new DefaultMotionState(startTransform, IndexedMatrix.Identity);
RigidBodyConstructionInfo rbInfo = new RigidBodyConstructionInfo(mass, myMotionState, colShape, localInertia);
RigidBody body = new RigidBody(rbInfo);
//body->setContactProcessingThreshold(colShape->getContactBreakingThreshold());
//body.SetActivationState(ActivationState.ISLAND_SLEEPING);
m_dynamicsWorld.AddRigidBody(body);
//body.SetActivationState(ActivationState.ISLAND_SLEEPING);
body.SetUserPointer(String.Format("Box X{0} Y{1} Z{2}", k, i, j));
}
}
}
}
//ClientResetScene();
}
public override void InitializeDemo()
{
//string filename = @"E:\users\man\bullet\xna-concaveray-output-1.txt";
//FileStream filestream = File.Open(filename, FileMode.Create, FileAccess.Write, FileShare.Read);
//BulletGlobals.g_streamWriter = new StreamWriter(filestream);
m_cameraDistance = 400f;
m_farClip = 1500f;
m_perspective = IndexedMatrix.CreatePerspectiveFieldOfView(MathHelper.ToRadians(40.0f), m_aspect, m_nearClip, m_farClip);
m_animatedMesh = true;
base.InitializeDemo();
int totalTriangles = 2 * (NUM_VERTS_X - 1) * (NUM_VERTS_Y - 1);
gVertices = new ObjectArray <IndexedVector3>(totalVerts);
int indicesTotal = totalTriangles * 3;
gIndices = new ObjectArray <int>(indicesTotal);
BulletGlobals.gContactAddedCallback = null;
SetVertexPositions(waveheight, 0f);
int vertStride = 1;
int indexStride = 3;
int index = 0;
//for (int i=0;i<NUM_VERTS_X-1;i++)
//{
// for (int j=0;j<NUM_VERTS_Y-1;j++)
// {
// gIndices[index++] = j * NUM_VERTS_X + i;
// gIndices[index++] = (j + 1) * NUM_VERTS_X + i + 1;
// gIndices[index++] = j * NUM_VERTS_X + i + 1;
// gIndices[index++] = j * NUM_VERTS_X + i;
// gIndices[index++] = (j + 1) * NUM_VERTS_X + i;
// gIndices[index++] = (j + 1) * NUM_VERTS_X + i + 1;
// }
//}
for (int i = 0; i < NUM_VERTS_X - 1; i++)
{
for (int j = 0; j < NUM_VERTS_Y - 1; j++)
{
gIndices[index++] = j * NUM_VERTS_X + i;
gIndices[index++] = j * NUM_VERTS_X + i + 1;
gIndices[index++] = (j + 1) * NUM_VERTS_X + i + 1;
gIndices[index++] = j * NUM_VERTS_X + i;
gIndices[index++] = (j + 1) * NUM_VERTS_X + i + 1;
gIndices[index++] = (j + 1) * NUM_VERTS_X + i;
}
}
TriangleIndexVertexArray indexVertexArrays = new TriangleIndexVertexArray(totalTriangles,
gIndices, indexStride, totalVerts, gVertices, vertStride);
bool useQuantizedAabbCompression = true;
float minX = NUM_VERTS_X * TRIANGLE_SIZE * 0.5f;
float minZ = NUM_VERTS_Y * TRIANGLE_SIZE * 0.5f;
//OptimizedBvh bvh = new OptimizedBvh();
IndexedVector3 aabbMin = new IndexedVector3(-minX, -5, -minZ);
IndexedVector3 aabbMax = new IndexedVector3(minX, 5, minZ);
m_trimeshShape = new BvhTriangleMeshShape(indexVertexArrays, useQuantizedAabbCompression, ref aabbMin, ref aabbMax, true);
//m_trimeshShape = new BvhTriangleMeshShape(indexVertexArrays, useQuantizedAabbCompression,true);
IndexedVector3 scaling = IndexedVector3.One;
CollisionShape groundShape = m_trimeshShape;
//groundShape = new TriangleMeshShape(indexVertexArrays);
//groundShape = new StaticPlaneShape(IndexedVector3.Up, 0f);
IndexedVector3 up = new IndexedVector3(0.4f, 1, 0);
up.Normalize();
//groundShape = new StaticPlaneShape(up, 0f);
//groundShape = new BoxShape(new IndexedVector3(1000, 10, 1000));
m_collisionConfiguration = new DefaultCollisionConfiguration();
m_dispatcher = new CollisionDispatcher(m_collisionConfiguration);
IndexedVector3 worldMin = aabbMin;
IndexedVector3 worldMax = aabbMax;
m_broadphase = new AxisSweep3Internal(ref worldMin, ref worldMax, 0xfffe, 0xffff, 16384, null, false);
m_constraintSolver = new SequentialImpulseConstraintSolver();
m_dynamicsWorld = new DiscreteDynamicsWorld(m_dispatcher, m_broadphase, m_constraintSolver, m_collisionConfiguration);
float mass = 0f;
IndexedMatrix startTransform = IndexedMatrix.CreateTranslation(new IndexedVector3(2, -2, 0));
startTransform = IndexedMatrix.Identity;
staticBody = LocalCreateRigidBody(mass, ref startTransform, groundShape);
staticBody.SetCollisionFlags(staticBody.GetCollisionFlags() | CollisionFlags.CF_KINEMATIC_OBJECT); //STATIC_OBJECT);
//enable custom material callback
staticBody.SetCollisionFlags(staticBody.GetCollisionFlags() | CollisionFlags.CF_CUSTOM_MATERIAL_CALLBACK);
//m_raycastBar = new btRaycastBar (m_debugDraw,4000.0f, 0.0f,-1000,30);
m_raycastBar = new btRaycastBar(m_debugDraw, 300.0f, 0.0f, -1000, 200, worldMin.X, worldMax.X);
m_raycastBar.min_x = -100;
m_raycastBar.max_x = -100;
ClientResetScene();
}
//----------------------------------------------------------------------------------------------------------------
public override void InitializeDemo()
{
CollisionShape groundShape = new BoxShape(new IndexedVector3(50, 0.1f, 50));
IndexedVector3 wheelDimensions = new IndexedVector3(wheelWidth, wheelRadius, wheelRadius);
m_wheelShape = new CylinderShapeX(ref wheelDimensions);
m_collisionShapes.Add(groundShape);
m_collisionConfiguration = new DefaultCollisionConfiguration();
m_dispatcher = new CollisionDispatcher(m_collisionConfiguration);
IndexedVector3 worldMin = new IndexedVector3(-1000, -1000, -1000);
IndexedVector3 worldMax = new IndexedVector3(1000, 1000, 1000);
m_broadphase = new AxisSweep3Internal(ref worldMin, ref worldMax, 0xfffe, 0xffff, 16384, null, false);
m_constraintSolver = new SequentialImpulseConstraintSolver();
m_dynamicsWorld = new DiscreteDynamicsWorld(m_dispatcher, m_broadphase, m_constraintSolver, m_collisionConfiguration);
IndexedMatrix tr = IndexedMatrix.CreateTranslation(0, -10, 0);
LocalCreateRigidBody(0f, ref tr, groundShape);
#if true
CollisionShape chassisShape = new BoxShape(new IndexedVector3(1.0f, 0.5f, 2.0f));
//CollisionShape chassisShape = new BoxShape(new IndexedVector3(1.0f, 0.5f, 1.0f));
m_collisionShapes.Add(chassisShape);
CompoundShape compound = new CompoundShape();
m_collisionShapes.Add(compound);
//localTrans effectively shifts the center of mass with respect to the chassis
IndexedMatrix localTrans = IndexedMatrix.CreateTranslation(0, 1, 0);
compound.AddChildShape(ref localTrans, chassisShape);
{
CollisionShape suppShape = new BoxShape(new IndexedVector3(0.5f, 0.1f, 0.5f));
//localTrans effectively shifts the center of mass with respect to the chassis
IndexedMatrix suppLocalTrans = IndexedMatrix.CreateTranslation(0f, 1.0f, 2.5f);
compound.AddChildShape(ref suppLocalTrans, suppShape);
}
tr._origin = IndexedVector3.Zero;
m_carChassis = LocalCreateRigidBody(800f, ref tr, compound);//chassisShape);
#endif
{
#if true
CollisionShape liftShape = new BoxShape(new IndexedVector3(0.5f, 2.0f, 0.05f));
m_collisionShapes.Add(liftShape);
m_liftStartPos = new IndexedVector3(0.0f, 2.5f, 3.05f);
IndexedMatrix liftTrans = IndexedMatrix.CreateTranslation(m_liftStartPos);
m_liftBody = LocalCreateRigidBody(10f, ref liftTrans, liftShape);
IndexedMatrix localA = MathUtil.SetEulerZYX(0f, MathUtil.SIMD_HALF_PI, 0f);
localA._origin = new IndexedVector3(0f, 1.0f, 3.05f);
IndexedMatrix localB = MathUtil.SetEulerZYX(0f, MathUtil.SIMD_HALF_PI, 0f);
localB._origin = new IndexedVector3(0f, -1.5f, -0.05f);
m_liftHinge = new HingeConstraint(m_carChassis, m_liftBody, ref localA, ref localB);
MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "hinge aFrame", m_liftHinge.GetAFrame());
MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "hinge bFrame", m_liftHinge.GetBFrame());
m_liftHinge.SetLimit(0.0f, 0.0f);
m_dynamicsWorld.AddConstraint(m_liftHinge, true);
CollisionShape forkShapeA = new BoxShape(new IndexedVector3(1.0f, 0.1f, 0.1f));
m_collisionShapes.Add(forkShapeA);
CompoundShape forkCompound = new CompoundShape();
m_collisionShapes.Add(forkCompound);
IndexedMatrix forkLocalTrans = IndexedMatrix.Identity;
forkCompound.AddChildShape(ref forkLocalTrans, forkShapeA);
CollisionShape forkShapeB = new BoxShape(new IndexedVector3(0.1f, 0.02f, 0.6f));
m_collisionShapes.Add(forkShapeB);
forkLocalTrans = IndexedMatrix.CreateTranslation(-0.9f, -0.08f, 0.7f);
forkCompound.AddChildShape(ref forkLocalTrans, forkShapeB);
CollisionShape forkShapeC = new BoxShape(new IndexedVector3(0.1f, 0.02f, 0.6f));
m_collisionShapes.Add(forkShapeC);
forkLocalTrans = IndexedMatrix.CreateTranslation(0.9f, -0.08f, 0.7f);
forkCompound.AddChildShape(ref forkLocalTrans, forkShapeC);
m_forkStartPos = new IndexedVector3(0.0f, 0.6f, 3.2f);
IndexedMatrix forkTrans = IndexedMatrix.CreateTranslation(m_forkStartPos);
m_forkBody = LocalCreateRigidBody(5f, ref forkTrans, forkCompound);
localA = IndexedMatrix.Identity;
localB = IndexedMatrix.Identity;
localA._basis.SetEulerZYX(0, 0, MathUtil.SIMD_HALF_PI);
localA._origin = new IndexedVector3(0.0f, -1.9f, 0.05f);
localB._basis.SetEulerZYX(0, 0, MathUtil.SIMD_HALF_PI);
localB._origin = new IndexedVector3(0.0f, 0.0f, -0.1f);
m_forkSlider = new SliderConstraint(m_liftBody, m_forkBody, ref localA, ref localB, true);
m_forkSlider.SetLowerLinLimit(0.1f);
m_forkSlider.SetUpperLinLimit(0.1f);
// m_forkSlider.setLowerAngLimit(-LIFT_EPS);
// m_forkSlider.setUpperAngLimit(LIFT_EPS);
m_forkSlider.SetLowerAngLimit(0.0f);
m_forkSlider.SetUpperAngLimit(0.0f);
m_dynamicsWorld.AddConstraint(m_forkSlider, true);
#endif
#if true
CompoundShape loadCompound = new CompoundShape(false);
m_collisionShapes.Add(loadCompound);
CollisionShape loadShapeA = new BoxShape(new IndexedVector3(2.0f, 0.5f, 0.5f));
m_collisionShapes.Add(loadShapeA);
IndexedMatrix loadTrans = IndexedMatrix.Identity;
loadCompound.AddChildShape(ref loadTrans, loadShapeA);
CollisionShape loadShapeB = new BoxShape(new IndexedVector3(0.1f, 1.0f, 1.0f));
m_collisionShapes.Add(loadShapeB);
loadTrans = IndexedMatrix.CreateTranslation(2.1f, 0.0f, 0.0f);
loadCompound.AddChildShape(ref loadTrans, loadShapeB);
CollisionShape loadShapeC = new BoxShape(new IndexedVector3(0.1f, 1.0f, 1.0f));
m_collisionShapes.Add(loadShapeC);
loadTrans = IndexedMatrix.CreateTranslation(-2.1f, 0.0f, 0.0f);
loadCompound.AddChildShape(ref loadTrans, loadShapeC);
m_loadStartPos = new IndexedVector3(0.0f, -3.5f, 7.0f);
loadTrans = IndexedMatrix.CreateTranslation(m_loadStartPos);
m_loadBody = LocalCreateRigidBody(4f, ref loadTrans, loadCompound);
#endif
}
//m_carChassis.setDamping(0.2f, 0.2f);
ClientResetScene();
/// create vehicle
{
m_vehicleRayCaster = new DefaultVehicleRaycaster(m_dynamicsWorld);
m_vehicle = new RaycastVehicle(m_tuning, m_carChassis, m_vehicleRayCaster);
///never deactivate the vehicle
m_carChassis.SetActivationState(ActivationState.DISABLE_DEACTIVATION);
m_dynamicsWorld.AddVehicle(m_vehicle);
float connectionHeight = 1.2f;
bool isFrontWheel = true;
//choose coordinate system
m_vehicle.SetCoordinateSystem(rightIndex, upIndex, forwardIndex);
IndexedVector3 connectionPointCS0 = IndexedVector3.Zero;
//connectionPointCS0 = new IndexedVector3(CUBE_HALF_EXTENTS, connectionHeight, CUBE_HALF_EXTENTS);
//m_vehicle.addWheel(ref connectionPointCS0, ref wheelDirectionCS0, ref wheelAxleCS, suspensionRestLength, wheelRadius, m_tuning, isFrontWheel);
connectionPointCS0 = new IndexedVector3(CUBE_HALF_EXTENTS - (0.3f * wheelWidth), connectionHeight, 2.0f * CUBE_HALF_EXTENTS - wheelRadius);
m_vehicle.AddWheel(ref connectionPointCS0, ref wheelDirectionCS0, ref wheelAxleCS, suspensionRestLength, wheelRadius, m_tuning, isFrontWheel);
connectionPointCS0 = new IndexedVector3(-CUBE_HALF_EXTENTS + (0.3f * wheelWidth), connectionHeight, 2.0f * CUBE_HALF_EXTENTS - wheelRadius);
m_vehicle.AddWheel(ref connectionPointCS0, ref wheelDirectionCS0, ref wheelAxleCS, suspensionRestLength, wheelRadius, m_tuning, isFrontWheel);
//isFrontWheel = false;
connectionPointCS0 = new IndexedVector3(-CUBE_HALF_EXTENTS + (0.3f * wheelWidth), connectionHeight, -2.0f * CUBE_HALF_EXTENTS + wheelRadius);
m_vehicle.AddWheel(ref connectionPointCS0, ref wheelDirectionCS0, ref wheelAxleCS, suspensionRestLength, wheelRadius, m_tuning, isFrontWheel);
connectionPointCS0 = new IndexedVector3(CUBE_HALF_EXTENTS - (0.3f * wheelWidth), connectionHeight, -2.0f * CUBE_HALF_EXTENTS + wheelRadius);
m_vehicle.AddWheel(ref connectionPointCS0, ref wheelDirectionCS0, ref wheelAxleCS, suspensionRestLength, wheelRadius, m_tuning, isFrontWheel);
for (int i = 0; i < m_vehicle.GetNumWheels(); i++)
{
WheelInfo wheel = m_vehicle.GetWheelInfo(i);
wheel.m_suspensionStiffness = suspensionStiffness;
wheel.m_wheelsDampingRelaxation = suspensionDamping;
wheel.m_wheelsDampingCompression = suspensionCompression;
wheel.m_frictionSlip = wheelFriction;
wheel.m_rollInfluence = rollInfluence;
}
}
SetCameraDistance(36.0f);
SetTexturing(true);
SetShadows(true);
}
//----------------------------------------------------------------------------------------------------------------
public override void ClientResetScene()
{
base.ClientResetScene();
gVehicleSteering = 0.0f;
gBreakingForce = defaultBreakingForce;
gEngineForce = 0.0f;
IndexedMatrix ident = IndexedMatrix.Identity;
m_carChassis.SetCenterOfMassTransform(ref ident);
IndexedVector3 zero = IndexedVector3.Zero;
m_carChassis.SetLinearVelocity(ref zero);
m_carChassis.SetAngularVelocity(ref zero);
m_dynamicsWorld.GetBroadphase().GetOverlappingPairCache().CleanProxyFromPairs(m_carChassis.GetBroadphaseHandle(), GetDynamicsWorld().GetDispatcher());
if (m_vehicle != null)
{
m_vehicle.ResetSuspension();
for (int i = 0; i < m_vehicle.GetNumWheels(); i++)
{
//synchronize the wheels with the (interpolated) chassis worldtransform
m_vehicle.UpdateWheelTransform(i, true);
}
}
if (m_liftBody != null)
{
IndexedMatrix liftTrans = IndexedMatrix.CreateTranslation(m_liftStartPos);
m_liftBody.Activate();
m_liftBody.SetCenterOfMassTransform(ref liftTrans);
m_liftBody.SetLinearVelocity(ref zero);
m_liftBody.SetAngularVelocity(ref zero);
}
if (m_forkBody != null)
{
IndexedMatrix forkTrans = IndexedMatrix.CreateTranslation(m_forkStartPos);
m_forkBody.Activate();
m_forkBody.SetCenterOfMassTransform(ref forkTrans);
m_forkBody.SetLinearVelocity(ref zero);
m_forkBody.SetAngularVelocity(ref zero);
}
if (m_liftHinge != null)
{
// m_liftHinge.setLimit(-LIFT_EPS, LIFT_EPS);
m_liftHinge.SetLimit(0.0f, 0.0f);
m_liftHinge.EnableAngularMotor(false, 0, 0);
}
if (m_forkSlider != null)
{
m_forkSlider.SetLowerLinLimit(0.1f);
m_forkSlider.SetUpperLinLimit(0.1f);
m_forkSlider.SetPoweredLinMotor(false);
}
if (m_loadBody != null)
{
IndexedMatrix loadTrans = IndexedMatrix.CreateTranslation(m_loadStartPos);
m_loadBody.Activate();
m_loadBody.SetCenterOfMassTransform(ref loadTrans);
m_loadBody.SetLinearVelocity(ref zero);
m_loadBody.SetAngularVelocity(ref zero);
}
}