public override void OnDrawGizmosSelected() { if (!drawGizmo) { return; } Gizmos.color = Color.yellow; CompoundShape compoundShape = GetCollisionShape() as CompoundShape; Matrix4x4 parentMatrix = this.transform.localToWorldMatrix * Matrix4x4.Scale(transform.lossyScale).inverse; for (int i = 0; i < compoundShape.NumChildShapes; i++) { CollisionShape collisionShape = compoundShape.GetChildShape(i); BulletSharp.Math.Matrix childShapeTransform = compoundShape.GetChildTransform(i); //childShapeTransform.Invert(); Gizmos.matrix = parentMatrix * childShapeTransform.ToUnity(); ConvexHullShape convexShape = collisionShape as ConvexHullShape; if (convexShape != null) { int nbEdges = convexShape.NumEdges; for (int j = 0; j < nbEdges; j++) { BulletSharp.Math.Vector3 vertex1; BulletSharp.Math.Vector3 vertex2; convexShape.GetEdge(j, out vertex1, out vertex2); Vector3 vertexUnity1 = BSExtensionMethods2.ToUnity(vertex1); Vector3 vertexUnity2 = BSExtensionMethods2.ToUnity(vertex2); Gizmos.DrawLine(vertexUnity1, vertexUnity2); } } BvhTriangleMeshShape triangleShape = collisionShape as BvhTriangleMeshShape; if (triangleShape != null) { DisplayTriangleCallback cb = new DisplayTriangleCallback(); triangleShape.MeshInterface.InternalProcessAllTriangles(cb, triangleShape.LocalAabbMin, triangleShape.LocalAabbMax); } } }
//[Import()] //VVVV. FLog; public void Evaluate(int SpreadMax) { if (this.FBodies.PluginIO.IsConnected) { this.FId.SliceCount = this.FBodies.SliceCount; this.FPosition.SliceCount = this.FBodies.SliceCount; this.FRotation.SliceCount = this.FBodies.SliceCount; this.FShapes.SliceCount = this.FBodies.SliceCount; this.FCustom.SliceCount = this.FBodies.SliceCount; this.FIsNew.SliceCount = this.FBodies.SliceCount; this.FLinVel.SliceCount = this.FBodies.SliceCount; this.FAngVel.SliceCount = this.FBodies.SliceCount; this.FActive.SliceCount = this.FBodies.SliceCount; this.FContactResponse.SliceCount = this.FBodies.SliceCount; this.FStatic.SliceCount = this.FBodies.SliceCount; this.FKinematic.SliceCount = this.FBodies.SliceCount; this.FShapeTransform.SliceCount = this.FBodies.SliceCount; this.FHasCustomObj.SliceCount = this.FBodies.SliceCount; this.FCustomObj.SliceCount = this.FBodies.SliceCount; List <Matrix4x4> transforms = new List <Matrix4x4>(); for (int i = 0; i < SpreadMax; i++) { RigidBody body = this.FBodies[i]; this.FPosition[i] = new Vector3D(body.MotionState.WorldTransform.M41, body.MotionState.WorldTransform.M42, body.MotionState.WorldTransform.M43); Quaternion rot = body.Orientation; this.FRotation[i] = new Vector4D(rot.X, rot.Y, rot.Z, rot.W); this.FLinVel[i] = body.LinearVelocity.ToVVVVector(); this.FAngVel[i] = body.AngularVelocity.ToVVVVector(); CollisionShape shape = body.CollisionShape; if (shape.IsCompound) { //CompoundShape sp = new CompoundShape( CompoundShape comp = (CompoundShape)shape; this.FShapes[i].SliceCount = comp.NumChildShapes; this.FShapeTransform[i].SliceCount = comp.NumChildShapes; for (int j = 0; j < comp.NumChildShapes; j++) { CollisionShape child = comp.GetChildShape(j); this.FShapes[i][j] = child; Matrix m = comp.GetChildTransform(j); Matrix4x4 mn = new Matrix4x4(m.M11, m.M12, m.M13, m.M14, m.M21, m.M22, m.M23, m.M24, m.M31, m.M32, m.M33, m.M34, m.M41, m.M42, m.M43, m.M44); mn *= VMath.Scale(child.LocalScaling.ToVVVVector()); this.FShapeTransform[i][j] = mn; //comp. //comp.GetChildTransform( //this.FShapes[i][j]. } } else { this.FShapes[i].SliceCount = 1; this.FShapes[i][0] = shape; this.FShapeTransform[i].SliceCount = 1; this.FShapeTransform[i][0] = VMath.Scale(shape.LocalScaling.ToVVVVector()); //transforms.Add(VMath.IdentityMatrix); } BodyCustomData bd = (BodyCustomData)body.UserObject; ShapeCustomData sc = (ShapeCustomData)shape.UserObject; this.FActive[i] = body.IsActive; this.FContactResponse[i] = body.HasContactResponse; this.FStatic[i] = body.IsStaticObject; this.FKinematic[i] = body.IsKinematicObject; //this.FShapeCount[i] = sc.ShapeDef.ShapeCount; this.FId[i] = bd.Id; this.FIsNew[i] = bd.Created; this.FCustom[i] = bd.Custom; if (bd.CustomObject != null) { this.FHasCustomObj[i] = true; this.FCustomObj[i] = bd.CustomObject; } else { this.FHasCustomObj[i] = false; this.FCustomObj[i] = null; } } //this.FShapeTransform.SliceCount = transforms.Count; //this.FShapeTransform.AssignFrom(transforms); } else { this.FId.SliceCount = 0; this.FPosition.SliceCount = 0; this.FRotation.SliceCount = 0; this.FShapes.SliceCount = 0; this.FCustom.SliceCount = 0; this.FIsNew.SliceCount = 0; this.FLinVel.SliceCount = 0; this.FAngVel.SliceCount = 0; this.FShapeTransform.SliceCount = 0; } }
public override void OnDrawGizmosSelected() { if (!drawGizmo) { return; } Gizmos.color = Color.yellow; CompoundShape compoundShape = GetCollisionShape() as CompoundShape; for (int i = 0; i < compoundShape.NumChildShapes; i++) { CollisionShape collisionShape = compoundShape.GetChildShape(i); ConvexHullShape convexShape = collisionShape as ConvexHullShape; if (convexShape != null) { BulletSharp.Math.Matrix childShapeTransform = compoundShape.GetChildTransform(i); //childShapeTransform.Invert(); BulletSharp.Math.Matrix shapeTransform = childShapeTransform * this.transform.localToWorldMatrix.ToBullet(); Gizmos.matrix = shapeTransform.ToUnity(); int nbEdges = convexShape.NumEdges; for (int j = 0; j < nbEdges; j++) { BulletSharp.Math.Vector3 vertex1; BulletSharp.Math.Vector3 vertex2; convexShape.GetEdge(j, out vertex1, out vertex2); Vector3 vertexUnity1 = BSExtensionMethods2.ToUnity(vertex1); Vector3 vertexUnity2 = BSExtensionMethods2.ToUnity(vertex2); Gizmos.DrawLine(vertexUnity1, vertexUnity2); } /*Mesh collisionMesh = new Mesh(); * Vector3[] newVertices = new Vector3[convexShape.NumVertices]; * int[] triangles = new int[convexShape.NumVertices * 3]; * for (int j = 0; j < convexShape.NumVertices; j++) * { * BulletSharp.Math.Vector3 vertex1; * convexShape.GetVertex(j, out vertex1); * newVertices[j] = vertex1.ToUnity(); * triangles[j] = j; * } * collisionMesh.vertices = newVertices; * collisionMesh.triangles = triangles; * collisionMesh.RecalculateNormals(); * Gizmos.color = Color.blue; * Gizmos.DrawMesh(collisionMesh); */ } BvhTriangleMeshShape triangleShape = collisionShape as BvhTriangleMeshShape; if (triangleShape != null) { BulletSharp.Math.Matrix shapeTransform = this.transform.localToWorldMatrix.ToBullet() * compoundShape.GetChildTransform(i); Gizmos.matrix = BSExtensionMethods2.ToUnity(shapeTransform); /*int nbEdges = triangleShape.; * for (int j = 0; j < nbEdges; j++) * { * BulletSharp.Math.Vector3 vertex1; * BulletSharp.Math.Vector3 vertex2; * triangleShape.GetEdge(j, out vertex1, out vertex2); * Vector3 vertexUnity1 = BSExtensionMethods2.ToUnity(vertex1); * Vector3 vertexUnity2 = BSExtensionMethods2.ToUnity(vertex2); * Gizmos.DrawLine(vertexUnity1, vertexUnity2); * }*/ } } }
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(); } }
public virtual void DrawShadow(ref IndexedMatrix m, ref IndexedVector3 extrusion, CollisionShape shape, ref IndexedVector3 worldBoundsMin, ref IndexedVector3 worldBoundsMax) { if (shape.GetShapeType() == BroadphaseNativeTypes.UNIFORM_SCALING_SHAPE_PROXYTYPE) { UniformScalingShape scalingShape = (UniformScalingShape)(shape); ConvexShape convexShape = scalingShape.GetChildShape(); float scalingFactor = (float)scalingShape.GetUniformScalingFactor(); IndexedMatrix tmpScaling = IndexedMatrix.CreateScale(scalingFactor); tmpScaling *= m; DrawShadow(ref tmpScaling, ref extrusion, convexShape, ref worldBoundsMin, ref worldBoundsMax); return; } else 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); //float childMat[16]; //childTrans.getOpenGLMatrix(childMat); IndexedVector3 transformedExtrude = childTrans._basis * extrusion; DrawShadow(ref childTrans, ref transformedExtrude, colShape, ref worldBoundsMin, ref worldBoundsMax); } } else { bool useWireframeFallback = true; if (shape.IsConvex()) { ShapeCache sc = Cache(shape as ConvexShape); ShapeHull hull = sc.m_shapehull; //glBegin(GL_QUADS); for (int i = 0; i < sc.m_edges.Count; ++i) { float d = IndexedVector3.Dot(sc.m_edges[i].n[0], extrusion); if ((d * IndexedVector3.Dot(sc.m_edges[i].n[1], extrusion)) < 0) { int q = d < 0?1:0; IndexedVector3 a = hull.m_vertices[sc.m_edges[i].v[q]]; IndexedVector3 b = hull.m_vertices[sc.m_edges[i].v[1 - q]]; IndexedVector3 ae = a + extrusion; IndexedVector3 be = b + extrusion; Vector2 tex = new Vector2(0, 0); // fix me. IndexedVector3 normal = new IndexedVector3(0, 1, 0); // gl_quad turned into two triangles. AddVertex(ref a, ref normal, ref tex); AddVertex(ref b, ref normal, ref tex); AddVertex(ref be, ref normal, ref tex); AddVertex(ref be, ref normal, ref tex); AddVertex(ref ae, ref normal, ref tex); AddVertex(ref a, ref normal, ref tex); } } //glEnd(); } } if (shape.IsConcave()) //>getShapeType() == TRIANGLE_MESH_SHAPE_PROXYTYPE||shape.getShapeType() == GIMPACT_SHAPE_PROXYTYPE) // if (shape.getShapeType() == TRIANGLE_MESH_SHAPE_PROXYTYPE) { ConcaveShape concaveMesh = (ConcaveShape)shape; XNADrawcallback drawCallback = new XNADrawcallback(this, ref m); drawCallback.m_wireframe = false; concaveMesh.ProcessAllTriangles(drawCallback, ref worldBoundsMin, ref worldBoundsMax); } //glPopMatrix(); }
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; } } } }