public void DebugDrawObject(Matrix worldTransform, CollisionShape shape, Vector3 color)
{
if (shape.ShapeType == BroadphaseNativeTypes.Compound)
{
CompoundShape compoundShape = shape as CompoundShape;
for (int i = compoundShape.ChildShapeCount - 1; i >= 0; i--)
{
Matrix childTrans = compoundShape.GetChildTransform(i);
CollisionShape colShape = compoundShape.GetChildShape(i);
DebugDrawObject(worldTransform * childTrans, colShape, color);
}
}
else
{
switch (shape.ShapeType)
{
case BroadphaseNativeTypes.Sphere:
{
SphereShape sphereShape = shape as SphereShape;
float radius = sphereShape.Margin;//radius doesn't include the margin, so draw with margin
Vector3 start = worldTransform.Translation;
DebugDrawer.DrawLine(start, start + Vector3.TransformNormal(new Vector3(radius, 0, 0), worldTransform), color);
DebugDrawer.DrawLine(start, start + Vector3.TransformNormal(new Vector3(0, radius, 0), worldTransform), color);
DebugDrawer.DrawLine(start, start + Vector3.TransformNormal(new Vector3(0, 0, radius), worldTransform), color);
//drawSphere
break;
}
case BroadphaseNativeTypes.MultiSphere:
case BroadphaseNativeTypes.Cone:
{
ConeShape coneShape = shape as ConeShape;
float radius = coneShape.Radius;//+coneShape->getMargin();
float height = coneShape.Height;//+coneShape->getMargin();
Vector3 start = worldTransform.Translation;
DebugDrawer.DrawLine(start + Vector3.TransformNormal(new Vector3(0f, 0f, 0.5f * height), worldTransform), start + Vector3.TransformNormal(new Vector3(radius, 0f, -0.5f * height), worldTransform), color);
DebugDrawer.DrawLine(start + Vector3.TransformNormal(new Vector3(0f, 0f, 0.5f * height), worldTransform), start + Vector3.TransformNormal(new Vector3(-radius, 0f, -0.5f * height), worldTransform), color);
DebugDrawer.DrawLine(start + Vector3.TransformNormal(new Vector3(0f, 0f, 0.5f * height), worldTransform), start + Vector3.TransformNormal(new Vector3(0f, radius, -0.5f * height), worldTransform), color);
DebugDrawer.DrawLine(start + Vector3.TransformNormal(new Vector3(0f, 0f, 0.5f * height), worldTransform), start + Vector3.TransformNormal(new Vector3(0f, -radius, -0.5f * height), worldTransform), color);
break;
}
case BroadphaseNativeTypes.Cylinder:
{
CylinderShape cylinder = shape as CylinderShape;
int upAxis = cylinder.UpAxis;
float radius = cylinder.Radius;
float halfHeight = MathHelper.GetElement(cylinder.HalfExtents, upAxis);
Vector3 start = worldTransform.Translation;
Vector3 offsetHeight = new Vector3();
MathHelper.SetElement(ref offsetHeight, upAxis, halfHeight);
Vector3 offsetRadius = new Vector3();
MathHelper.SetElement(ref offsetRadius, (upAxis + 1) % 3, radius);
DebugDrawer.DrawLine(start + Vector3.TransformNormal(offsetHeight + offsetRadius, worldTransform), start + Vector3.TransformNormal(-offsetHeight + offsetRadius, worldTransform), color);
DebugDrawer.DrawLine(start + Vector3.TransformNormal(offsetHeight - offsetRadius, worldTransform), start + Vector3.TransformNormal(-offsetHeight - offsetRadius, worldTransform), color);
break;
}
default:
{
if (shape.ShapeType == BroadphaseNativeTypes.TriangleMesh)
{
TriangleMeshShape concaveMesh = shape as TriangleMeshShape;
//btVector3 aabbMax(1e30f,1e30f,1e30f);
//btVector3 aabbMax(100,100,100);//1e30f,1e30f,1e30f);
//todo pass camera, for some culling
Vector3 aabbMax = new Vector3(1e30f, 1e30f, 1e30f);
Vector3 aabbMin = new Vector3(-1e30f, -1e30f, -1e30f);
DebugDrawCallback drawCallback = new DebugDrawCallback(DebugDrawer, worldTransform, color);
concaveMesh.ProcessAllTriangles(drawCallback, aabbMin, aabbMax);
}
if (shape.ShapeType == BroadphaseNativeTypes.ConvexTriangleMesh)
{
ConvexTriangleMeshShape convexMesh = shape as ConvexTriangleMeshShape;
//todo: pass camera for some culling
Vector3 aabbMax = new Vector3(1e30f, 1e30f, 1e30f);
Vector3 aabbMin = new Vector3(-1e30f, -1e30f, -1e30f);
//DebugDrawcallback drawCallback;
DebugDrawCallback drawCallback = new DebugDrawCallback(DebugDrawer, worldTransform, color);
convexMesh.getStridingMesh().InternalProcessAllTriangles(drawCallback, aabbMin, aabbMax);
}
// for polyhedral shapes
if (shape.IsPolyhedral)
{
PolyhedralConvexShape polyshape = shape as PolyhedralConvexShape;
for (int i = 0; i < polyshape.EdgeCount; i++)
{
Vector3 a, b;
polyshape.GetEdge(i, out a, out b);
a = Vector3.TransformNormal(a, worldTransform);
b = Vector3.TransformNormal(b, worldTransform);
DebugDrawer.DrawLine(a, b, color);
}
}
break;
}
}
}
}
public void DebugDrawObject(Matrix worldTransform, CollisionShape shape, Vector3 color)
{
if (shape.ShapeType == BroadphaseNativeTypes.Compound)
{
CompoundShape compoundShape = shape as CompoundShape;
for (int i = compoundShape.ChildShapeCount - 1; i >= 0; i--)
{
Matrix childTrans = compoundShape.GetChildTransform(i);
CollisionShape colShape = compoundShape.GetChildShape(i);
DebugDrawObject(worldTransform * childTrans, colShape, color);
}
}
else
{
switch (shape.ShapeType)
{
case BroadphaseNativeTypes.Sphere:
{
SphereShape sphereShape = shape as SphereShape;
float radius = sphereShape.Margin; //radius doesn't include the margin, so draw with margin
Vector3 start = worldTransform.Translation;
DebugDrawer.DrawLine(start, start + Vector3.TransformNormal(new Vector3(radius, 0, 0), worldTransform), color);
DebugDrawer.DrawLine(start, start + Vector3.TransformNormal(new Vector3(0, radius, 0), worldTransform), color);
DebugDrawer.DrawLine(start, start + Vector3.TransformNormal(new Vector3(0, 0, radius), worldTransform), color);
//drawSphere
break;
}
case BroadphaseNativeTypes.MultiSphere:
case BroadphaseNativeTypes.Cone:
{
ConeShape coneShape = shape as ConeShape;
float radius = coneShape.Radius; //+coneShape->getMargin();
float height = coneShape.Height; //+coneShape->getMargin();
Vector3 start = worldTransform.Translation;
DebugDrawer.DrawLine(start + Vector3.TransformNormal(new Vector3(0f, 0f, 0.5f * height), worldTransform), start + Vector3.TransformNormal(new Vector3(radius, 0f, -0.5f * height), worldTransform), color);
DebugDrawer.DrawLine(start + Vector3.TransformNormal(new Vector3(0f, 0f, 0.5f * height), worldTransform), start + Vector3.TransformNormal(new Vector3(-radius, 0f, -0.5f * height), worldTransform), color);
DebugDrawer.DrawLine(start + Vector3.TransformNormal(new Vector3(0f, 0f, 0.5f * height), worldTransform), start + Vector3.TransformNormal(new Vector3(0f, radius, -0.5f * height), worldTransform), color);
DebugDrawer.DrawLine(start + Vector3.TransformNormal(new Vector3(0f, 0f, 0.5f * height), worldTransform), start + Vector3.TransformNormal(new Vector3(0f, -radius, -0.5f * height), worldTransform), color);
break;
}
case BroadphaseNativeTypes.Cylinder:
{
CylinderShape cylinder = shape as CylinderShape;
int upAxis = cylinder.UpAxis;
float radius = cylinder.Radius;
float halfHeight = MathHelper.GetElement(cylinder.HalfExtents, upAxis);
Vector3 start = worldTransform.Translation;
Vector3 offsetHeight = new Vector3();
MathHelper.SetElement(ref offsetHeight, upAxis, halfHeight);
Vector3 offsetRadius = new Vector3();
MathHelper.SetElement(ref offsetRadius, (upAxis + 1) % 3, radius);
DebugDrawer.DrawLine(start + Vector3.TransformNormal(offsetHeight + offsetRadius, worldTransform), start + Vector3.TransformNormal(-offsetHeight + offsetRadius, worldTransform), color);
DebugDrawer.DrawLine(start + Vector3.TransformNormal(offsetHeight - offsetRadius, worldTransform), start + Vector3.TransformNormal(-offsetHeight - offsetRadius, worldTransform), color);
break;
}
default:
{
if (shape.ShapeType == BroadphaseNativeTypes.TriangleMesh)
{
TriangleMeshShape concaveMesh = shape as TriangleMeshShape;
//btVector3 aabbMax(1e30f,1e30f,1e30f);
//btVector3 aabbMax(100,100,100);//1e30f,1e30f,1e30f);
//todo pass camera, for some culling
Vector3 aabbMax = new Vector3(1e30f, 1e30f, 1e30f);
Vector3 aabbMin = new Vector3(-1e30f, -1e30f, -1e30f);
DebugDrawCallback drawCallback = new DebugDrawCallback(DebugDrawer, worldTransform, color);
concaveMesh.ProcessAllTriangles(drawCallback, aabbMin, aabbMax);
}
if (shape.ShapeType == BroadphaseNativeTypes.ConvexTriangleMesh)
{
ConvexTriangleMeshShape convexMesh = shape as ConvexTriangleMeshShape;
//todo: pass camera for some culling
Vector3 aabbMax = new Vector3(1e30f, 1e30f, 1e30f);
Vector3 aabbMin = new Vector3(-1e30f, -1e30f, -1e30f);
//DebugDrawcallback drawCallback;
DebugDrawCallback drawCallback = new DebugDrawCallback(DebugDrawer, worldTransform, color);
convexMesh.getStridingMesh().InternalProcessAllTriangles(drawCallback, aabbMin, aabbMax);
}
// for polyhedral shapes
if (shape.IsPolyhedral)
{
PolyhedralConvexShape polyshape = shape as PolyhedralConvexShape;
for (int i = 0; i < polyshape.EdgeCount; i++)
{
Vector3 a, b;
polyshape.GetEdge(i, out a, out b);
a = Vector3.TransformNormal(a, worldTransform);
b = Vector3.TransformNormal(b, worldTransform);
DebugDrawer.DrawLine(a, b, color);
}
}
break;
}
}
}
}