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