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; } } } }