//----------------------------------------------------------------------------------------------
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()
{
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();
}
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();
}
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 bool GetSphereDistance(CollisionObject boxObj, ref IndexedVector3 pointOnBox, ref IndexedVector3 normal, ref float penetrationDepth, IndexedVector3 sphereCenter, float fRadius, float maxContactDistance)
{
BoxShape boxShape = boxObj.GetCollisionShape() as BoxShape;
IndexedVector3 boxHalfExtent = boxShape.GetHalfExtentsWithoutMargin();
float boxMargin = boxShape.GetMargin();
penetrationDepth = 1.0f;
// convert the sphere position to the box's local space
IndexedMatrix m44T = boxObj.GetWorldTransform();
IndexedVector3 sphereRelPos = m44T.InvXform(sphereCenter);
// Determine the closest point to the sphere center in the box
IndexedVector3 closestPoint = sphereRelPos;
closestPoint.X = (Math.Min(boxHalfExtent.X, closestPoint.X));
closestPoint.X = (Math.Max(-boxHalfExtent.X, closestPoint.X));
closestPoint.Y = (Math.Min(boxHalfExtent.Y, closestPoint.Y));
closestPoint.Y = (Math.Max(-boxHalfExtent.Y, closestPoint.Y));
closestPoint.Z = (Math.Min(boxHalfExtent.Z, closestPoint.Z));
closestPoint.Z = (Math.Max(-boxHalfExtent.Z, closestPoint.Z));
float intersectionDist = fRadius + boxMargin;
float contactDist = intersectionDist + maxContactDistance;
normal = sphereRelPos - closestPoint;
//if there is no penetration, we are done
float dist2 = normal.LengthSquared();
if (dist2 > contactDist * contactDist)
{
return(false);
}
float distance;
//special case if the sphere center is inside the box
if (dist2 == 0.0f)
{
distance = -GetSpherePenetration(ref boxHalfExtent, ref sphereRelPos, ref closestPoint, ref normal);
}
else //compute the penetration details
{
distance = normal.Length();
normal /= distance;
}
pointOnBox = closestPoint + normal * boxMargin;
// v3PointOnSphere = sphereRelPos - (normal * fRadius);
penetrationDepth = distance - intersectionDist;
// transform back in world space
IndexedVector3 tmp = m44T * pointOnBox;
pointOnBox = tmp;
// tmp = m44T(v3PointOnSphere);
// v3PointOnSphere = tmp;
tmp = m44T._basis * normal;
normal = tmp;
return(true);
}
private void BuildBoard()
{
float width = 40;
float height = 40;
IndexedVector3 boardBackExtents = new IndexedVector3(width,height,2) /2f;
IndexedVector3 boardSideExtents = new IndexedVector3(1,height,2) /2f;
IndexedVector3 boardBarExtents = new IndexedVector3(width,1,2)/2f;
IndexedVector3 pinExtent = new IndexedVector3(0.5f,1f,2f);
IndexedVector3 boardCenter = new IndexedVector3(0,0,0);
// build frame.objects
BoxShape boardBack = new BoxShape(boardBackExtents);
BoxShape boardSide = new BoxShape(boardSideExtents);
BoxShape boardBar = new BoxShape(boardBarExtents);
StaticPlaneShape boardFront = new StaticPlaneShape(new IndexedVector3(0,0,-1),-(boardBackExtents.Z+boardSideExtents.Z));
CollisionShape pinShape = new CapsuleShapeZ(pinExtent.Y,pinExtent.Z);
//pinShape = new BoxShape(pinExtent);
//pinShape = new SphereShape(pinExtent.Y);
m_cameraUp = Vector3.Up;
// now RB's
Matrix m = Matrix.Identity;
IndexedMatrix trans = m;
trans._origin = boardCenter;
float mass = 0f;
//LocalCreateRigidBody(mass,trans,boardBack);
//LocalCreateRigidBody(mass, trans, boardFront);
IndexedVector3 leftSide = new IndexedVector3(boardCenter.X - boardBackExtents.X + boardSideExtents.X, boardCenter.Y, boardCenter.Z + boardSideExtents.Z);
IndexedVector3 rightSide = new IndexedVector3(boardCenter.X + boardBackExtents.X - boardSideExtents.X, boardCenter.Y, boardCenter.Z + boardSideExtents.Z);
IndexedVector3 topBar = new IndexedVector3(boardCenter.X, boardCenter.Y + boardBackExtents.Y - boardBarExtents.Y, boardCenter.Z + boardSideExtents.Z);
IndexedVector3 bottomBar = new IndexedVector3(boardCenter.X, boardCenter.Y - boardBackExtents.Y + boardBarExtents.Y, boardCenter.Z + boardSideExtents.Z);
trans._origin = leftSide;
LocalCreateRigidBody(mass, trans, boardSide);
trans._origin = rightSide;
LocalCreateRigidBody(mass, trans, boardSide);
trans._origin = topBar;
LocalCreateRigidBody(mass, trans, boardBar);
trans._origin = bottomBar;
LocalCreateRigidBody(mass, trans, boardBar);
// now place the pins? (simple offset grid to start)
int numPinsX = 8;
int numPinsY = 8;
// fixme
IndexedVector3 pinTopLeft = new IndexedVector3(-boardBackExtents.X + 4, boardBackExtents.Y - 5, boardCenter.Z + boardSideExtents.Z);
IndexedVector3 pinSpacer = new IndexedVector3(pinExtent.Y * 4f, -pinExtent.Y * 4f, 0);
float ballRadius = 0.9f;
float fudge = 3f;
m_dropSphereShape = new SphereShape(ballRadius);
m_ballDropSpot = new Vector3(1f, pinTopLeft.Y + 1, boardCenter.Z + boardBackExtents.Z+ballRadius);
//new SphereShape(0.95f);
m_debugDraw.SetDebugMode(m_debugDraw.GetDebugMode() | DebugDrawModes.DBG_DrawAabb);
trans._origin = pinTopLeft;
LocalCreateRigidBody(mass, trans, pinShape);
for (int i = 0; i < numPinsX; ++i)
{
for (int j = 0; j < numPinsY; ++j)
{
IndexedVector3 pos = new IndexedVector3(pinSpacer.X * i, (pinSpacer.Y * j), pinSpacer.Z);
// stagger rows.
if (j % 2 == 1)
{
pos.X += pinSpacer.X/2f;
}
trans._origin = pinTopLeft + pos;
LocalCreateRigidBody(mass, trans, pinShape);
}
}
}
public void InitializePhysicsBulletXNA()
{
#if BULLETXNA
CollisionFilterGroups colGroup = CollisionFilterGroups.DefaultFilter;
CollisionFilterGroups colMask = CollisionFilterGroups.AllFilter;
float sphereMass = 5f;
CollisionShape sphereShape = new Bullet.BulletCollision.SphereShape(1);
Bullet.LinearMath.IndexedVector3 sphereLocalInertia;
sphereShape.CalculateLocalInertia(sphereMass,out sphereLocalInertia);
BulletXNAObject ball = new BulletXNAObject(this, "Models/Sphere", sphereMass, new Bullet.DefaultMotionState(Matrix.CreateTranslation(camera.Position + new Vector3(0, 10, -5)), Matrix.Identity),sphereShape , sphereLocalInertia, colGroup, colMask);
ball.TranslateAA(camera.Position + new Vector3(0, 10, 0));
ball.TextureMaterials.Add("Textures/core1");
ball.NormalMaterials.Add("Textures/core1Normal");
ball.RigidBody.SetFriction(1);
boxs.Add(ball);
Components.Add(ball);
// create a few dynamic rigidbodies
float mass = 1.0f;
Bullet.BulletCollision.CollisionShape colShape = new Bullet.BulletCollision.BoxShape(Vector3.One);
//CollisionShapes.Add(colShape);
Vector3 localInertia = Vector3.Zero;
Bullet.LinearMath.IndexedVector3 li = Bullet.LinearMath.IndexedVector3.Zero;
colShape.CalculateLocalInertia(mass, out li);
localInertia = li;
float start_x = StartPosX - ArraySizeX / 2;
float start_y = StartPosY;
float start_z = StartPosZ - ArraySizeZ / 2;
start_z -= 8;
Random rnd = new Random();
int k, i, j;
for (k = 0; k < ArraySizeY; k++)
{
for (i = 0; i < ArraySizeX; i++)
{
for (j = 0; j < ArraySizeZ; j++)
{
Matrix startTransform = Matrix.CreateTranslation(
2 * i + start_x,
2 * k + start_y,
2 * j + start_z
);
// using motionstate is recommended, it provides interpolation capabilities
// and only synchronizes 'active' objects
BulletXNAObject box = new BulletXNAObject(this, "Models/Box", mass, new Bullet.DefaultMotionState(startTransform, Matrix.Identity), colShape, localInertia, colGroup, colMask);
box.TextureMaterials.Add("Textures/BoxColor");
box.NormalMaterials.Add("Textures/BoxNormal");
box.TranslateAA(new Vector3(0, 7, 0));
boxs.Add(box);
Components.Add(box);
}
}
}
start_x -= 8;
for (k = 0; k < 2; k++)
{
for (i = 0; i < 2; i++)
{
for (j = 0; j < 2; j++)
{
Matrix startTransform = Matrix.CreateTranslation(
2 * i + start_x,
2 * k + start_y,
2 * j + start_z
);
// using motionstate is recommended, it provides interpolation capabilities
// and only synchronizes 'active' objects
BulletXNAObject box = new BulletXNAObject(this, "Models/Box", mass, new Bullet.DefaultMotionState(startTransform, Matrix.Identity), colShape, localInertia, colGroup, colMask);
box.TextureMaterials.Add("Textures/BoxColor01");
box.NormalMaterials.Add("Textures/BoxNormal01");
box.TranslateAA(new Vector3(0, 7, 0));
boxs.Add(box);
Components.Add(box);
}
}
}
IsPhysicsEnabled = false;
#endif
}
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 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()
{
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 BoxBoxDetector(BoxShape box1, BoxShape box2)
//{
// m_box1 = box1;
// m_box2 = box2;
//}
// Work in progress to copy redo the box detector to remove un-necessary allocations
public static void GetClosestPoints(BoxShape box1,BoxShape box2, ref ClosestPointInput input, ManifoldResult output, IDebugDraw debugDraw, bool swapResults)
{
IndexedMatrix transformA = input.m_transformA;
IndexedMatrix transformB = input.m_transformB;
#if DEBUG
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugBoxBoxDetector)
{
MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "BoxBox:GCP:transformA", transformA);
MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "BoxBox:GCP:transformB", transformB);
}
#endif
int skip = 0;
Object contact = null;
IndexedVector3 normal = new IndexedVector3();
float depth = 0f;
int return_code = -1;
int maxc = 4;
IndexedVector3 translationA = new IndexedVector3(transformA._origin);
IndexedVector3 translationB = new IndexedVector3(transformB._origin);
//IndexedVector3 debugExtents = new IndexedVector3(2f, 2f, 2f);
IndexedVector3 box1Margin = new IndexedVector3(2f * box1.GetHalfExtentsWithMargin());
IndexedVector3 box2Margin = new IndexedVector3(2f * box2.GetHalfExtentsWithMargin());
//IndexedVector3 box1Margin = 2f * debugExtents;
//IndexedVector3 box2Margin = 2f * debugExtents;
IndexedBasisMatrix rotateA = transformA._basis.Transpose();
IndexedBasisMatrix rotateB = transformB._basis.Transpose();
for (int j = 0; j < 3; j++)
{
s_temp1[0 + 4 * j] = transformA._basis[j].X;
s_temp2[0 + 4 * j] = transformB._basis[j].X;
s_temp1[1 + 4 * j] = transformA._basis[j].Y;
s_temp2[1 + 4 * j] = transformB._basis[j].Y;
s_temp1[2 + 4 * j] = transformA._basis[j].Z;
s_temp2[2 + 4 * j] = transformB._basis[j].Z;
}
//s_temp1[0] = rotateA._Row0.X;
//s_temp1[1] = rotateA._Row0.Y;
//s_temp1[2] = rotateA._Row0.Z;
//s_temp1[4] = rotateA._Row1.X;
//s_temp1[5] = rotateA._Row1.Y;
//s_temp1[6] = rotateA._Row1.Z;
//s_temp1[8] = rotateA._Row2.X;
//s_temp1[9] = rotateA._Row2.X;
//s_temp1[10] = rotateA._Row2.X;
//s_temp2[0] = rotateB._Row0.X;
//s_temp2[1] = rotateB._Row0.Y;
//s_temp2[2] = rotateB._Row0.Z;
//s_temp2[4] = rotateB._Row1.X;
//s_temp2[5] = rotateB._Row1.Y;
//s_temp2[6] = rotateB._Row1.Z;
//s_temp2[8] = rotateB._Row2.X;
//s_temp2[9] = rotateB._Row2.Y;
//s_temp2[10] = rotateB._Row2.Z;
DBoxBox2(ref translationA,
s_temp1,
ref box1Margin,
ref translationB,
s_temp2,
ref box2Margin,
ref normal, ref depth, ref return_code,
maxc, contact, skip,
output);
}
//-----------------------------------------------------------------------------------------------
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 BoxBoxDetector(BoxShape box1, BoxShape box2)
//{
// m_box1 = box1;
// m_box2 = box2;
//}
// Work in progress to copy redo the box detector to remove un-necessary allocations
public static void GetClosestPoints(BoxShape box1, BoxShape box2, ref ClosestPointInput input, ManifoldResult output, IDebugDraw debugDraw, bool swapResults)
{
IndexedMatrix transformA = input.m_transformA;
IndexedMatrix transformB = input.m_transformB;
#if DEBUG
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugBoxBoxDetector)
{
MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "BoxBox:GCP:transformA", transformA);
MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "BoxBox:GCP:transformB", transformB);
}
#endif
int skip = 0;
Object contact = null;
IndexedVector3 normal = new IndexedVector3();
float depth = 0f;
int return_code = -1;
int maxc = 4;
IndexedVector3 translationA = new IndexedVector3(transformA._origin);
IndexedVector3 translationB = new IndexedVector3(transformB._origin);
//IndexedVector3 debugExtents = new IndexedVector3(2f, 2f, 2f);
IndexedVector3 box1Margin = new IndexedVector3(2f * box1.GetHalfExtentsWithMargin());
IndexedVector3 box2Margin = new IndexedVector3(2f * box2.GetHalfExtentsWithMargin());
//IndexedVector3 box1Margin = 2f * debugExtents;
//IndexedVector3 box2Margin = 2f * debugExtents;
IndexedBasisMatrix rotateA = transformA._basis.Transpose();
IndexedBasisMatrix rotateB = transformB._basis.Transpose();
for (int j = 0; j < 3; j++)
{
s_temp1[0 + 4 * j] = transformA._basis[j].X;
s_temp2[0 + 4 * j] = transformB._basis[j].X;
s_temp1[1 + 4 * j] = transformA._basis[j].Y;
s_temp2[1 + 4 * j] = transformB._basis[j].Y;
s_temp1[2 + 4 * j] = transformA._basis[j].Z;
s_temp2[2 + 4 * j] = transformB._basis[j].Z;
}
//s_temp1[0] = rotateA._Row0.X;
//s_temp1[1] = rotateA._Row0.Y;
//s_temp1[2] = rotateA._Row0.Z;
//s_temp1[4] = rotateA._Row1.X;
//s_temp1[5] = rotateA._Row1.Y;
//s_temp1[6] = rotateA._Row1.Z;
//s_temp1[8] = rotateA._Row2.X;
//s_temp1[9] = rotateA._Row2.X;
//s_temp1[10] = rotateA._Row2.X;
//s_temp2[0] = rotateB._Row0.X;
//s_temp2[1] = rotateB._Row0.Y;
//s_temp2[2] = rotateB._Row0.Z;
//s_temp2[4] = rotateB._Row1.X;
//s_temp2[5] = rotateB._Row1.Y;
//s_temp2[6] = rotateB._Row1.Z;
//s_temp2[8] = rotateB._Row2.X;
//s_temp2[9] = rotateB._Row2.Y;
//s_temp2[10] = rotateB._Row2.Z;
DBoxBox2(ref translationA,
s_temp1,
ref box1Margin,
ref translationB,
s_temp2,
ref box2Margin,
ref normal, ref depth, ref return_code,
maxc, contact, skip,
output);
}
public void InitializePhysicsBulletXNA()
{
#if BULLETXNA
CollisionFilterGroups colGroup = CollisionFilterGroups.DefaultFilter;
CollisionFilterGroups colMask = CollisionFilterGroups.AllFilter;
float sphereMass = 5f;
CollisionShape sphereShape = new Bullet.BulletCollision.SphereShape(1);
Bullet.LinearMath.IndexedVector3 sphereLocalInertia;
sphereShape.CalculateLocalInertia(sphereMass, out sphereLocalInertia);
BulletXNAObject ball = new BulletXNAObject(this, "Models/Sphere", sphereMass, new Bullet.DefaultMotionState(Matrix.CreateTranslation(camera.Position + new Vector3(0, 10, -5)), Matrix.Identity), sphereShape, sphereLocalInertia, colGroup, colMask);
ball.TranslateAA(camera.Position + new Vector3(0, 10, 0));
ball.TextureMaterials.Add("Textures/core1");
ball.NormalMaterials.Add("Textures/core1Normal");
ball.RigidBody.SetFriction(1);
boxs.Add(ball);
Components.Add(ball);
// create a few dynamic rigidbodies
float mass = 1.0f;
Bullet.BulletCollision.CollisionShape colShape = new Bullet.BulletCollision.BoxShape(Vector3.One);
//CollisionShapes.Add(colShape);
Vector3 localInertia = Vector3.Zero;
Bullet.LinearMath.IndexedVector3 li = Bullet.LinearMath.IndexedVector3.Zero;
colShape.CalculateLocalInertia(mass, out li);
localInertia = li;
float start_x = StartPosX - ArraySizeX / 2;
float start_y = StartPosY;
float start_z = StartPosZ - ArraySizeZ / 2;
start_z -= 8;
Random rnd = new Random();
int k, i, j;
for (k = 0; k < ArraySizeY; k++)
{
for (i = 0; i < ArraySizeX; i++)
{
for (j = 0; j < ArraySizeZ; j++)
{
Matrix startTransform = Matrix.CreateTranslation(
2 * i + start_x,
2 * k + start_y,
2 * j + start_z
);
// using motionstate is recommended, it provides interpolation capabilities
// and only synchronizes 'active' objects
BulletXNAObject box = new BulletXNAObject(this, "Models/Box", mass, new Bullet.DefaultMotionState(startTransform, Matrix.Identity), colShape, localInertia, colGroup, colMask);
box.TextureMaterials.Add("Textures/BoxColor");
box.NormalMaterials.Add("Textures/BoxNormal");
box.TranslateAA(new Vector3(0, 7, 0));
boxs.Add(box);
Components.Add(box);
}
}
}
start_x -= 8;
for (k = 0; k < 2; k++)
{
for (i = 0; i < 2; i++)
{
for (j = 0; j < 2; j++)
{
Matrix startTransform = Matrix.CreateTranslation(
2 * i + start_x,
2 * k + start_y,
2 * j + start_z
);
// using motionstate is recommended, it provides interpolation capabilities
// and only synchronizes 'active' objects
BulletXNAObject box = new BulletXNAObject(this, "Models/Box", mass, new Bullet.DefaultMotionState(startTransform, Matrix.Identity), colShape, localInertia, colGroup, colMask);
box.TextureMaterials.Add("Textures/BoxColor01");
box.NormalMaterials.Add("Textures/BoxNormal01");
box.TranslateAA(new Vector3(0, 7, 0));
boxs.Add(box);
Components.Add(box);
}
}
}
IsPhysicsEnabled = false;
#endif
}
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()
{
//string filename = @"E:\users\man\bullet\xna-constraint-output.txt";
//FileStream filestream = File.Open(filename, FileMode.Create, FileAccess.Write, FileShare.Read);
//BulletGlobals.g_streamWriter = new StreamWriter(filestream);
//maxiterations = 100;
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 with a breaking threshold
{
trans = IndexedMatrix.Identity;
trans._origin = new IndexedVector3(1,30,-5);
LocalCreateRigidBody( mass,trans,shape);
trans._origin = new IndexedVector3(0,0,-5);
RigidBody body0 = LocalCreateRigidBody( mass,trans,shape);
trans._origin = new IndexedVector3(2*CUBE_HALF_EXTENTS,20,0);
mass = 1.0f;
RigidBody body1 = null;//localCreateRigidBody( mass,trans,shape);
IndexedVector3 pivotInA = new IndexedVector3(CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS,0);
TypedConstraint p2p = new Point2PointConstraint(body0,ref pivotInA);
m_dynamicsWorld.AddConstraint(p2p);
p2p.SetBreakingImpulseThreshold(10.2f);
p2p.SetDbgDrawSize(5.0f);
}
#endif
#if true
//point to point constraint (ball socket)
//SEEMS OK
{
//trans = IndexedMatrix.Identity;
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;
#if P2P
TypedConstraint p2p = new Point2PointConstraint(body0,ref pivotInA);
//btTypedConstraint* p2p = new btPoint2PointConstraint(*body0,*body1,pivotInA,pivotInB);
//btTypedConstraint* hinge = new btHingeConstraint(*body0,*body1,pivotInA,pivotInB,axisInA,axisInB);
m_dynamicsWorld.AddConstraint(p2p);
p2p.SetDbgDrawSize(5.0f);
#else
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
}
#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(new IndexedQuaternion(sliderAxis,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.SetAngularLowerLimit(new IndexedVector3(-MathUtil.SIMD_PI, 0, 0));
spSlider6Dof.SetAngularUpperLimit(new IndexedVector3(1.5f, 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 * 0.6f, MathUtil.SIMD_QUARTER_PI, MathUtil.SIMD_PI * 0.8f, 0.5f); // 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
#if true
{ // 6DOF connected to the world, with motor
IndexedMatrix tr = IndexedMatrix.CreateTranslation(10,-15,0);
RigidBody pBody = LocalCreateRigidBody( 1.0f, ref tr, shape);
pBody.SetActivationState(ActivationState.DISABLE_DEACTIVATION);
IndexedMatrix frameB = IndexedMatrix.Identity;
Generic6DofConstraint pGen6Dof = new Generic6DofConstraint(pBody, ref frameB, false );
m_dynamicsWorld.AddConstraint(pGen6Dof);
pGen6Dof.SetDbgDrawSize(5.0f);
pGen6Dof.SetAngularLowerLimit(new IndexedVector3(0,0,0));
pGen6Dof.SetAngularUpperLimit(new IndexedVector3(0,0,0));
pGen6Dof.SetLinearLowerLimit(new IndexedVector3(-10.0f, 0, 0));
pGen6Dof.SetLinearUpperLimit(new IndexedVector3(10.0f, 0, 0));
pGen6Dof.GetTranslationalLimitMotor().m_enableMotor[0] = true;
pGen6Dof.GetTranslationalLimitMotor().m_targetVelocity[0] = 5.0f;
pGen6Dof.GetTranslationalLimitMotor().m_maxMotorForce[0] = 0.1f;
}
#endif
}
protected override void Initialize()
{
base.Initialize();
SetCameraDistance(50.0f);
///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);
m_broadphase = new DbvtBroadphase();
///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);
IndexedVector3 gravity = new IndexedVector3(0, -10, 0);
m_dynamicsWorld.SetGravity(ref gravity);
// NEW => btGhostPairCallback =================================
m_ghostPairCallback = new GhostPairCallback();
m_dynamicsWorld.GetBroadphase().GetOverlappingPairCache().SetInternalGhostPairCallback(m_ghostPairCallback); // Needed once to enable ghost objects inside Bullet
// NEW => btGhostObject =======================================
m_ghostObject = new GhostObject();
CollisionShape shape = new BoxShape(new IndexedVector3(5f)); // As far as I know only the world aabb of the shape will be used (i.e. a box always parallel to the x,y,z axis of variable size)
m_collisionShapes.Add(shape);
m_ghostObject.SetCollisionShape(shape);
m_ghostObject.SetCollisionFlags(CollisionFlags.CF_NO_CONTACT_RESPONSE); // We can choose to make it "solid" if we want...
m_dynamicsWorld.AddCollisionObject(m_ghostObject, CollisionFilterGroups.SensorTrigger, CollisionFilterGroups.AllFilter & ~CollisionFilterGroups.SensorTrigger);
//m_ghostObject.setWorldTransform(btTransform(btQuaternion::getIdentity(),btVector3(0,5,-15)));
IndexedMatrix im = IndexedMatrix.CreateFromQuaternion(quatDeg45Y);
im._origin = new IndexedVector3(0, 5, -15);
m_ghostObject.SetWorldTransform(im);
// NEW => btPairCachingGhostObject ============================
m_pairCachingGhostObject = new PairCachingGhostObject();
shape = new ConeShape(7.0f, 14.0f);
m_collisionShapes.Add(shape);
m_pairCachingGhostObject.SetCollisionShape(shape);
m_pairCachingGhostObject.SetCollisionFlags(CollisionFlags.CF_NO_CONTACT_RESPONSE); // We can choose to make it "solid" if we want...
m_dynamicsWorld.AddCollisionObject(m_pairCachingGhostObject, CollisionFilterGroups.SensorTrigger, CollisionFilterGroups.AllFilter & ~CollisionFilterGroups.SensorTrigger);
//m_pairCachingGhostObject.setWorldTransform(btTransform(btQuaternion::getIdentity(),btVector3(0,5,15)));
im._origin = new IndexedVector3(0, 7, 15);
m_pairCachingGhostObject.SetWorldTransform(im);
//=============================================================
///create a few basic rigid bodies
CollisionShape groundShape = new BoxShape(new IndexedVector3(50));
m_collisionShapes.Add(groundShape);
IndexedMatrix groundTransform = IndexedMatrix.Identity;
groundTransform._origin = new IndexedVector3(0, -50, 0);
float mass = 0.0f;
LocalCreateRigidBody(mass, groundTransform, groundShape);
// spawn some cubes (code pasted from appBasicDemo...)
if(true)
{
//create a few dynamic rigidbodies
CollisionShape colShape = new BoxShape(new IndexedVector3(SCALING, SCALING, SCALING));
//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()
{
m_cameraDistance = 10.0f;
//string filename = @"e:\users\man\bullet\gimpact-demo-xna.txt";
//FileStream filestream = File.Open(filename, FileMode.Create, FileAccess.Write, FileShare.Read);
//BulletGlobals.g_streamWriter = new StreamWriter(filestream);
/// Init Bullet
m_collisionConfiguration = new DefaultCollisionConfiguration();
m_dispatcher = new CollisionDispatcher(m_collisionConfiguration);
//btOverlappingPairCache* broadphase = new btSimpleBroadphase();
//m_broadphase = new btSimpleBroadphase();
int maxProxies = 1024;
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, false);
//m_broadphase = new SimpleBroadphase(16384,null);
m_constraintSolver = new SequentialImpulseConstraintSolver();
m_dynamicsWorld = new DiscreteDynamicsWorld(m_dispatcher, m_broadphase, m_constraintSolver, m_collisionConfiguration);
//create trimesh model and shape
InitGImpactCollision();
/// Create Scene
float mass = 0.0f;
IndexedMatrix startTransform = IndexedMatrix.Identity;
CollisionShape staticboxShape1 = new BoxShape(new IndexedVector3(200, 1, 200));//floor
staticboxShape1.SetUserPointer("Floor");
CollisionShape staticboxShape2 = new BoxShape(new IndexedVector3(1, 50, 200));//left wall
staticboxShape1.SetUserPointer("LeftWall");
CollisionShape staticboxShape3 = new BoxShape(new IndexedVector3(1, 50, 200));//right wall
staticboxShape1.SetUserPointer("RightWall");
CollisionShape staticboxShape4 = new BoxShape(new IndexedVector3(200, 50, 1));//front wall
staticboxShape1.SetUserPointer("FrontWall");
CollisionShape staticboxShape5 = new BoxShape(new IndexedVector3(200, 50, 1));//back wall
staticboxShape1.SetUserPointer("BackWall");
CompoundShape staticScenario = new CompoundShape();//static scenario
startTransform._origin = new IndexedVector3(0, 0, 0);
staticScenario.AddChildShape(ref startTransform, staticboxShape1);
startTransform._origin = new IndexedVector3(-200, 25, 0);
staticScenario.AddChildShape(ref startTransform, staticboxShape2);
startTransform._origin = new IndexedVector3(200, 25, 0);
staticScenario.AddChildShape(ref startTransform, staticboxShape3);
startTransform._origin = new IndexedVector3(0, 25, 200);
staticScenario.AddChildShape(ref startTransform, staticboxShape4);
startTransform._origin = new IndexedVector3(0, 25, -200);
staticScenario.AddChildShape(ref startTransform, staticboxShape5);
startTransform._origin = new IndexedVector3(0, 0, 0);
//RigidBody staticBody = LocalCreateRigidBody(mass, startTransform, staticScenario);
RigidBody staticBody = LocalCreateRigidBody(mass, startTransform, staticboxShape1);
staticBody.SetCollisionFlags(staticBody.GetCollisionFlags()|CollisionFlags.CF_STATIC_OBJECT);
//enable custom material callback
staticBody.SetCollisionFlags(staticBody.GetCollisionFlags()|CollisionFlags.CF_CUSTOM_MATERIAL_CALLBACK);
//static plane
IndexedVector3 normal = new IndexedVector3(0.4f,1.5f,-0.4f);
normal.Normalize();
CollisionShape staticplaneShape6 = new StaticPlaneShape(ref normal,0.0f);// A plane
startTransform._origin = IndexedVector3.Zero;
RigidBody staticBody2 = LocalCreateRigidBody(mass, startTransform, staticplaneShape6);
staticBody2.SetCollisionFlags(staticBody2.GetCollisionFlags()|CollisionFlags.CF_STATIC_OBJECT);
startTransform = IndexedMatrix.Identity;
/// Create Dynamic Boxes
{
int numBoxes = 1;
for (int i = 0; i < numBoxes; i++)
{
CollisionShape boxShape = new BoxShape(new IndexedVector3(1, 1, 1));
//CollisionShape mesh = new BvhTriangleMeshShape(m_indexVertexArrays2,true,true);
startTransform._origin = new IndexedVector3(2 * i - (numBoxes-1), 2, -3);
//startTransform._origin = new IndexedVector3(2 * i - 5, 10, -3);
//LocalCreateRigidBody(1, startTransform, m_trimeshShape2);
LocalCreateRigidBody(1, startTransform, boxShape);
}
}
}
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);
//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(BulletXNA.LinearMath.DebugDrawModes.DBG_DrawAabb | BulletXNA.LinearMath.DebugDrawModes.DBG_DrawNormals | BulletXNA.LinearMath.DebugDrawModes.DBG_DrawContactPoints);
m_dynamicsWorld.SetDebugDrawer(BulletGlobals.gDebugDraw);
//ClientResetScene();
}
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 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();
}
//(PhysicsScene.World.ptr, nativeShapeData)
public override BulletShape BuildNativeShape(BulletWorld pWorld, ShapeData pShapeData)
{
DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
CollisionShape shape = null;
switch (pShapeData.Type)
{
case BSPhysicsShapeType.SHAPE_BOX:
shape = new BoxShape(new IndexedVector3(0.5f,0.5f,0.5f));
break;
case BSPhysicsShapeType.SHAPE_CONE:
shape = new ConeShapeZ(0.5f, 1.0f);
break;
case BSPhysicsShapeType.SHAPE_CYLINDER:
shape = new CylinderShapeZ(new IndexedVector3(0.5f, 0.5f, 0.5f));
break;
case BSPhysicsShapeType.SHAPE_SPHERE:
shape = new SphereShape(0.5f);
break;
}
if (shape != null)
{
IndexedVector3 scaling = new IndexedVector3(pShapeData.Scale.X, pShapeData.Scale.Y, pShapeData.Scale.Z);
shape.SetMargin(world.WorldSettings.Params.collisionMargin);
shape.SetLocalScaling(ref scaling);
}
return new BulletShapeXNA(shape, pShapeData.Type);
}
//(PhysicsScene.World.ptr, nativeShapeData)
internal static object BuildNativeShape2(object pWorld, ShapeData pShapeData)
{
var world = pWorld as DiscreteDynamicsWorld;
CollisionShape shape = null;
switch (pShapeData.Type)
{
case BSPhysicsShapeType.SHAPE_BOX:
shape = new BoxShape(new IndexedVector3(0.5f,0.5f,0.5f));
break;
case BSPhysicsShapeType.SHAPE_CONE:
shape = new ConeShapeZ(0.5f, 1.0f);
break;
case BSPhysicsShapeType.SHAPE_CYLINDER:
shape = new CylinderShapeZ(new IndexedVector3(0.5f, 0.5f, 0.5f));
break;
case BSPhysicsShapeType.SHAPE_SPHERE:
shape = new SphereShape(0.5f);
break;
}
if (shape != null)
{
IndexedVector3 scaling = new IndexedVector3(pShapeData.Scale.X, pShapeData.Scale.Y, pShapeData.Scale.Z);
shape.SetMargin(world.WorldSettings.Params.collisionMargin);
shape.SetLocalScaling(ref scaling);
}
return shape;
}
public void BuildCollisionData(Color[] map, int width, int height, Vector3 bottomLeft)
{
Vector2 dim = Vector2.One;
Vector3 scale = new Vector3(dim, 1);
BoxShape collisionBoxShape = new BoxShape(new IndexedVector3(0.5f));
for (int x = 0; x < width; x++)
{
for (int y = 0; y < height; y++)
{
int currentIndex = x + y * width;
if (map[currentIndex] == Color.White)
{
float yOffset = -dim.Y;//0f;// -(dim.Y / 2f);
Vector3 position = new Vector3(x - bottomLeft.X, (bottomLeft.Y - y) + yOffset, bottomLeft.Z);
m_waterLocations.Add(position);
}
if (map[currentIndex] == Color.White)
{
// check the 4 ordinals , if we're surrounded by other solid blocks
// then we don't need a block here.
bool upSet = false;
bool downSet = false;
bool leftSet = false;
bool rightSet = false;
if (x >= 1 && x < width - 1)
{
if (map[currentIndex - 1] == Color.White)
{
leftSet = true;
}
if (map[currentIndex + 1] == Color.White)
{
rightSet = true;
}
}
if (y >= 1 && y < height - 1)
{
if (map[currentIndex - height] == Color.White)
{
upSet = true;
}
if (map[currentIndex + height] == Color.White)
{
downSet = true;
}
}
// if we're not surrounded by blocks then add in.
if (!(upSet && downSet && leftSet && rightSet))
{
Object rigifdBody;
float yOffset = -dim.Y;//0f;// -(dim.Y / 2f);
Vector3 position = new Vector3(x - bottomLeft.X, (bottomLeft.Y - y) + yOffset, bottomLeft.Z);
RigidBodyConstructionInfo constructionInfo = new BulletXNA.BulletDynamics.RigidBodyConstructionInfo(0f, null, (BulletXNA.BulletCollision.CollisionShape)collisionBoxShape);
RigidBody rigidBody = new BulletXNA.BulletDynamics.RigidBody(constructionInfo);
Matrix bsm = Matrix.CreateTranslation(position);
rigidBody.SetWorldTransform(bsm);
// FIXME MAN - setup some collision flags on these bodies...
BulletXNA.BulletCollision.CollisionFilterGroups flags = (BulletXNA.BulletCollision.CollisionFilterGroups)(1 << 8);
BulletXNA.BulletCollision.CollisionFilterGroups mask = (BulletXNA.BulletCollision.CollisionFilterGroups)(1 << 9);
//rigidBody.CollisionFlags |= (BulletSharp.CollisionFlags)CollisionObjectType.Ground;
m_dynamicsWorld.AddRigidBody(rigidBody, flags, mask);
}
}
// Build water ghost objects.
foreach (Vector3 pos in m_waterLocations)
{
GhostObject ghostObject = new GhostObject();
ghostObject.SetCollisionShape((BulletXNA.BulletCollision.CollisionShape)collisionBoxShape);
CollisionFilterGroups flags = (CollisionFilterGroups)(1 << 10);
CollisionFilterGroups mask = (CollisionFilterGroups)(1<<9);
ghostObject.SetCollisionFlags(CollisionFlags.CF_NO_CONTACT_RESPONSE | CollisionFlags.CF_STATIC_OBJECT); // We can choose to make it "solid" if we want...
ghostObject.SetWorldTransform(BulletXNA.LinearMath.IndexedMatrix.CreateTranslation(pos));
m_dynamicsWorld.AddCollisionObject(ghostObject, flags, mask);
break;
}
}
}
}
//----------------------------------------------------------------------------------------------
///Demo functions
public virtual void SetShootBoxShape()
{
if (m_shootBoxShape == null)
{
//#define TEST_UNIFORM_SCALING_SHAPE 1
#if TEST_UNIFORM_SCALING_SHAPE
ConvexShape childShape = new BoxShape(new IndexedVector3(1f,1f,1f));
m_shootBoxShape = new UniformScalingShape(childShape,0.5f);
#else
//m_shootBoxShape = new SphereShape(1f);//BoxShape(btVector3(1.f,1.f,1.f));
m_shootBoxShape = new BoxShape(new IndexedVector3(0.5f, 0.5f, 0.5f));
#endif//
}
}
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 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 ProcessCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
if (m_manifoldPtr == null)
{
return;
}
CollisionObject convexObj = m_isSwapped ? body1 : body0;
CollisionObject planeObj = m_isSwapped ? body0 : body1;
ConvexShape convexShape = convexObj.GetCollisionShape() as ConvexShape;
if (convexShape == null)
{
convexShape = new BoxShape(IndexedVector3.One * 0.5f);
body0.SetCollisionShape(convexShape);
}
StaticPlaneShape planeShape = planeObj.GetCollisionShape() as StaticPlaneShape;
if (planeShape == null)
{
planeShape = new StaticPlaneShape(IndexedVector3.Up, 9f);
}
bool hasCollision = false;
if (convexShape == null || planeShape == null)
{
//resultOut = null;
return;
}
IndexedVector3 planeNormal = planeShape.GetPlaneNormal();
float planeConstant = planeShape.GetPlaneConstant();
IndexedMatrix planeInConvex;
planeInConvex = convexObj.GetWorldTransform().Inverse() * planeObj.GetWorldTransform();
IndexedMatrix convexInPlaneTrans;
convexInPlaneTrans = planeObj.GetWorldTransform().Inverse() * convexObj.GetWorldTransform();
IndexedVector3 vtx = convexShape.LocalGetSupportingVertex(planeInConvex._basis * -planeNormal);
IndexedVector3 vtxInPlane = convexInPlaneTrans * vtx;
float distance = (planeNormal.Dot(vtxInPlane) - planeConstant);
IndexedVector3 vtxInPlaneProjected = vtxInPlane - distance * planeNormal;
IndexedVector3 vtxInPlaneWorld = planeObj.GetWorldTransform() * vtxInPlaneProjected;
hasCollision = distance < m_manifoldPtr.GetContactBreakingThreshold();
resultOut.SetPersistentManifold(m_manifoldPtr);
if (hasCollision)
{
/// report a contact. internally this will be kept persistent, and contact reduction is done
IndexedVector3 normalOnSurfaceB = planeObj.GetWorldTransform()._basis *planeNormal;
IndexedVector3 pOnB = vtxInPlaneWorld;
resultOut.AddContactPoint(normalOnSurfaceB, pOnB, distance);
}
////first perform a collision query with the non-perturbated collision objects
//{
// IndexedQuaternion rotq = IndexedQuaternion.Identity;
// CollideSingleContact(ref rotq, body0, body1, dispatchInfo, resultOut);
//}
if (convexShape.IsPolyhedral() && resultOut.GetPersistentManifold().GetNumContacts() < m_minimumPointsPerturbationThreshold)
{
IndexedVector3 v0;
IndexedVector3 v1;
TransformUtil.PlaneSpace1(ref planeNormal, out v0, out v1);
//now perform 'm_numPerturbationIterations' collision queries with the perturbated collision objects
float angleLimit = 0.125f * MathUtil.SIMD_PI;
float perturbeAngle;
float radius = convexShape.GetAngularMotionDisc();
perturbeAngle = BulletGlobals.gContactBreakingThreshold / radius;
if (perturbeAngle > angleLimit)
{
perturbeAngle = angleLimit;
}
IndexedQuaternion perturbeRot = new IndexedQuaternion(v0, perturbeAngle);
for (int i = 0; i < m_numPerturbationIterations; i++)
{
float iterationAngle = i * (MathUtil.SIMD_2_PI / (float)m_numPerturbationIterations);
IndexedQuaternion rotq = new IndexedQuaternion(planeNormal, iterationAngle);
rotq = IndexedQuaternion.Inverse(rotq) * perturbeRot * rotq;
CollideSingleContact(ref rotq, body0, body1, dispatchInfo, resultOut);
}
}
if (m_ownManifold)
{
if (m_manifoldPtr.GetNumContacts() > 0)
{
resultOut.RefreshContactPoints();
}
}
}
