public static void DebugDrawObject(ref Matrix worldTransform, CollisionShape shape, ref Vector3 color,
IDebugDraw debugDraw)
{
// Draw a small simplex at the center of the object
{
Vector3 start = worldTransform.Translation;
float scale = 10f;
debugDraw.DrawLine(start, start + (Vector3.TransformNormal(Vector3.Right, worldTransform) * scale), Vector3.Right);
debugDraw.DrawLine(start, start + (Vector3.TransformNormal(Vector3.Up, worldTransform) * scale), Vector3.Up);
debugDraw.DrawLine(start, start + (Vector3.TransformNormal(Vector3.Backward, worldTransform) * scale),
Vector3.Backward);
}
//return;
if (shape.ShapeType == BroadphaseNativeTypes.COMPOUND_SHAPE_PROXYTYPE)
{
var compoundShape = (CompoundShape)shape;
for (int i = compoundShape.GetNumChildShapes() - 1; i >= 0; i--)
{
Matrix childTrans = compoundShape.GetChildTransform(i);
CollisionShape colShape = compoundShape.GetChildShape(i);
Matrix temp = MathUtil.BulletMatrixMultiply(worldTransform, childTrans);
DebugDrawObject(ref temp, colShape, ref color, debugDraw);
}
}
else
{
switch (shape.ShapeType)
{
case BroadphaseNativeTypes.SPHERE_SHAPE_PROXYTYPE:
{
var sphereShape = (SphereShape)shape;
float radius = sphereShape.Margin; //radius doesn't include the margin, so draw with margin
DebugDrawSphere(radius, ref worldTransform, ref color, debugDraw);
break;
}
case BroadphaseNativeTypes.MULTI_SPHERE_SHAPE_PROXYTYPE:
{
var multiSphereShape = (MultiSphereShape)shape;
for (int i = multiSphereShape.GetSphereCount() - 1; i >= 0; i--)
{
Matrix childTransform = worldTransform;
childTransform.Translation += multiSphereShape.GetSpherePosition(i);
DebugDrawSphere(multiSphereShape.GetSphereRadius(i), ref childTransform, ref color, debugDraw);
}
break;
}
case BroadphaseNativeTypes.CAPSULE_SHAPE_PROXYTYPE:
{
var capsuleShape = (CapsuleShape)shape;
float radius = capsuleShape.getRadius();
float halfHeight = capsuleShape.getHalfHeight();
int upAxis = capsuleShape.GetUpAxis();
Vector3 capStart = Vector3.Zero;
;
MathUtil.VectorComponent(ref capStart, upAxis, -halfHeight);
Vector3 capEnd = Vector3.Zero;
MathUtil.VectorComponent(ref capEnd, upAxis, halfHeight);
// Draw the ends
{
Matrix childTransform = worldTransform;
childTransform.Translation = Vector3.Transform(capStart, worldTransform);
DebugDrawSphere(radius, ref childTransform, ref color, debugDraw);
}
{
Matrix childTransform = worldTransform;
childTransform.Translation = Vector3.Transform(capEnd, worldTransform);
DebugDrawSphere(radius, ref childTransform, ref color, debugDraw);
}
// Draw some additional lines
Vector3 start = worldTransform.Translation;
MathUtil.VectorComponent(ref capStart, (upAxis + 1) % 3, radius);
MathUtil.VectorComponent(ref capEnd, (upAxis + 1) % 3, radius);
debugDraw.DrawLine(start + Vector3.TransformNormal(capStart, worldTransform),
start + Vector3.TransformNormal(capEnd, worldTransform), color);
MathUtil.VectorComponent(ref capStart, (upAxis + 1) % 3, -radius);
MathUtil.VectorComponent(ref capEnd, (upAxis + 1) % 3, -radius);
debugDraw.DrawLine(start + Vector3.TransformNormal(capStart, worldTransform),
start + Vector3.TransformNormal(capEnd, worldTransform), color);
MathUtil.VectorComponent(ref capStart, (upAxis + 1) % 3, radius);
MathUtil.VectorComponent(ref capEnd, (upAxis + 1) % 3, radius);
MathUtil.VectorComponent(ref capStart, (upAxis + 2) % 3, radius);
MathUtil.VectorComponent(ref capEnd, (upAxis + 2) % 3, radius);
debugDraw.DrawLine(start + Vector3.TransformNormal(capStart, worldTransform),
start + Vector3.TransformNormal(capEnd, worldTransform), color);
MathUtil.VectorComponent(ref capStart, (upAxis + 2) % 3, -radius);
MathUtil.VectorComponent(ref capEnd, (upAxis + 2) % 3, -radius);
debugDraw.DrawLine(start + Vector3.TransformNormal(capStart, worldTransform),
start + Vector3.TransformNormal(capEnd, worldTransform), color);
break;
}
case BroadphaseNativeTypes.CONE_SHAPE_PROXYTYPE:
{
var coneShape = (ConeShape)shape;
float radius = coneShape.GetRadius(); //+coneShape->getMargin();
float height = coneShape.GetHeight(); //+coneShape->getMargin();
Vector3 start = worldTransform.Translation;
int upAxis = coneShape.GetConeUpIndex();
Vector3 offsetHeight = Vector3.Zero;
MathUtil.VectorComponent(ref offsetHeight, upAxis, height * 0.5f);
Vector3 offsetRadius = Vector3.Zero;
MathUtil.VectorComponent(ref offsetRadius, (upAxis + 1) % 3, radius);
Vector3 offset2Radius = Vector3.Zero;
MathUtil.VectorComponent(ref offsetRadius, (upAxis + 2) % 3, radius);
debugDraw.DrawLine(start + Vector3.TransformNormal(offsetHeight, worldTransform),
start + Vector3.TransformNormal(-offsetHeight + offsetRadius, worldTransform), color);
debugDraw.DrawLine(start + Vector3.TransformNormal(offsetHeight, worldTransform),
start + Vector3.TransformNormal(-offsetHeight - offsetRadius, worldTransform), color);
debugDraw.DrawLine(start + Vector3.TransformNormal(offsetHeight, worldTransform),
start + Vector3.TransformNormal(-offsetHeight + offset2Radius, worldTransform), color);
debugDraw.DrawLine(start + Vector3.TransformNormal(offsetHeight, worldTransform),
start + Vector3.TransformNormal(-offsetHeight - offset2Radius, worldTransform), color);
break;
}
case BroadphaseNativeTypes.CYLINDER_SHAPE_PROXYTYPE:
{
var cylinder = (CylinderShape)shape;
int upAxis = cylinder.GetUpAxis();
float radius = cylinder.GetRadius();
float halfHeight = MathUtil.VectorComponent(cylinder.GetHalfExtentsWithMargin(), upAxis);
Vector3 start = worldTransform.Translation;
Vector3 offsetHeight = Vector3.Zero;
MathUtil.VectorComponent(ref offsetHeight, upAxis, halfHeight);
Vector3 offsetRadius = Vector3.Zero;
MathUtil.VectorComponent(ref offsetRadius, (upAxis + 1) % 3, radius);
debugDraw.DrawLine(start + Vector3.TransformNormal(offsetHeight + offsetRadius, worldTransform),
start + Vector3.TransformNormal(-offsetHeight + offsetRadius, worldTransform), color);
debugDraw.DrawLine(start + Vector3.TransformNormal(offsetHeight - offsetRadius, worldTransform),
start + Vector3.TransformNormal(-offsetHeight - offsetRadius, worldTransform), color);
break;
}
case BroadphaseNativeTypes.STATIC_PLANE_PROXYTYPE:
{
var staticPlaneShape = (StaticPlaneShape)shape;
float planeConst = staticPlaneShape.GetPlaneConstant();
Vector3 planeNormal = staticPlaneShape.GetPlaneNormal();
Vector3 planeOrigin = planeNormal * planeConst;
Vector3 vec0 = Vector3.Zero, vec1 = Vector3.Zero;
TransformUtil.PlaneSpace1(ref planeNormal, ref vec0, ref vec1);
float vecLen = 100f;
Vector3 pt0 = planeOrigin + vec0 * vecLen;
Vector3 pt1 = planeOrigin - vec0 * vecLen;
Vector3 pt2 = planeOrigin + vec1 * vecLen;
Vector3 pt3 = planeOrigin - vec1 * vecLen;
debugDraw.DrawLine(Vector3.Transform(pt0, worldTransform), Vector3.Transform(pt1, worldTransform), color);
debugDraw.DrawLine(Vector3.Transform(pt2, worldTransform), Vector3.Transform(pt3, worldTransform), color);
break;
}
//case (BroadphaseNativeTypes.BOX_SHAPE_PROXYTYPE):
// {
// BoxShape boxShape = (BoxShape)shape;
// Vector3 minPos = Vector3.Zero;
// Vector3 maxPos = Vector3.Zero;
// Matrix transform = Matrix.Identity;
// boxShape.getAabb(ref transform, ref minPos,ref maxPos);
// debugDraw.drawBox(ref minPos, ref maxPos, ref worldTransform, ref color);
// break;
// }
default:
{
if (shape.IsConcave())
{
var concaveMesh = (ConcaveShape)shape;
///@todo pass camera, for some culling? no -> we are not a graphics lib
Vector3 aabbMax = MathUtil.MAX_VECTOR;
Vector3 aabbMin = MathUtil.MIN_VECTOR;
var drawCallback = new DebugDrawcallback(debugDraw, ref worldTransform, ref color);
concaveMesh.ProcessAllTriangles(drawCallback, ref aabbMin, ref aabbMax);
drawCallback.Cleanup();
}
else if (shape.ShapeType == BroadphaseNativeTypes.CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE)
{
var convexMesh = (ConvexTriangleMeshShape)shape;
//todo: pass camera for some culling
Vector3 aabbMax = MathUtil.MAX_VECTOR;
Vector3 aabbMin = MathUtil.MIN_VECTOR;
//DebugDrawcallback drawCallback;
var drawCallback = new DebugDrawcallback(debugDraw, ref worldTransform, ref color);
convexMesh.GetMeshInterface().InternalProcessAllTriangles(drawCallback, ref aabbMin, ref aabbMax);
drawCallback.Cleanup();
}
else /// for polyhedral shapes
if (shape.IsPolyhedral())
{
var polyshape = (PolyhedralConvexShape)shape;
for (int i = 0; i < polyshape.GetNumEdges(); i++)
{
Vector3 a = Vector3.Zero, b = Vector3.Zero;
polyshape.GetEdge(i, ref a, ref b);
Vector3 wa = Vector3.Transform(a, worldTransform);
Vector3 wb = Vector3.Transform(b, worldTransform);
debugDraw.DrawLine(ref wa, ref wb, ref color);
}
}
break;
}
}
}
}
