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 IndexedVector3 LocalGetSupportVertexWithoutMarginNonVirtual(ref IndexedVector3 localDir)
{
singleResult = IndexedVector3.Zero;
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugConvexShape)
{
BulletGlobals.g_streamWriter.WriteLine("localGetSupportVertexWithoutMarginNonVirtual " + GetName());
}
switch (m_shapeType)
{
case BroadphaseNativeTypes.SPHERE_SHAPE_PROXYTYPE:
{
singleResult = new IndexedVector3();
break;
}
case BroadphaseNativeTypes.BOX_SHAPE_PROXYTYPE:
{
BoxShape convexShape = this as BoxShape;
IndexedVector3 halfExtents = convexShape.GetImplicitShapeDimensions();
singleResult = new IndexedVector3(MathUtil.FSel(localDir.X, halfExtents.X, -halfExtents.X),
MathUtil.FSel(localDir.Y, halfExtents.Y, -halfExtents.Y),
MathUtil.FSel(localDir.Z, halfExtents.Z, -halfExtents.Z));
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugConvexShape)
{
BulletGlobals.g_streamWriter.WriteLine("localGetSupportVertexWithoutMarginNonVirtual::Box");
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "halfEx", halfExtents);
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "localDir", localDir);
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "result", singleResult);
}
break;
}
case BroadphaseNativeTypes.TRIANGLE_SHAPE_PROXYTYPE:
{
TriangleShape triangleShape = (TriangleShape)this;
IndexedVector3 dir = localDir;
IndexedVector3[] vertices = triangleShape.m_vertices1;
IndexedVector3 dots = new IndexedVector3(IndexedVector3.Dot(ref dir, ref vertices[0]), IndexedVector3.Dot(ref dir, ref vertices[1]), IndexedVector3.Dot(ref dir, ref vertices[2]));
int maxAxis = MathUtil.MaxAxis(ref dots);
IndexedVector3 sup = vertices[maxAxis];
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugConvexShape)
{
BulletGlobals.g_streamWriter.WriteLine("localGetSupportVertexWithoutMarginNonVirtual::Triangle");
BulletGlobals.g_streamWriter.WriteLine(String.Format("MaxAxis [{0}]", maxAxis));
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "vtx0", vertices[0]);
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "vtx1", vertices[1]);
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "vtx2", vertices[2]);
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "dir", dir);
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "dots", dots);
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "sup", sup);
}
singleResult = sup;
break;
}
case BroadphaseNativeTypes.CYLINDER_SHAPE_PROXYTYPE:
{
CylinderShape cylShape = (CylinderShape)this;
//mapping of halfextents/dimension onto radius/height depends on how cylinder local orientation is (upAxis)
IndexedVector3 halfExtents = cylShape.GetImplicitShapeDimensions();
IndexedVector3 v = localDir;
int cylinderUpAxis = cylShape.GetUpAxis();
int XX = 1;
int YY = 0;
int ZZ = 2;
switch (cylinderUpAxis)
{
case 0:
{
XX = 1;
YY = 0;
ZZ = 2;
}
break;
case 1:
{
XX = 0;
YY = 1;
ZZ = 2;
}
break;
case 2:
{
XX = 0;
YY = 2;
ZZ = 1;
}
break;
default:
Debug.Assert(false);
break;
}
;
float radius = halfExtents[XX];
float halfHeight = halfExtents[cylinderUpAxis];
IndexedVector3 tmp = new IndexedVector3();
float d;
float vx = v[XX];
float vz = v[ZZ];
float s = (float)Math.Sqrt(vx * vx + vz * vz);
if (s != 0f)
{
d = radius / s;
tmp[XX] = v[XX] * d;
tmp[YY] = v[YY] < 0.0f ? -halfHeight : halfHeight;
tmp[ZZ] = v[ZZ] * d;
singleResult = tmp;
}
else
{
tmp[XX] = radius;
tmp[YY] = v[YY] < 0.0f ? -halfHeight : halfHeight;
tmp[ZZ] = 0.0f;
singleResult = tmp;
}
break;
}
case BroadphaseNativeTypes.CAPSULE_SHAPE_PROXYTYPE:
{
IndexedVector3 vec0 = localDir;
CapsuleShape capsuleShape = this as CapsuleShape;
float halfHeight = capsuleShape.GetHalfHeight();
int capsuleUpAxis = capsuleShape.GetUpAxis();
float radius = capsuleShape.GetRadius();
IndexedVector3 supVec = new IndexedVector3();
float maxDot = float.MinValue;
IndexedVector3 vec = vec0;
float lenSqr = vec.LengthSquared();
if (lenSqr < 0.0001f)
{
vec = new IndexedVector3(1, 0, 0);
}
else
{
float rlen = (1.0f) / (float)Math.Sqrt(lenSqr);
vec *= rlen;
//vec = IndexedVector3.Normalize(vec);
}
IndexedVector3 vtx;
float newDot;
{
IndexedVector3 pos = new IndexedVector3();
pos[capsuleUpAxis] = halfHeight;
//vtx = pos +vec*(radius);
vtx = pos + vec * (radius) - vec * capsuleShape.GetMarginNV();
newDot = IndexedVector3.Dot(ref vec, ref vtx);
if (newDot > maxDot)
{
maxDot = newDot;
supVec = vtx;
}
}
{
IndexedVector3 pos = new IndexedVector3();
pos[capsuleUpAxis] = -halfHeight;
//vtx = pos +vec*(radius);
vtx = pos + vec * (radius) - vec * capsuleShape.GetMarginNV();
newDot = IndexedVector3.Dot(ref vec, ref vtx);
if (newDot > maxDot)
{
maxDot = newDot;
supVec = vtx;
}
}
singleResult = supVec;
break;
}
case BroadphaseNativeTypes.CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE:
{
ConvexPointCloudShape convexPointCloudShape = (ConvexPointCloudShape)this;
IList <IndexedVector3> points = convexPointCloudShape.GetUnscaledPoints();
int numPoints = convexPointCloudShape.GetNumPoints();
IndexedVector3 localScaling = convexPointCloudShape.GetLocalScalingNV();
singleResult = ConvexHullSupport(ref localDir, points, numPoints, ref localScaling);
break;
}
case BroadphaseNativeTypes.CONVEX_HULL_SHAPE_PROXYTYPE:
{
ConvexHullShape convexHullShape = (ConvexHullShape)this;
IList <IndexedVector3> points = convexHullShape.GetUnscaledPoints();
int numPoints = convexHullShape.GetNumPoints();
IndexedVector3 localScaling = convexHullShape.GetLocalScalingNV();
singleResult = ConvexHullSupport(ref localDir, points, numPoints, ref localScaling);
break;
}
default:
singleResult = LocalGetSupportingVertexWithoutMargin(ref localDir);
break;
}
// should never reach here
//Debug.Assert(false);
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugConvexShape)
{
BulletGlobals.g_streamWriter.WriteLine("localGetSupportVertexWithoutMarginNonVirtual");
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "localDir", localDir);
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "result", singleResult);
}
return(singleResult);
}
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);
}
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;
}
