public virtual void DebugDrawObject(ref IndexedMatrix worldTransform, CollisionShape shape, ref IndexedVector3 color)
{
// Draw a small simplex at the center of the object
GetDebugDrawer().DrawTransform(ref worldTransform, 1.0f);
switch (shape.GetShapeType())
{
case BroadphaseNativeTypes.COMPOUND_SHAPE_PROXYTYPE:
{
CompoundShape compoundShape = (CompoundShape)shape;
for (int i = compoundShape.GetNumChildShapes() - 1; i >= 0; i--)
{
IndexedMatrix childTrans = compoundShape.GetChildTransform(i);
CollisionShape colShape = compoundShape.GetChildShape(i);
IndexedMatrix temp = worldTransform * childTrans;
DebugDrawObject(ref temp, colShape, ref color);
}
break;
}
case (BroadphaseNativeTypes.BOX_SHAPE_PROXYTYPE):
{
BoxShape boxShape = shape as BoxShape;
IndexedVector3 halfExtents = boxShape.GetHalfExtentsWithMargin();
IndexedVector3 negHalfExtents = -halfExtents;
GetDebugDrawer().DrawBox(ref negHalfExtents, ref halfExtents, ref worldTransform, ref color);
break;
}
case BroadphaseNativeTypes.SPHERE_SHAPE_PROXYTYPE:
{
SphereShape sphereShape = shape as SphereShape;
float radius = sphereShape.GetMargin();//radius doesn't include the margin, so draw with margin
GetDebugDrawer().DrawSphere(radius, ref worldTransform, ref color);
break;
}
case BroadphaseNativeTypes.MULTI_SPHERE_SHAPE_PROXYTYPE:
{
MultiSphereShape multiSphereShape = (MultiSphereShape)shape;
for (int i = multiSphereShape.GetSphereCount() - 1; i >= 0; i--)
{
IndexedMatrix childTransform = worldTransform;
childTransform._origin += multiSphereShape.GetSpherePosition(i);
GetDebugDrawer().DrawSphere(multiSphereShape.GetSphereRadius(i), ref childTransform, ref color);
}
break;
}
case BroadphaseNativeTypes.CAPSULE_SHAPE_PROXYTYPE:
{
CapsuleShape capsuleShape = shape as CapsuleShape;
float radius = capsuleShape.GetRadius();
float halfHeight = capsuleShape.GetHalfHeight();
int upAxis = capsuleShape.GetUpAxis();
GetDebugDrawer().DrawCapsule(radius, halfHeight, upAxis, ref worldTransform, ref color);
break;
}
case BroadphaseNativeTypes.CONE_SHAPE_PROXYTYPE:
{
ConeShape coneShape = (ConeShape)shape;
float radius = coneShape.GetRadius();//+coneShape->getMargin();
float height = coneShape.GetHeight();//+coneShape->getMargin();
int upAxis = coneShape.GetConeUpIndex();
GetDebugDrawer().DrawCone(radius, height, upAxis, ref worldTransform, ref color);
break;
}
case BroadphaseNativeTypes.CYLINDER_SHAPE_PROXYTYPE:
{
CylinderShape cylinder = (CylinderShape)shape;
int upAxis = cylinder.GetUpAxis();
float radius = cylinder.GetRadius();
float halfHeight = cylinder.GetHalfExtentsWithMargin()[upAxis];
GetDebugDrawer().DrawCylinder(radius, halfHeight, upAxis, ref worldTransform, ref color);
break;
}
case BroadphaseNativeTypes.STATIC_PLANE_PROXYTYPE:
{
StaticPlaneShape staticPlaneShape = shape as StaticPlaneShape;
float planeConst = staticPlaneShape.GetPlaneConstant();
IndexedVector3 planeNormal = staticPlaneShape.GetPlaneNormal();
GetDebugDrawer().DrawPlane(ref planeNormal, planeConst, ref worldTransform, ref color);
break;
}
default:
{
if (shape.IsPolyhedral())/// for polyhedral shapes
{
PolyhedralConvexShape polyshape = shape as PolyhedralConvexShape;
ConvexPolyhedron poly = polyshape.GetConvexPolyhedron();
if (poly != null)
{
for (int i = 0; i < poly.m_faces.Count; i++)
{
IndexedVector3 centroid = IndexedVector3.Zero;
int numVerts = poly.m_faces[i].m_indices.Count;
if (numVerts != 0)
{
int lastV = poly.m_faces[i].m_indices[numVerts - 1];
for (int v = 0; v < poly.m_faces[i].m_indices.Count; v++)
{
int curVert = poly.m_faces[i].m_indices[v];
centroid += poly.m_vertices[curVert];
GetDebugDrawer().DrawLine(worldTransform * poly.m_vertices[lastV], worldTransform * poly.m_vertices[curVert], color);
lastV = curVert;
}
}
centroid *= 1.0f / (float)(numVerts);
IndexedVector3 normalColor = new IndexedVector3(1, 1, 0);
IndexedVector3 faceNormal = new IndexedVector3(poly.m_faces[i].m_plane[0], poly.m_faces[i].m_plane[1], poly.m_faces[i].m_plane[2]);
GetDebugDrawer().DrawLine(worldTransform * centroid, worldTransform * (centroid + faceNormal), normalColor);
}
}
else
{
for (int i = 0; i < polyshape.GetNumEdges(); i++)
{
IndexedVector3 a, b;
polyshape.GetEdge(i, out a, out b);
IndexedVector3 wa = worldTransform * a;
IndexedVector3 wb = worldTransform * b;
GetDebugDrawer().DrawLine(ref wa, ref wb, ref color);
}
}
}
if (shape.IsConcave())
{
ConcaveShape concaveMesh = (ConcaveShape)shape;
///@todo pass camera, for some culling? no -> we are not a graphics lib
IndexedVector3 aabbMax = MathUtil.MAX_VECTOR;
IndexedVector3 aabbMin = MathUtil.MIN_VECTOR;
using (BulletXNA.DebugDrawcallback drawCallback = BulletGlobals.DebugDrawcallbackPool.Get())
{
drawCallback.Initialise(GetDebugDrawer(), ref worldTransform, ref color);
concaveMesh.ProcessAllTriangles(drawCallback, ref aabbMin, ref aabbMax);
}
}
else if (shape.GetShapeType() == BroadphaseNativeTypes.CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE)
{
ConvexTriangleMeshShape convexMesh = (ConvexTriangleMeshShape)shape;
//todo: pass camera for some culling
IndexedVector3 aabbMax = MathUtil.MAX_VECTOR;
IndexedVector3 aabbMin = MathUtil.MIN_VECTOR;
//DebugDrawcallback drawCallback;
//DebugDrawcallback drawCallback = new DebugDrawcallback(debugDraw, ref worldTransform, ref color);
//convexMesh.GetMeshInterface().InternalProcessAllTriangles(drawCallback, ref aabbMin, ref aabbMax);
//drawCallback.Cleanup();
}
break;
}
}
}
public void DrawXNA(ref IndexedMatrix m, CollisionShape shape, ref IndexedVector3 color, DebugDrawModes debugMode, ref IndexedVector3 worldBoundsMin, ref IndexedVector3 worldBoundsMax, ref IndexedMatrix view, ref IndexedMatrix projection)
{
//btglMultMatrix(m);
if (shape == null)
{
return;
}
if (shape.GetShapeType() == BroadphaseNativeTypes.UNIFORM_SCALING_SHAPE_PROXYTYPE)
{
UniformScalingShape scalingShape = (UniformScalingShape)shape;
ConvexShape convexShape = scalingShape.GetChildShape();
float scalingFactor = scalingShape.GetUniformScalingFactor();
IndexedMatrix scaleMatrix = IndexedMatrix.CreateScale(scalingFactor);
IndexedMatrix finalMatrix = scaleMatrix * m;
DrawXNA(ref finalMatrix, convexShape, ref color, debugMode, ref worldBoundsMin, ref worldBoundsMax,ref view,ref projection);
return;
}
if (shape.GetShapeType() == BroadphaseNativeTypes.COMPOUND_SHAPE_PROXYTYPE)
{
CompoundShape compoundShape = (CompoundShape)shape;
for (int i = compoundShape.GetNumChildShapes() - 1; i >= 0; i--)
{
IndexedMatrix childTrans = compoundShape.GetChildTransform(i);
CollisionShape colShape = compoundShape.GetChildShape(i);
IndexedMatrix childMat = childTrans;
//childMat = MathUtil.bulletMatrixMultiply(m, childMat);
//childMat = childMat * m;
childMat = m * childMat;
DrawXNA(ref childMat, colShape, ref color, debugMode, ref worldBoundsMin, ref worldBoundsMax,ref view,ref projection);
}
}
else
{
bool useWireframeFallback = true;
if ((debugMode & DebugDrawModes.DBG_DrawWireframe) == 0)
{
///you can comment out any of the specific cases, and use the default
///the benefit of 'default' is that it approximates the actual collision shape including collision margin
//BroadphaseNativeTypes shapetype = m_textureEnabled ? BroadphaseNativeTypes.MAX_BROADPHASE_COLLISION_TYPES : shape.getShapeType();
BroadphaseNativeTypes shapetype = shape.GetShapeType();
switch (shapetype)
{
case BroadphaseNativeTypes.BOX_SHAPE_PROXYTYPE:
{
BoxShape boxShape = shape as BoxShape;
IndexedVector3 halfExtents = boxShape.GetHalfExtentsWithMargin();
DrawSolidCube(ref halfExtents, ref m, ref view, ref projection,ref color);
//drawSolidSphere(halfExtents.X, 10, 10, ref m, ref view, ref projection);
//drawCylinder(halfExtents.X, halfExtents.Y, 1, ref m, ref view, ref projection);
//drawSolidCone(halfExtents.Y, halfExtents.X, ref m, ref view, ref projection);
//DrawText("Hello World", new IndexedVector3(20, 20, 0), new IndexedVector3(255, 255, 255));
useWireframeFallback = false;
break;
}
case BroadphaseNativeTypes.SPHERE_SHAPE_PROXYTYPE:
{
SphereShape sphereShape = shape as SphereShape;
float radius = sphereShape.GetMargin();//radius doesn't include the margin, so draw with margin
DrawSolidSphere(radius, 10, 10, ref m, ref view, ref projection, ref color);
//glutSolidSphere(radius,10,10);
useWireframeFallback = false;
break;
}
case BroadphaseNativeTypes.CAPSULE_SHAPE_PROXYTYPE:
{
CapsuleShape capsuleShape = shape as CapsuleShape;
float radius = capsuleShape.GetRadius();
float halfHeight = capsuleShape.GetHalfHeight();
int upAxis = capsuleShape.GetUpAxis();
IndexedVector3 capStart = IndexedVector3.Zero;
capStart[upAxis] = -halfHeight;
IndexedVector3 capEnd = IndexedVector3.Zero;
capEnd[upAxis] = halfHeight;
// Draw the ends
{
IndexedMatrix childTransform = IndexedMatrix.Identity;
childTransform._origin = m * capStart;
DrawSolidSphere(radius, 5, 5, ref childTransform, ref view, ref projection, ref color);
}
{
IndexedMatrix childTransform = IndexedMatrix.Identity;
childTransform._origin = m * capEnd;
DrawSolidSphere(radius, 5, 5, ref childTransform, ref view, ref projection, ref color);
}
DrawCylinder(radius, halfHeight, upAxis, ref m, ref view, ref projection,ref color);
break;
}
case BroadphaseNativeTypes.CONE_SHAPE_PROXYTYPE:
{
ConeShape coneShape = (ConeShape)(shape);
int upIndex = coneShape.GetConeUpIndex();
float radius = coneShape.GetRadius();//+coneShape.getMargin();
float height = coneShape.GetHeight();//+coneShape.getMargin();
IndexedMatrix rotateMatrix = IndexedMatrix.Identity;
switch (upIndex)
{
case 0:
rotateMatrix = IndexedMatrix.CreateRotationX(-MathUtil.SIMD_HALF_PI);
break;
case 1:
break;
case 2:
rotateMatrix = IndexedMatrix.CreateRotationX(MathUtil.SIMD_HALF_PI);
break;
default:
{
break;
}
};
IndexedMatrix translationMatrix = IndexedMatrix.CreateTranslation(0f, 0f, -0.5f * height);
IndexedMatrix resultant = m * rotateMatrix * translationMatrix;
DrawSolidCone(height, radius, ref resultant, ref view, ref projection, ref color);
useWireframeFallback = false;
break;
}
case BroadphaseNativeTypes.STATIC_PLANE_PROXYTYPE:
{
StaticPlaneShape staticPlaneShape = shape as StaticPlaneShape;
float planeConst = staticPlaneShape.GetPlaneConstant();
IndexedVector3 planeNormal = staticPlaneShape.GetPlaneNormal();
IndexedVector3 planeOrigin = planeNormal * planeConst;
IndexedVector3 vec0, vec1;
TransformUtil.PlaneSpace1(ref planeNormal, out vec0, out vec1);
float vecLen = 100f;
IndexedVector3 pt0 = planeOrigin + vec0 * vecLen;
IndexedVector3 pt1 = planeOrigin - vec0 * vecLen;
IndexedVector3 pt2 = planeOrigin + vec1 * vecLen;
IndexedVector3 pt3 = planeOrigin - vec1 * vecLen;
// Fallback to debug draw - needs tidying
IndexedVector3 colour = new IndexedVector3(255, 255, 255);
DrawLine(ref pt0, ref pt1, ref colour);
DrawLine(ref pt1, ref pt2, ref colour);
DrawLine(ref pt2, ref pt3, ref colour);
DrawLine(ref pt3, ref pt1, ref colour);
break;
}
case BroadphaseNativeTypes.CYLINDER_SHAPE_PROXYTYPE:
{
CylinderShape cylinder = (CylinderShape)(shape);
int upAxis = cylinder.GetUpAxis();
float radius = cylinder.GetRadius();
float halfHeight = cylinder.GetHalfExtentsWithMargin()[upAxis];
DrawCylinder(radius, halfHeight, upAxis, ref m, ref view, ref projection, ref color);
break;
}
default:
{
if (shape.IsConvex())
{
ShapeCache sc=Cache(shape as ConvexShape);
//if (shape.getUserPointer())
{
//glutSolidCube(1.0);
ShapeHull hull = sc.m_shapehull/*(btShapeHull*)shape.getUserPointer()*/;
int numTriangles = hull.NumTriangles();
int numIndices = hull.NumIndices();
int numVertices = hull.NumVertices();
if (numTriangles > 0)
{
int index = 0;
IList<int> idx = hull.m_indices;
IList<IndexedVector3> vtx = hull.m_vertices;
for (int i = 0; i < numTriangles; i++)
{
int i1 = index++;
int i2 = index++;
int i3 = index++;
Debug.Assert(i1 < numIndices &&
i2 < numIndices &&
i3 < numIndices);
int index1 = idx[i1];
int index2 = idx[i2];
int index3 = idx[i3];
Debug.Assert(index1 < numVertices &&
index2 < numVertices &&
index3 < numVertices);
IndexedVector3 v1 = m * vtx[index1];
IndexedVector3 v2 = m * vtx[index2];
IndexedVector3 v3 = m * vtx[index3];
IndexedVector3 normal = IndexedVector3.Cross((v3-v1),(v2-v1));
normal.Normalize();
Vector2 tex = new Vector2(0,0);
AddVertex(ref v1, ref normal,ref tex);
AddVertex(ref v2, ref normal, ref tex);
AddVertex(ref v3, ref normal, ref tex);
}
}
}
}
break;
}
}
}
/// for polyhedral shapes
if (debugMode == DebugDrawModes.DBG_DrawFeaturesText && (shape.IsPolyhedral()))
{
PolyhedralConvexShape polyshape = (PolyhedralConvexShape)shape;
{
//BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),polyshape.getExtraDebugInfo());
IndexedVector3 colour = new IndexedVector3(255, 255, 255);
for (int i = 0; i < polyshape.GetNumVertices(); i++)
{
IndexedVector3 vtx;
polyshape.GetVertex(i, out vtx);
String buf = " " + i;
DrawText(buf, ref vtx, ref colour);
}
for (int i = 0; i < polyshape.GetNumPlanes(); i++)
{
IndexedVector3 normal;
IndexedVector3 vtx;
polyshape.GetPlane(out normal, out vtx, i);
float d = IndexedVector3.Dot(vtx, normal);
vtx *= d;
String buf = " plane " + i;
DrawText(buf, ref vtx, ref colour);
}
}
}
if (shape.IsConcave() && !shape.IsInfinite())//>getShapeType() == TRIANGLE_MESH_SHAPE_PROXYTYPE||shape.getShapeType() == GIMPACT_SHAPE_PROXYTYPE)
// if (shape.getShapeType() == TRIANGLE_MESH_SHAPE_PROXYTYPE)
{
ConcaveShape concaveMesh = shape as ConcaveShape;
XNADrawcallback drawCallback = new XNADrawcallback(this,ref m);
drawCallback.m_wireframe = (debugMode & DebugDrawModes.DBG_DrawWireframe) != 0;
concaveMesh.ProcessAllTriangles(drawCallback, ref worldBoundsMin, ref worldBoundsMax);
}
//glDisable(GL_DEPTH_TEST);
//glRasterPos3f(0,0,0);//mvtx.x(), vtx.y(), vtx.z());
if ((debugMode & DebugDrawModes.DBG_DrawText) != 0)
{
IndexedVector3 position = IndexedVector3.Zero;
IndexedVector3 colour = new IndexedVector3(255, 255, 255);
DrawText(shape.GetName(), ref position, ref colour);
}
if ((debugMode & DebugDrawModes.DBG_DrawFeaturesText) != 0)
{
//drawText(shape.getEx]
//BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),shape.getExtraDebugInfo());
}
//glEnable(GL_DEPTH_TEST);
//// glPopMatrix();
//if(m_textureenabled) glDisable(GL_TEXTURE_2D);
// }
// glPopMatrix();
}
}