// MyContactCallback is just an example to show how to get access to the child shape that collided
void MyContactCallback(ManifoldPoint cp, CollisionObjectWrapper colObj0Wrap, int partId0, int index0, CollisionObjectWrapper colObj1Wrap, int partId1, int index1)
{
if (colObj0Wrap.CollisionObject.CollisionShape.ShapeType == BroadphaseNativeType.CompoundShape)
{
CompoundShape compound = colObj0Wrap.CollisionObject.CollisionShape as CompoundShape;
CollisionShape childShape = compound.GetChildShape(index0);
}
if (colObj1Wrap.CollisionObject.CollisionShape.ShapeType == BroadphaseNativeType.CompoundShape)
{
CompoundShape compound = colObj1Wrap.CollisionObject.CollisionShape as CompoundShape;
CollisionShape childShape = compound.GetChildShape(index1);
}
}
// MyContactCallback is just an example to show how to get access to the child shape that collided
bool MyContactCallback(ManifoldPoint cp, CollisionObject colObj0, int partId0, int index0, CollisionObject colObj1, int partId1, int index1)
{
if (colObj0.CollisionShape.ShapeType == BroadphaseNativeType.CompoundShape)
{
CompoundShape compound = colObj0.CollisionShape as CompoundShape;
CollisionShape childShape = compound.GetChildShape(index0);
}
if (colObj1.CollisionShape.ShapeType == BroadphaseNativeType.CompoundShape)
{
CompoundShape compound = colObj1.CollisionShape as CompoundShape;
CollisionShape childShape = compound.GetChildShape(index1);
}
return(true);
}
protected override void ProcessObject(RigidBody obj, int slice)
{
if (obj.CollisionShape.IsCompound && FWorld.PluginIO.IsConnected)
{
CompoundShape comp = (CompoundShape)obj.CollisionShape;
BodyCustomData bc = (BodyCustomData)obj.UserObject;
bc.MarkedForDeletion = true;
for (int i = 0; i < comp.ChildList.Count; i++)
{
CollisionShape shape = comp.GetChildShape(i);
float mass = 1.0f / obj.InvMass;
float massshape = mass / (float)comp.ChildList.Count;
Vector3 inert;
shape.CalculateLocalInertia(massshape, out inert);
Matrix m = obj.MotionState.WorldTransform;
MotionState ms = new DefaultMotionState(m);
//List<RigidBody> bodies = new List<RigidBody>();
RigidBody rb = new RigidBody(new RigidBodyConstructionInfo(mass, ms, shape, inert));
rb.LinearVelocity = obj.LinearVelocity;
rb.AngularVelocity = obj.AngularVelocity;
rb.Restitution = obj.Restitution;
rb.Friction = obj.Friction;
//rb.CollisionShape = shape;
BodyCustomData copy = new BodyCustomData();
copy.Id = this.FWorld[0].GetNewBodyId();
copy.Custom = bc.Custom;
rb.UserObject = copy;
this.FWorld[0].Register(rb);
//bodies.Add(rb);
}
}
}
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;
}
}
}
}