public RayTester(CollisionObject collisionObject,
CompoundShape compoundShape,
ref IndexedMatrix colObjWorldTransform,
ref IndexedMatrix rayFromTrans,
ref IndexedMatrix rayToTrans,
RayResultCallback resultCallback)
{
m_collisionObject = collisionObject;
m_compoundShape = compoundShape;
m_colObjWorldTransform = colObjWorldTransform;
m_rayFromTrans = rayFromTrans;
m_rayToTrans = rayToTrans;
m_resultCallback = resultCallback;
}
public override BulletShape CreateHullShape(BulletWorld pWorld, int pHullCount, float[] pConvHulls)
{
DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
CompoundShape compoundshape = new CompoundShape(false);
compoundshape.SetMargin(world.WorldSettings.Params.collisionMargin);
int ii = 1;
for (int i = 0; i < pHullCount; i++)
{
int vertexCount = (int) pConvHulls[ii];
IndexedVector3 centroid = new IndexedVector3(pConvHulls[ii + 1], pConvHulls[ii + 2], pConvHulls[ii + 3]);
IndexedMatrix childTrans = IndexedMatrix.Identity;
childTrans._origin = centroid;
List<IndexedVector3> virts = new List<IndexedVector3>();
int ender = ((ii + 4) + (vertexCount*3));
for (int iii = ii + 4; iii < ender; iii+=3)
{
virts.Add(new IndexedVector3(pConvHulls[iii], pConvHulls[iii + 1], pConvHulls[iii +2]));
}
ConvexHullShape convexShape = new ConvexHullShape(virts, vertexCount);
convexShape.SetMargin(world.WorldSettings.Params.collisionMargin);
compoundshape.AddChildShape(ref childTrans, convexShape);
ii += (vertexCount*3 + 4);
}
return new BulletShapeXNA(compoundshape, BSPhysicsShapeType.SHAPE_HULL);
}
public void GImpactVsShape(CollisionObject body0,
CollisionObject body1,
GImpactShapeInterface shape0,
CollisionShape shape1, bool swapped)
{
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugGimpactAlgo)
{
BulletGlobals.g_streamWriter.WriteLine("GImpactAglo::GImpactVsShape");
}
if (shape0.GetGImpactShapeType() == GIMPACT_SHAPE_TYPE.CONST_GIMPACT_TRIMESH_SHAPE)
{
GImpactMeshShape meshshape0 = shape0 as GImpactMeshShape;
// check this...
//int& part = swapped ? m_part1 : m_part0;
//part = meshshape0.GetMeshPartCount();
int part = meshshape0.GetMeshPartCount();
while (part-- != 0)
{
GImpactVsShape(body0,
body1,
meshshape0.GetMeshPart(part),
shape1, swapped);
}
if (swapped)
{
m_part1 = part;
}
else
{
m_part0 = part;
}
return;
}
#if GIMPACT_VS_PLANE_COLLISION
if (shape0.GetGImpactShapeType() == GIMPACT_SHAPE_TYPE.CONST_GIMPACT_TRIMESH_SHAPE_PART &&
shape1.GetShapeType() == BroadphaseNativeTypes.STATIC_PLANE_PROXYTYPE)
{
GImpactMeshShapePart shapepart = shape0 as GImpactMeshShapePart;
StaticPlaneShape planeshape = shape1 as StaticPlaneShape;
GImpactTrimeshpartVsPlaneCollision(body0, body1, shapepart, planeshape, swapped);
return;
}
#endif
if (shape1.IsCompound())
{
CompoundShape compoundshape = shape1 as CompoundShape;
GImpactVsCompoundshape(body0, body1, shape0, compoundshape, swapped);
return;
}
else if (shape1.IsConcave())
{
ConcaveShape concaveshape = shape1 as ConcaveShape;
GImpactVsConcave(body0, body1, shape0, concaveshape, swapped);
return;
}
IndexedMatrix orgtrans0 = body0.GetWorldTransform();
IndexedMatrix orgtrans1 = body1.GetWorldTransform();
ObjectArray <int> collided_results = new ObjectArray <int>(64);
GImpactVsShapeFindPairs(ref orgtrans0, ref orgtrans1, shape0, shape1, collided_results);
if (collided_results.Count == 0)
{
return;
}
shape0.LockChildShapes();
using (GIM_ShapeRetriever retriever0 = BulletGlobals.GIM_ShapeRetrieverPool.Get())
{
retriever0.Initialize(shape0);
bool child_has_transform0 = shape0.ChildrenHasTransform();
int i = collided_results.Count;
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugGimpactAlgo)
{
BulletGlobals.g_streamWriter.WriteLine("GImpactAglo::GImpactVsShape [{0}]", collided_results.Count);
}
while (i-- != 0)
{
int child_index = collided_results[i];
if (swapped)
{
m_triface1 = child_index;
}
else
{
m_triface0 = child_index;
}
CollisionShape colshape0 = retriever0.GetChildShape(child_index);
if (child_has_transform0)
{
body0.SetWorldTransform(orgtrans0 * shape0.GetChildTransform(child_index));
}
//collide two shapes
if (swapped)
{
ShapeVsShapeCollision(body1, body0, shape1, colshape0);
}
else
{
ShapeVsShapeCollision(body0, body1, colshape0, shape1);
}
//restore transforms
if (child_has_transform0)
{
body0.SetWorldTransform(ref orgtrans0);
}
}
shape0.UnlockChildShapes();
}
}
public void GImpactVsCompoundshape(CollisionObject body0,
CollisionObject body1,
GImpactShapeInterface shape0,
CompoundShape shape1, bool swapped)
{
IndexedMatrix orgtrans1 = body1.GetWorldTransform();
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugGimpactAlgo)
{
BulletGlobals.g_streamWriter.WriteLine("GImpactAglo::GImpactVsCompoundshape");
}
int i = shape1.GetNumChildShapes();
while (i-- != 0)
{
CollisionShape colshape1 = shape1.GetChildShape(i);
IndexedMatrix childtrans1 = orgtrans1 * shape1.GetChildTransform(i);
body1.SetWorldTransform(ref childtrans1);
//collide child shape
GImpactVsShape(body0, body1,
shape0, colshape1, swapped);
//restore transforms
body1.SetWorldTransform(ref orgtrans1);
}
}
internal static object CreateHullShape2(object pWorld, int pHullCount, float[] pConvHulls)
{
CompoundShape compoundshape = new CompoundShape(false);
var world = pWorld as DiscreteDynamicsWorld;
compoundshape.SetMargin(world.WorldSettings.Params.collisionMargin);
int ii = 1;
for (int i = 0; i < pHullCount; i++)
{
int vertexCount = (int) pConvHulls[ii];
IndexedVector3 centroid = new IndexedVector3(pConvHulls[ii + 1], pConvHulls[ii + 2], pConvHulls[ii + 3]);
IndexedMatrix childTrans = IndexedMatrix.Identity;
childTrans._origin = centroid;
List<IndexedVector3> virts = new List<IndexedVector3>();
int ender = ((ii + 4) + (vertexCount*3));
for (int iii = ii + 4; iii < ender; iii+=3)
{
virts.Add(new IndexedVector3(pConvHulls[iii], pConvHulls[iii + 1], pConvHulls[iii +2]));
}
ConvexHullShape convexShape = new ConvexHullShape(virts, vertexCount);
convexShape.SetMargin(world.WorldSettings.Params.collisionMargin);
compoundshape.AddChildShape(ref childTrans, convexShape);
ii += (vertexCount*3 + 4);
}
return compoundshape;
}
public void ProcessChildShape(CollisionShape childShape, int index)
{
Debug.Assert(index >= 0);
CompoundShape compoundShape = (CompoundShape)(m_compoundColObj.GetCollisionShape());
Debug.Assert(index < compoundShape.GetNumChildShapes());
//backup
IndexedMatrix orgTrans = m_compoundColObj.GetWorldTransform();
IndexedMatrix orgInterpolationTrans = m_compoundColObj.GetInterpolationWorldTransform();
IndexedMatrix childTrans = compoundShape.GetChildTransform(index);
IndexedMatrix newChildWorldTrans = orgTrans * childTrans;
//perform an AABB check first
IndexedVector3 aabbMin0;
IndexedVector3 aabbMax0;
IndexedVector3 aabbMin1;
IndexedVector3 aabbMax1;
childShape.GetAabb(ref newChildWorldTrans, out aabbMin0, out aabbMax0);
m_otherObj.GetCollisionShape().GetAabb(m_otherObj.GetWorldTransform(), out aabbMin1, out aabbMax1);
if (AabbUtil2.TestAabbAgainstAabb2(ref aabbMin0, ref aabbMax0, ref aabbMin1, ref aabbMax1))
{
m_compoundColObj.SetWorldTransform(ref newChildWorldTrans);
m_compoundColObj.SetInterpolationWorldTransform(ref newChildWorldTrans);
//the contactpoint is still projected back using the original inverted worldtrans
CollisionShape tmpShape = m_compoundColObj.GetCollisionShape();
m_compoundColObj.InternalSetTemporaryCollisionShape(childShape);
if (m_childCollisionAlgorithms[index] == null)
{
m_childCollisionAlgorithms[index] = m_dispatcher.FindAlgorithm(m_compoundColObj, m_otherObj, m_sharedManifold);
if (m_childCollisionAlgorithms[index] == m_parent)
{
int ibreak = 0;
}
}
///detect swapping case
if (m_resultOut.GetBody0Internal() == m_compoundColObj)
{
m_resultOut.SetShapeIdentifiersA(-1, index);
}
else
{
m_resultOut.SetShapeIdentifiersB(-1, index);
}
m_childCollisionAlgorithms[index].ProcessCollision(m_compoundColObj, m_otherObj, m_dispatchInfo, m_resultOut);
if (m_dispatchInfo.getDebugDraw() != null && (((m_dispatchInfo.getDebugDraw().GetDebugMode() & DebugDrawModes.DBG_DrawAabb)) != 0))
{
IndexedVector3 worldAabbMin = IndexedVector3.Zero, worldAabbMax = IndexedVector3.Zero;
m_dispatchInfo.getDebugDraw().DrawAabb(aabbMin0, aabbMax0, new IndexedVector3(1, 1, 1));
m_dispatchInfo.getDebugDraw().DrawAabb(aabbMin1, aabbMax1, new IndexedVector3(1, 1, 1));
}
//revert back transform
m_compoundColObj.InternalSetTemporaryCollisionShape(tmpShape);
m_compoundColObj.SetWorldTransform(ref orgTrans);
m_compoundColObj.SetInterpolationWorldTransform(ref orgInterpolationTrans);
}
}
public override void ProcessCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
//resultOut = null;
CollisionObject colObj = m_isSwapped ? body1 : body0;
CollisionObject otherObj = m_isSwapped ? body0 : body1;
Debug.Assert(colObj.GetCollisionShape().IsCompound());
CompoundShape compoundShape = (CompoundShape)(colObj.GetCollisionShape());
///btCompoundShape might have changed:
////make sure the internal child collision algorithm caches are still valid
if (compoundShape.GetUpdateRevision() != m_compoundShapeRevision)
{
///clear and update all
RemoveChildAlgorithms();
PreallocateChildAlgorithms(body0, body1);
}
Dbvt tree = compoundShape.GetDynamicAabbTree();
//use a dynamic aabb tree to cull potential child-overlaps
using (CompoundLeafCallback callback = BulletGlobals.CompoundLeafCallbackPool.Get())
{
callback.Initialize(colObj, otherObj, m_dispatcher, dispatchInfo, resultOut, this, m_childCollisionAlgorithms, m_sharedManifold);
///we need to refresh all contact manifolds
///note that we should actually recursively traverse all children, btCompoundShape can nested more then 1 level deep
///so we should add a 'refreshManifolds' in the btCollisionAlgorithm
{
m_manifoldArray.Clear();
for (int i = 0; i < m_childCollisionAlgorithms.Count; i++)
{
if (m_childCollisionAlgorithms[i] != null)
{
m_childCollisionAlgorithms[i].GetAllContactManifolds(m_manifoldArray);
for (int m = 0; m < m_manifoldArray.Count; m++)
{
if (m_manifoldArray[m].GetNumContacts() > 0)
{
resultOut.SetPersistentManifold(m_manifoldArray[m]);
resultOut.RefreshContactPoints();
resultOut.SetPersistentManifold(null);//??necessary?
}
}
m_manifoldArray.Clear();
}
}
}
if (tree != null)
{
IndexedVector3 localAabbMin;
IndexedVector3 localAabbMax;
IndexedMatrix otherInCompoundSpace;
//otherInCompoundSpace = MathUtil.BulletMatrixMultiply(colObj.GetWorldTransform(),otherObj.GetWorldTransform());
otherInCompoundSpace = colObj.GetWorldTransform().Inverse() * otherObj.GetWorldTransform();
otherObj.GetCollisionShape().GetAabb(ref otherInCompoundSpace, out localAabbMin, out localAabbMax);
DbvtAabbMm bounds = DbvtAabbMm.FromMM(ref localAabbMin, ref localAabbMax);
//process all children, that overlap with the given AABB bounds
Dbvt.CollideTV(tree.m_root, ref bounds, callback, tree.CollideTVStack, ref tree.CollideTVCount);
}
else
{
//iterate over all children, perform an AABB check inside ProcessChildShape
int numChildren = m_childCollisionAlgorithms.Count;
for (int i = 0; i < numChildren; i++)
{
callback.ProcessChildShape(compoundShape.GetChildShape(i), i);
}
}
{
//iterate over all children, perform an AABB check inside ProcessChildShape
int numChildren = m_childCollisionAlgorithms.Count;
m_manifoldArray.Clear();
CollisionShape childShape = null;
IndexedMatrix orgTrans;
IndexedMatrix orgInterpolationTrans;
IndexedMatrix newChildWorldTrans;
for (int i = 0; i < numChildren; i++)
{
if (m_childCollisionAlgorithms[i] != null)
{
childShape = compoundShape.GetChildShape(i);
//if not longer overlapping, remove the algorithm
orgTrans = colObj.GetWorldTransform();
orgInterpolationTrans = colObj.GetInterpolationWorldTransform();
IndexedMatrix childTrans = compoundShape.GetChildTransform(i);
newChildWorldTrans = orgTrans * childTrans;
//perform an AABB check first
IndexedVector3 aabbMin0;
IndexedVector3 aabbMax0;
IndexedVector3 aabbMin1;
IndexedVector3 aabbMax1;
childShape.GetAabb(ref newChildWorldTrans, out aabbMin0, out aabbMax0);
otherObj.GetCollisionShape().GetAabb(otherObj.GetWorldTransform(), out aabbMin1, out aabbMax1);
if (!AabbUtil2.TestAabbAgainstAabb2(ref aabbMin0, ref aabbMax0, ref aabbMin1, ref aabbMax1))
{
m_dispatcher.FreeCollisionAlgorithm(m_childCollisionAlgorithms[i]);
m_childCollisionAlgorithms[i] = null;
}
}
}
}
}
}
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()
{
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);
}