public void AddChildShape(ref IndexedMatrix localTransform, CollisionShape shape)
{
m_updateRevision++;
//m_childTransforms.push_back(localTransform);
//m_childShapes.push_back(shape);
CompoundShapeChild child = new CompoundShapeChild();
child.m_transform = localTransform;
child.m_childShape = shape;
child.m_childShapeType = shape.GetShapeType();
child.m_childMargin = shape.GetMargin();
//extend the local aabbMin/aabbMax
IndexedVector3 localAabbMin;
IndexedVector3 localAabbMax;
shape.GetAabb(ref localTransform, out localAabbMin, out localAabbMax);
MathUtil.VectorMin(ref localAabbMin, ref m_localAabbMin);
MathUtil.VectorMax(ref localAabbMax, ref m_localAabbMax);
if (m_dynamicAabbTree != null)
{
DbvtAabbMm bounds = DbvtAabbMm.FromMM(ref localAabbMin, ref localAabbMax);
int index = m_children.Count;
child.m_treeNode = m_dynamicAabbTree.Insert(ref bounds, (object)index);
}
m_children.Add(child);
}
public virtual void SetCollisionShape(CollisionShape collisionShape)
{
m_collisionShape = collisionShape;
m_rootCollisionShape = collisionShape;
}
public BulletShapeXNA(CollisionShape xx, BSPhysicsShapeType typ)
: base()
{
shape = xx;
shapeType = typ;
}
public virtual void DebugDrawObject(ref IndexedMatrix worldTransform, CollisionShape shape, ref IndexedVector3 color)
{
// Draw a small simplex at the center of the object
GetDebugDrawer().DrawTransform(ref worldTransform, 1.0f);
switch (shape.GetShapeType())
{
case 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 temp = worldTransform * childTrans;
DebugDrawObject(ref temp, colShape, ref color);
}
break;
}
case (BroadphaseNativeTypes.BOX_SHAPE_PROXYTYPE):
{
BoxShape boxShape = shape as BoxShape;
IndexedVector3 halfExtents = boxShape.GetHalfExtentsWithMargin();
IndexedVector3 negHalfExtents = -halfExtents;
GetDebugDrawer().DrawBox(ref negHalfExtents, ref halfExtents, ref worldTransform, ref color);
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
GetDebugDrawer().DrawSphere(radius, ref worldTransform, ref color);
break;
}
case BroadphaseNativeTypes.MULTI_SPHERE_SHAPE_PROXYTYPE:
{
MultiSphereShape multiSphereShape = (MultiSphereShape)shape;
for (int i = multiSphereShape.GetSphereCount() - 1; i >= 0; i--)
{
IndexedMatrix childTransform = worldTransform;
childTransform._origin += multiSphereShape.GetSpherePosition(i);
GetDebugDrawer().DrawSphere(multiSphereShape.GetSphereRadius(i), ref childTransform, ref color);
}
break;
}
case BroadphaseNativeTypes.CAPSULE_SHAPE_PROXYTYPE:
{
CapsuleShape capsuleShape = shape as CapsuleShape;
float radius = capsuleShape.GetRadius();
float halfHeight = capsuleShape.GetHalfHeight();
int upAxis = capsuleShape.GetUpAxis();
GetDebugDrawer().DrawCapsule(radius, halfHeight, upAxis, ref worldTransform, ref color);
break;
}
case BroadphaseNativeTypes.CONE_SHAPE_PROXYTYPE:
{
ConeShape coneShape = (ConeShape)shape;
float radius = coneShape.GetRadius();//+coneShape->getMargin();
float height = coneShape.GetHeight();//+coneShape->getMargin();
int upAxis = coneShape.GetConeUpIndex();
GetDebugDrawer().DrawCone(radius, height, upAxis, ref worldTransform, ref color);
break;
}
case BroadphaseNativeTypes.CYLINDER_SHAPE_PROXYTYPE:
{
CylinderShape cylinder = (CylinderShape)shape;
int upAxis = cylinder.GetUpAxis();
float radius = cylinder.GetRadius();
float halfHeight = cylinder.GetHalfExtentsWithMargin()[upAxis];
GetDebugDrawer().DrawCylinder(radius, halfHeight, upAxis, ref worldTransform, ref color);
break;
}
case BroadphaseNativeTypes.STATIC_PLANE_PROXYTYPE:
{
StaticPlaneShape staticPlaneShape = shape as StaticPlaneShape;
float planeConst = staticPlaneShape.GetPlaneConstant();
IndexedVector3 planeNormal = staticPlaneShape.GetPlaneNormal();
GetDebugDrawer().DrawPlane(ref planeNormal, planeConst, ref worldTransform, ref color);
break;
}
default:
{
if (shape.IsPolyhedral())/// for polyhedral shapes
{
PolyhedralConvexShape polyshape = shape as PolyhedralConvexShape;
ConvexPolyhedron poly = polyshape.GetConvexPolyhedron();
if (poly != null)
{
for (int i = 0; i < poly.m_faces.Count; i++)
{
IndexedVector3 centroid = IndexedVector3.Zero;
int numVerts = poly.m_faces[i].m_indices.Count;
if (numVerts != 0)
{
int lastV = poly.m_faces[i].m_indices[numVerts - 1];
for (int v = 0; v < poly.m_faces[i].m_indices.Count; v++)
{
int curVert = poly.m_faces[i].m_indices[v];
centroid += poly.m_vertices[curVert];
GetDebugDrawer().DrawLine(worldTransform * poly.m_vertices[lastV], worldTransform * poly.m_vertices[curVert], color);
lastV = curVert;
}
}
centroid *= 1.0f / (float)(numVerts);
IndexedVector3 normalColor = new IndexedVector3(1, 1, 0);
IndexedVector3 faceNormal = new IndexedVector3(poly.m_faces[i].m_plane[0], poly.m_faces[i].m_plane[1], poly.m_faces[i].m_plane[2]);
GetDebugDrawer().DrawLine(worldTransform * centroid, worldTransform * (centroid + faceNormal), normalColor);
}
}
else
{
for (int i = 0; i < polyshape.GetNumEdges(); i++)
{
IndexedVector3 a, b;
polyshape.GetEdge(i, out a, out b);
IndexedVector3 wa = worldTransform * a;
IndexedVector3 wb = worldTransform * b;
GetDebugDrawer().DrawLine(ref wa, ref wb, ref color);
}
}
}
if (shape.IsConcave())
{
ConcaveShape concaveMesh = (ConcaveShape)shape;
///@todo pass camera, for some culling? no -> we are not a graphics lib
IndexedVector3 aabbMax = MathUtil.MAX_VECTOR;
IndexedVector3 aabbMin = MathUtil.MIN_VECTOR;
using (BulletXNA.DebugDrawcallback drawCallback = BulletGlobals.DebugDrawcallbackPool.Get())
{
drawCallback.Initialise(GetDebugDrawer(), ref worldTransform, ref color);
concaveMesh.ProcessAllTriangles(drawCallback, ref aabbMin, ref aabbMax);
}
}
else if (shape.GetShapeType() == BroadphaseNativeTypes.CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE)
{
ConvexTriangleMeshShape convexMesh = (ConvexTriangleMeshShape)shape;
//todo: pass camera for some culling
IndexedVector3 aabbMax = MathUtil.MAX_VECTOR;
IndexedVector3 aabbMin = MathUtil.MIN_VECTOR;
//DebugDrawcallback drawCallback;
//DebugDrawcallback drawCallback = new DebugDrawcallback(debugDraw, ref worldTransform, ref color);
//convexMesh.GetMeshInterface().InternalProcessAllTriangles(drawCallback, ref aabbMin, ref aabbMax);
//drawCallback.Cleanup();
}
break;
}
}
}
public void GImpactVsShape(CollisionObject body0,
CollisionObject body1,
GImpactShapeInterface shape0,
CollisionShape shape1, bool swapped)
{
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugGimpactAlgo)
{
BulletGlobals.g_streamWriter.WriteLine("GImpactAglo::GImpactVsShape");
}
if (shape0.GetGImpactShapeType() == GIMPACT_SHAPE_TYPE.CONST_GIMPACT_TRIMESH_SHAPE)
{
GImpactMeshShape meshshape0 = shape0 as GImpactMeshShape;
// check this...
//int& part = swapped ? m_part1 : m_part0;
//part = meshshape0.GetMeshPartCount();
int part = meshshape0.GetMeshPartCount();
while (part-- != 0)
{
GImpactVsShape(body0,
body1,
meshshape0.GetMeshPart(part),
shape1, swapped);
}
if (swapped)
{
m_part1 = part;
}
else
{
m_part0 = part;
}
return;
}
#if GIMPACT_VS_PLANE_COLLISION
if(shape0.GetGImpactShapeType() == GIMPACT_SHAPE_TYPE.CONST_GIMPACT_TRIMESH_SHAPE_PART &&
shape1.GetShapeType() == BroadphaseNativeTypes.STATIC_PLANE_PROXYTYPE)
{
GImpactMeshShapePart shapepart = shape0 as GImpactMeshShapePart;
StaticPlaneShape planeshape = shape1 as StaticPlaneShape;
GImpactTrimeshpartVsPlaneCollision(body0, body1, shapepart, planeshape, swapped);
return;
}
#endif
if (shape1.IsCompound())
{
CompoundShape compoundshape = shape1 as CompoundShape;
GImpactVsCompoundshape(body0, body1, shape0, compoundshape, swapped);
return;
}
else if (shape1.IsConcave())
{
ConcaveShape concaveshape = shape1 as ConcaveShape;
GImpactVsConcave(body0, body1, shape0, concaveshape, swapped);
return;
}
IndexedMatrix orgtrans0 = body0.GetWorldTransform();
IndexedMatrix orgtrans1 = body1.GetWorldTransform();
ObjectArray<int> collided_results = new ObjectArray<int>(64);
GImpactVsShapeFindPairs(ref orgtrans0, ref orgtrans1, shape0, shape1, collided_results);
if (collided_results.Count == 0) return;
shape0.LockChildShapes();
using (GIM_ShapeRetriever retriever0 = BulletGlobals.GIM_ShapeRetrieverPool.Get())
{
retriever0.Initialize(shape0);
bool child_has_transform0 = shape0.ChildrenHasTransform();
int i = collided_results.Count;
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugGimpactAlgo)
{
BulletGlobals.g_streamWriter.WriteLine("GImpactAglo::GImpactVsShape [{0}]", collided_results.Count);
}
while (i-- != 0)
{
int child_index = collided_results[i];
if (swapped)
m_triface1 = child_index;
else
m_triface0 = child_index;
CollisionShape colshape0 = retriever0.GetChildShape(child_index);
if (child_has_transform0)
{
body0.SetWorldTransform(orgtrans0 * shape0.GetChildTransform(child_index));
}
//collide two shapes
if (swapped)
{
ShapeVsShapeCollision(body1, body0, shape1, colshape0);
}
else
{
ShapeVsShapeCollision(body0, body1, colshape0, shape1);
}
//restore transforms
if (child_has_transform0)
{
body0.SetWorldTransform(ref orgtrans0);
}
}
shape0.UnlockChildShapes();
}
}
protected void ConvexVsConvexCollision(CollisionObject body0,
CollisionObject body1,
CollisionShape shape0,
CollisionShape shape1)
{
CollisionShape tmpShape0 = body0.GetCollisionShape();
CollisionShape tmpShape1 = body1.GetCollisionShape();
body0.InternalSetTemporaryCollisionShape(shape0);
body1.InternalSetTemporaryCollisionShape(shape1);
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugGimpactAlgo)
{
BulletGlobals.g_streamWriter.WriteLine("GImpactAglo::ConvexVsConvex");
}
m_resultOut.SetShapeIdentifiersA(m_part0, m_triface0);
m_resultOut.SetShapeIdentifiersB(m_part1, m_triface1);
CheckConvexAlgorithm(body0, body1);
m_convex_algorithm.ProcessCollision(body0, body1, m_dispatchInfo, m_resultOut);
body0.InternalSetTemporaryCollisionShape(tmpShape0);
body1.InternalSetTemporaryCollisionShape(tmpShape1);
}
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 RigidBody LocalCreateRigidBody(float mass, IndexedMatrix startTransform, CollisionShape shape,bool addToWorld)
{
return LocalCreateRigidBody(mass, ref startTransform, shape,addToWorld);
}
//----------------------------------------------------------------------------------------------
///Demo functions
public virtual void SetShootBoxShape()
{
if (m_shootBoxShape == null)
{
//#define TEST_UNIFORM_SCALING_SHAPE 1
#if TEST_UNIFORM_SCALING_SHAPE
ConvexShape childShape = new BoxShape(new IndexedVector3(1f,1f,1f));
m_shootBoxShape = new UniformScalingShape(childShape,0.5f);
#else
//m_shootBoxShape = new SphereShape(1f);//BoxShape(btVector3(1.f,1.f,1.f));
m_shootBoxShape = new BoxShape(new IndexedVector3(0.5f, 0.5f, 0.5f));
#endif//
}
}
//----------------------------------------------------------------------------------------------
public virtual void Cleanup()
{
//#ifndef BT_NO_PROFILE
// CProfileManager::Release_Iterator(m_profileIterator);
//#endif //BT_NO_PROFILE
int i = BulletGlobals.s_collisionAlgorithmInstanceCount;
m_shootBoxShape = null;
m_shapeDrawer = null;
}
//----------------------------------------------------------------------------------------------
public DemoApplication()
{
m_dynamicsWorld = null;
m_pickConstraint = null;
m_shootBoxShape = null;
m_cameraDistance = 30f;
m_pitch =(20f/360f)*MathUtil.SIMD_2_PI;
m_yaw = 0f;
m_cameraPosition = IndexedVector3.Zero;
m_cameraTargetPosition = IndexedVector3.Zero;
m_scaleBottom = 0.5f;
m_scaleFactor = 2f;
m_cameraUp = new IndexedVector3(0, 1, 0);
m_forwardAxis = 2;
m_glutScreenWidth = 0;
m_glutScreenHeight = 0;
m_ShootBoxInitialSpeed = 40f;
m_stepping = true;
m_singleStep = false;
m_idle = false;
m_enableshadows = true;
m_lightPosition = new IndexedVector3(5, 5, 5);
//m_lightDirection = IndexedVector3.Down;
m_lightDirection = new IndexedVector3(.5f, -.5f, .5f);
m_lightDirection.Normalize();
//#ifndef BT_NO_PROFILE
// m_profileIterator = CProfileManager::Get_Iterator();
//#endif //BT_NO_PROFILE
Content.RootDirectory = "Content";
m_graphics = new GraphicsDeviceManager(this);
m_graphics.PreferredBackBufferWidth = 800;
m_graphics.PreferredBackBufferHeight = 600;
SetSize(m_graphics.PreferredBackBufferWidth,m_graphics.PreferredBackBufferHeight);
m_nearClip = 1f;
m_farClip = 1000f;
m_aspect = m_glutScreenWidth / m_glutScreenHeight;
m_perspective = IndexedMatrix.CreatePerspectiveFieldOfView(MathHelper.ToRadians(40.0f), m_aspect, m_nearClip, m_farClip);
}
public void InitGImpactCollision()
{
/// Create Torus Shape
{
m_indexVertexArrays = new TriangleIndexVertexArray(DemoMeshes.TORUS_NUM_TRIANGLES, DemoMeshes.gTorusIndices, 3, DemoMeshes.TORUS_NUM_VERTICES, DemoMeshes.gTorusVertices, 3);
#if BULLET_GIMPACT_CONVEX_DECOMPOSITION
btGImpactConvexDecompositionShape * trimesh = new
btGImpactConvexDecompositionShape(
m_indexVertexArrays, IndexedVector3(1.f,1.f,1.f),btScalar(0.01));
trimesh->setMargin(0.07);
trimesh->updateBound();
#else
//GImpactMeshShape trimesh = new GImpactMeshShape(m_indexVertexArrays);
//IndexedVector3 scaling = IndexedVector3.One;
//trimesh.SetLocalScaling(ref scaling);
//trimesh.SetMargin(0.07f); ///?????
//trimesh.UpdateBound();
#endif
//m_trimeshShape = trimesh;
}
/// Create Bunny Shape
{
m_indexVertexArrays2 = new TriangleIndexVertexArray(DemoMeshes.BUNNY_NUM_TRIANGLES, DemoMeshes.gBunnyIndices, 3, DemoMeshes.BUNNY_NUM_VERTICES, DemoMeshes.gBunnyVertices, 3);
#if BULLET_GIMPACT_CONVEX_DECOMPOSITION
btGImpactConvexDecompositionShape * trimesh2 = new
btGImpactConvexDecompositionShape(
m_indexVertexArrays2, IndexedVector3(4.f,4.f,4.f),btScalar(0.01));
trimesh2->setMargin(0.07);
trimesh2->updateBound();
#else
GImpactMeshShape trimesh2 = new GImpactMeshShape(m_indexVertexArrays2);
IndexedVector3 scaling = new IndexedVector3(4.0f, 4.0f, 4.0f);
trimesh2.SetLocalScaling(ref scaling);
//trimesh2.SetMargin(0.07f); ///?????
trimesh2.UpdateBound();
#endif
m_trimeshShape2 = trimesh2;
}
///register GIMPACT algorithm
CollisionDispatcher dispatcher = m_dynamicsWorld.GetDispatcher() as CollisionDispatcher;
GImpactCollisionAlgorithm.RegisterAlgorithm(dispatcher);
}
public void ProcessChildShape(CollisionShape childShape, int index)
{
Debug.Assert(index >= 0);
CompoundShape compoundShape = (CompoundShape)(m_compoundColObj.GetCollisionShape());
Debug.Assert(index < compoundShape.GetNumChildShapes());
//backup
IndexedMatrix orgTrans = m_compoundColObj.GetWorldTransform();
IndexedMatrix orgInterpolationTrans = m_compoundColObj.GetInterpolationWorldTransform();
IndexedMatrix childTrans = compoundShape.GetChildTransform(index);
IndexedMatrix newChildWorldTrans = orgTrans * childTrans;
//perform an AABB check first
IndexedVector3 aabbMin0;
IndexedVector3 aabbMax0;
IndexedVector3 aabbMin1;
IndexedVector3 aabbMax1;
childShape.GetAabb(ref newChildWorldTrans, out aabbMin0, out aabbMax0);
m_otherObj.GetCollisionShape().GetAabb(m_otherObj.GetWorldTransform(), out aabbMin1, out aabbMax1);
if (AabbUtil2.TestAabbAgainstAabb2(ref aabbMin0, ref aabbMax0, ref aabbMin1, ref aabbMax1))
{
m_compoundColObj.SetWorldTransform(ref newChildWorldTrans);
m_compoundColObj.SetInterpolationWorldTransform(ref newChildWorldTrans);
//the contactpoint is still projected back using the original inverted worldtrans
CollisionShape tmpShape = m_compoundColObj.GetCollisionShape();
m_compoundColObj.InternalSetTemporaryCollisionShape(childShape);
if (m_childCollisionAlgorithms[index] == null)
{
m_childCollisionAlgorithms[index] = m_dispatcher.FindAlgorithm(m_compoundColObj, m_otherObj, m_sharedManifold);
if (m_childCollisionAlgorithms[index] == m_parent)
{
int ibreak = 0;
}
}
///detect swapping case
if (m_resultOut.GetBody0Internal() == m_compoundColObj)
{
m_resultOut.SetShapeIdentifiersA(-1, index);
}
else
{
m_resultOut.SetShapeIdentifiersB(-1, index);
}
m_childCollisionAlgorithms[index].ProcessCollision(m_compoundColObj, m_otherObj, m_dispatchInfo, m_resultOut);
if (m_dispatchInfo.getDebugDraw() != null && (((m_dispatchInfo.getDebugDraw().GetDebugMode() & DebugDrawModes.DBG_DrawAabb)) != 0))
{
IndexedVector3 worldAabbMin = IndexedVector3.Zero, worldAabbMax = IndexedVector3.Zero;
m_dispatchInfo.getDebugDraw().DrawAabb(aabbMin0, aabbMax0, new IndexedVector3(1, 1, 1));
m_dispatchInfo.getDebugDraw().DrawAabb(aabbMin1, aabbMax1, new IndexedVector3(1, 1, 1));
}
//revert back transform
m_compoundColObj.InternalSetTemporaryCollisionShape(tmpShape);
m_compoundColObj.SetWorldTransform(ref orgTrans);
m_compoundColObj.SetInterpolationWorldTransform(ref orgInterpolationTrans);
}
}
public override void ProcessCollision(CollisionObject body0, CollisionObject body1, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
{
//resultOut = null;
CollisionObject colObj = m_isSwapped ? body1 : body0;
CollisionObject otherObj = m_isSwapped ? body0 : body1;
Debug.Assert(colObj.GetCollisionShape().IsCompound());
CompoundShape compoundShape = (CompoundShape)(colObj.GetCollisionShape());
///btCompoundShape might have changed:
////make sure the internal child collision algorithm caches are still valid
if (compoundShape.GetUpdateRevision() != m_compoundShapeRevision)
{
///clear and update all
RemoveChildAlgorithms();
PreallocateChildAlgorithms(body0, body1);
}
Dbvt tree = compoundShape.GetDynamicAabbTree();
//use a dynamic aabb tree to cull potential child-overlaps
using (CompoundLeafCallback callback = BulletGlobals.CompoundLeafCallbackPool.Get())
{
callback.Initialize(colObj, otherObj, m_dispatcher, dispatchInfo, resultOut, this, m_childCollisionAlgorithms, m_sharedManifold);
///we need to refresh all contact manifolds
///note that we should actually recursively traverse all children, btCompoundShape can nested more then 1 level deep
///so we should add a 'refreshManifolds' in the btCollisionAlgorithm
{
m_manifoldArray.Clear();
for (int i = 0; i < m_childCollisionAlgorithms.Count; i++)
{
if (m_childCollisionAlgorithms[i] != null)
{
m_childCollisionAlgorithms[i].GetAllContactManifolds(m_manifoldArray);
for (int m = 0; m < m_manifoldArray.Count; m++)
{
if (m_manifoldArray[m].GetNumContacts() > 0)
{
resultOut.SetPersistentManifold(m_manifoldArray[m]);
resultOut.RefreshContactPoints();
resultOut.SetPersistentManifold(null);//??necessary?
}
}
m_manifoldArray.Clear();
}
}
}
if (tree != null)
{
IndexedVector3 localAabbMin;
IndexedVector3 localAabbMax;
IndexedMatrix otherInCompoundSpace;
//otherInCompoundSpace = MathUtil.BulletMatrixMultiply(colObj.GetWorldTransform(),otherObj.GetWorldTransform());
otherInCompoundSpace = colObj.GetWorldTransform().Inverse() * otherObj.GetWorldTransform();
otherObj.GetCollisionShape().GetAabb(ref otherInCompoundSpace, out localAabbMin, out localAabbMax);
DbvtAabbMm bounds = DbvtAabbMm.FromMM(ref localAabbMin, ref localAabbMax);
//process all children, that overlap with the given AABB bounds
Dbvt.CollideTV(tree.m_root, ref bounds, callback);
}
else
{
//iterate over all children, perform an AABB check inside ProcessChildShape
int numChildren = m_childCollisionAlgorithms.Count;
for (int i = 0; i < numChildren; i++)
{
callback.ProcessChildShape(compoundShape.GetChildShape(i), i);
}
}
{
//iterate over all children, perform an AABB check inside ProcessChildShape
int numChildren = m_childCollisionAlgorithms.Count;
m_manifoldArray.Clear();
CollisionShape childShape = null;
IndexedMatrix orgTrans;
IndexedMatrix orgInterpolationTrans;
IndexedMatrix newChildWorldTrans;
for (int i = 0; i < numChildren; i++)
{
if (m_childCollisionAlgorithms[i] != null)
{
childShape = compoundShape.GetChildShape(i);
//if not longer overlapping, remove the algorithm
orgTrans = colObj.GetWorldTransform();
orgInterpolationTrans = colObj.GetInterpolationWorldTransform();
IndexedMatrix childTrans = compoundShape.GetChildTransform(i);
newChildWorldTrans = orgTrans * childTrans;
//perform an AABB check first
IndexedVector3 aabbMin0;
IndexedVector3 aabbMax0;
IndexedVector3 aabbMin1;
IndexedVector3 aabbMax1;
childShape.GetAabb(ref newChildWorldTrans, out aabbMin0, out aabbMax0);
otherObj.GetCollisionShape().GetAabb(otherObj.GetWorldTransform(), out aabbMin1, out aabbMax1);
if (!AabbUtil2.TestAabbAgainstAabb2(ref aabbMin0, ref aabbMax0, ref aabbMin1, ref aabbMax1))
{
m_dispatcher.FreeCollisionAlgorithm(m_childCollisionAlgorithms[i]);
m_childCollisionAlgorithms[i] = null;
}
}
}
}
}
}
public RigidBody LocalCreateRigidBody(float mass, ref IndexedMatrix startTransform, CollisionShape shape)
{
return LocalCreateRigidBody(mass, ref startTransform, shape, true);
}
///Avoid using this internal API call
///internalSetTemporaryCollisionShape is used to temporary replace the actual collision shape by a child collision shape.
public void InternalSetTemporaryCollisionShape(CollisionShape collisionShape)
{
m_collisionShape = collisionShape;
}
//----------------------------------------------------------------------------------------------
public RigidBody LocalCreateRigidBodyMultiWorld(float mass, ref IndexedMatrix startTransform, CollisionShape shape, DiscreteDynamicsWorld world)
{
Debug.Assert((shape == null || shape.GetShapeType() != BroadphaseNativeTypes.INVALID_SHAPE_PROXYTYPE));
//rigidbody is dynamic if and only if mass is non zero, otherwise static
bool isDynamic = !MathUtil.CompareFloat(mass, 0f);
IndexedVector3 localInertia = IndexedVector3.Zero;
if (isDynamic)
{
shape.CalculateLocalInertia(mass, out localInertia);
}
//using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
//#define USE_MOTIONSTATE 1
//#ifdef USE_MOTIONSTATE
DefaultMotionState myMotionState = new DefaultMotionState(startTransform, IndexedMatrix.Identity);
RigidBodyConstructionInfo cInfo = new RigidBodyConstructionInfo(mass, myMotionState, shape, localInertia);
RigidBody body = new RigidBody(cInfo);
if (BulletGlobals.g_streamWriter != null && true)
{
BulletGlobals.g_streamWriter.WriteLine("localCreateRigidBody [{0}] startTransform", body.m_debugBodyId);
MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, startTransform);
BulletGlobals.g_streamWriter.WriteLine("");
}
world.AddRigidBody(body);
return body;
}
public void DrawXNA(IndexedMatrix m, CollisionShape shape, IndexedVector3 color, DebugDrawModes debugMode, IndexedVector3 worldBoundsMin, IndexedVector3 worldBoundsMax, IndexedMatrix view, IndexedMatrix projection)
{
DrawXNA(ref m, shape, ref color, debugMode, ref worldBoundsMin, ref worldBoundsMax, ref view, ref projection);
}
public RigidBodyConstructionInfo(float mass, IMotionState motionState, CollisionShape collisionShape): this(mass,motionState,collisionShape,new IndexedVector3(0))
{
}
protected void ShapeVsShapeCollision(
CollisionObject body0,
CollisionObject body1,
CollisionShape shape0,
CollisionShape shape1)
{
CollisionShape tmpShape0 = body0.GetCollisionShape();
CollisionShape tmpShape1 = body1.GetCollisionShape();
body0.InternalSetTemporaryCollisionShape(shape0);
body1.InternalSetTemporaryCollisionShape(shape1);
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugGimpactAlgo)
{
BulletGlobals.g_streamWriter.WriteLine("GImpactAglo::ShapeVsShape");
}
{
CollisionAlgorithm algor = NewAlgorithm(body0, body1);
// post : checkManifold is called
m_resultOut.SetShapeIdentifiersA(m_part0, m_triface0);
m_resultOut.SetShapeIdentifiersB(m_part1, m_triface1);
algor.ProcessCollision(body0, body1, m_dispatchInfo, m_resultOut);
m_dispatcher.FreeCollisionAlgorithm(algor);
}
body0.InternalSetTemporaryCollisionShape(tmpShape0);
body1.InternalSetTemporaryCollisionShape(tmpShape1);
}
public RigidBodyConstructionInfo(float mass, IMotionState motionState, CollisionShape collisionShape, IndexedVector3 localInertia)
{
m_mass = mass;
m_motionState =motionState;
m_collisionShape = collisionShape;
m_localInertia = localInertia;
m_linearDamping = 0f;
m_angularDamping = 0f;
m_friction = 0.5f;
m_restitution = 0f;
m_linearSleepingThreshold = 0.8f;
m_angularSleepingThreshold = 1f;
m_additionalDamping = false;
m_additionalDampingFactor = 0.005f;
m_additionalLinearDampingThresholdSqr = 0.01f;
m_additionalAngularDampingThresholdSqr = 0.01f;
m_additionalAngularDampingFactor = 0.01f;
m_startWorldTransform = IndexedMatrix.Identity;
}
protected void GImpactVsShapeFindPairs(
ref IndexedMatrix trans0,
ref IndexedMatrix trans1,
GImpactShapeInterface shape0,
CollisionShape shape1,
ObjectArray<int> collided_primitives)
{
AABB boxshape = new AABB();
if (shape0.HasBoxSet())
{
IndexedMatrix trans1to0 = trans0.Inverse();
//trans1to0 *= trans1;
trans1to0 = trans1to0 * trans1;
//trans1to0 = MathUtil.BulletMatrixMultiply(trans1,trans1to0);
shape1.GetAabb(ref trans1to0, out boxshape.m_min, out boxshape.m_max);
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugGimpactAlgo)
{
MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "GImpactAglo::GImpactVsShapeFindPairs trans1to0", trans1to0);
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "box min", boxshape.m_min);
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "box max", boxshape.m_max);
}
shape0.GetBoxSet().BoxQuery(ref boxshape, collided_primitives);
}
else
{
shape1.GetAabb(ref trans1, out boxshape.m_min, out boxshape.m_max);
AABB boxshape0 = new AABB();
int i = shape0.GetNumChildShapes();
while (i-- != 0)
{
shape0.GetChildAabb(i, ref trans0, out boxshape0.m_min, out boxshape0.m_max);
if (boxshape.HasCollision(ref boxshape0))
{
collided_primitives.Add(i);
}
}
}
}
///btRigidBody constructor for backwards compatibility.
///To specify friction (etc) during rigid body construction, please use the other constructor (using btRigidBodyConstructionInfo)
public RigidBody(float mass, IMotionState motionState, CollisionShape collisionShape, IndexedVector3 localInertia)
{
RigidBodyConstructionInfo cinfo = new RigidBodyConstructionInfo(mass,motionState,collisionShape,localInertia);
SetupRigidBody(cinfo);
}
/// objectQuerySingle performs a collision detection query and calls the resultCallback. It is used internally by rayTest.
public static void ObjectQuerySingle(ConvexShape castShape, ref IndexedMatrix convexFromTrans, ref IndexedMatrix convexToTrans,
CollisionObject collisionObject, CollisionShape collisionShape,
ref IndexedMatrix colObjWorldTransform,
ConvexResultCallback resultCallback, float allowedPenetration)
{
if (collisionShape.IsConvex())
{
BulletGlobals.StartProfile("convexSweepConvex");
CastResult castResult = BulletGlobals.CastResultPool.Get();
castResult.m_allowedPenetration = allowedPenetration;
castResult.m_fraction = resultCallback.m_closestHitFraction;//float(1.);//??
ConvexShape convexShape = collisionShape as ConvexShape;
VoronoiSimplexSolver simplexSolver = BulletGlobals.VoronoiSimplexSolverPool.Get();
GjkEpaPenetrationDepthSolver gjkEpaPenetrationSolver = BulletGlobals.GjkEpaPenetrationDepthSolverPool.Get();
ContinuousConvexCollision convexCaster1 = BulletGlobals.ContinuousConvexCollisionPool.Get();
convexCaster1.Initialize(castShape, convexShape, simplexSolver, gjkEpaPenetrationSolver);
//btGjkConvexCast convexCaster2(castShape,convexShape,&simplexSolver);
//btSubsimplexConvexCast convexCaster3(castShape,convexShape,&simplexSolver);
IConvexCast castPtr = convexCaster1;
if (castPtr.CalcTimeOfImpact(ref convexFromTrans, ref convexToTrans, ref colObjWorldTransform, ref colObjWorldTransform, castResult))
{
//add hit
if (castResult.m_normal.LengthSquared() > 0.0001f)
{
if (castResult.m_fraction < resultCallback.m_closestHitFraction)
{
castResult.m_normal.Normalize();
LocalConvexResult localConvexResult = new LocalConvexResult
(
collisionObject,
//null, // updated to allow different ctor on struct
ref castResult.m_normal,
ref castResult.m_hitPoint,
castResult.m_fraction
);
bool normalInWorldSpace = true;
resultCallback.AddSingleResult(ref localConvexResult, normalInWorldSpace);
}
}
}
BulletGlobals.ContinuousConvexCollisionPool.Free(convexCaster1);
BulletGlobals.GjkEpaPenetrationDepthSolverPool.Free(gjkEpaPenetrationSolver);
BulletGlobals.VoronoiSimplexSolverPool.Free(simplexSolver);
castResult.Cleanup();
BulletGlobals.StopProfile();
}
else
{
if (collisionShape.IsConcave())
{
if (collisionShape.GetShapeType() == BroadphaseNativeTypes.TRIANGLE_MESH_SHAPE_PROXYTYPE)
{
BulletGlobals.StartProfile("convexSweepbtBvhTriangleMesh");
BvhTriangleMeshShape triangleMesh = (BvhTriangleMeshShape)collisionShape;
IndexedMatrix worldTocollisionObject = colObjWorldTransform.Inverse();
IndexedVector3 convexFromLocal = worldTocollisionObject * convexFromTrans._origin;
IndexedVector3 convexToLocal = worldTocollisionObject * convexToTrans._origin;
// rotation of box in local mesh space = MeshRotation^-1 * ConvexToRotation
IndexedMatrix rotationXform = new IndexedMatrix(worldTocollisionObject._basis * convexToTrans._basis,new IndexedVector3(0));
using (BridgeTriangleConvexcastCallback tccb = BulletGlobals.BridgeTriangleConvexcastCallbackPool.Get())
{
tccb.Initialize(castShape, ref convexFromTrans, ref convexToTrans, resultCallback, collisionObject, triangleMesh, ref colObjWorldTransform);
tccb.m_hitFraction = resultCallback.m_closestHitFraction;
tccb.m_allowedPenetration = allowedPenetration;
IndexedVector3 boxMinLocal;
IndexedVector3 boxMaxLocal;
castShape.GetAabb(ref rotationXform, out boxMinLocal, out boxMaxLocal);
triangleMesh.PerformConvexCast(tccb, ref convexFromLocal, ref convexToLocal, ref boxMinLocal, ref boxMaxLocal);
}
BulletGlobals.StopProfile();
}
else
{
if (collisionShape.GetShapeType() == BroadphaseNativeTypes.STATIC_PLANE_PROXYTYPE)
{
CastResult castResult = BulletGlobals.CastResultPool.Get();
castResult.m_allowedPenetration = allowedPenetration;
castResult.m_fraction = resultCallback.m_closestHitFraction;
StaticPlaneShape planeShape = collisionShape as StaticPlaneShape;
ContinuousConvexCollision convexCaster1 = new ContinuousConvexCollision(castShape, planeShape);
if (convexCaster1.CalcTimeOfImpact(ref convexFromTrans, ref convexToTrans, ref colObjWorldTransform, ref colObjWorldTransform, castResult))
{
//add hit
if (castResult.m_normal.LengthSquared() > 0.0001f)
{
if (castResult.m_fraction < resultCallback.m_closestHitFraction)
{
castResult.m_normal.Normalize();
LocalConvexResult localConvexResult = new LocalConvexResult
(
collisionObject,
//null, // updated to allow different ctor on struct
ref castResult.m_normal,
ref castResult.m_hitPoint,
castResult.m_fraction
);
bool normalInWorldSpace = true;
resultCallback.AddSingleResult(ref localConvexResult, normalInWorldSpace);
}
}
}
castResult.Cleanup();
}
else
{
BulletGlobals.StartProfile("convexSweepConcave");
ConcaveShape concaveShape = (ConcaveShape)collisionShape;
IndexedMatrix worldTocollisionObject = colObjWorldTransform.Inverse();
IndexedVector3 convexFromLocal = worldTocollisionObject * convexFromTrans._origin;
IndexedVector3 convexToLocal = worldTocollisionObject * convexToTrans._origin;
// rotation of box in local mesh space = MeshRotation^-1 * ConvexToRotation
IndexedMatrix rotationXform = new IndexedMatrix(worldTocollisionObject._basis * convexToTrans._basis, new IndexedVector3(0));
using (BridgeTriangleConvexcastCallback tccb = BulletGlobals.BridgeTriangleConvexcastCallbackPool.Get())
{
tccb.Initialize(castShape, ref convexFromTrans, ref convexToTrans, resultCallback, collisionObject, concaveShape, ref colObjWorldTransform);
tccb.m_hitFraction = resultCallback.m_closestHitFraction;
tccb.m_allowedPenetration = allowedPenetration;
IndexedVector3 boxMinLocal;
IndexedVector3 boxMaxLocal;
castShape.GetAabb(ref rotationXform, out boxMinLocal, out boxMaxLocal);
IndexedVector3 rayAabbMinLocal = convexFromLocal;
MathUtil.VectorMin(ref convexToLocal, ref rayAabbMinLocal);
//rayAabbMinLocal.setMin(convexToLocal);
IndexedVector3 rayAabbMaxLocal = convexFromLocal;
//rayAabbMaxLocal.setMax(convexToLocal);
MathUtil.VectorMax(ref convexToLocal, ref rayAabbMaxLocal);
rayAabbMinLocal += boxMinLocal;
rayAabbMaxLocal += boxMaxLocal;
concaveShape.ProcessAllTriangles(tccb, ref rayAabbMinLocal, ref rayAabbMaxLocal);
BulletGlobals.StopProfile();
}
}
}
}
else
{
///@todo : use AABB tree or other BVH acceleration structure!
if (collisionShape.IsCompound())
{
BulletGlobals.StartProfile("convexSweepCompound");
CompoundShape compoundShape = (CompoundShape)collisionShape;
for (int i = 0; i < compoundShape.GetNumChildShapes(); i++)
{
IndexedMatrix childTrans = compoundShape.GetChildTransform(i);
CollisionShape childCollisionShape = compoundShape.GetChildShape(i);
IndexedMatrix childWorldTrans = colObjWorldTransform * childTrans;
// replace collision shape so that callback can determine the triangle
CollisionShape saveCollisionShape = collisionObject.GetCollisionShape();
collisionObject.InternalSetTemporaryCollisionShape(childCollisionShape);
LocalInfoAdder my_cb = new LocalInfoAdder(i, resultCallback);
my_cb.m_closestHitFraction = resultCallback.m_closestHitFraction;
ObjectQuerySingle(castShape, ref convexFromTrans, ref convexToTrans,
collisionObject,
childCollisionShape,
ref childWorldTrans,
my_cb, allowedPenetration);
// restore
collisionObject.InternalSetTemporaryCollisionShape(saveCollisionShape);
}
BulletGlobals.StopProfile();
}
}
}
}
public void ProcessChildShape(CollisionShape childShape, int index)
{
Debug.Assert(index >= 0);
CompoundShape compoundShape = (CompoundShape)(m_compoundColObj.GetCollisionShape());
Debug.Assert(index < compoundShape.GetNumChildShapes());
//backup
IndexedMatrix orgTrans = m_compoundColObj.GetWorldTransform();
IndexedMatrix orgInterpolationTrans = m_compoundColObj.GetInterpolationWorldTransform();
IndexedMatrix childTrans = compoundShape.GetChildTransform(index);
IndexedMatrix newChildWorldTrans = orgTrans * childTrans;
//perform an AABB check first
IndexedVector3 aabbMin0;
IndexedVector3 aabbMax0;
IndexedVector3 aabbMin1;
IndexedVector3 aabbMax1;
childShape.GetAabb(ref newChildWorldTrans, out aabbMin0, out aabbMax0);
m_otherObj.GetCollisionShape().GetAabb(m_otherObj.GetWorldTransform(), out aabbMin1, out aabbMax1);
if (AabbUtil2.TestAabbAgainstAabb2(ref aabbMin0, ref aabbMax0, ref aabbMin1, ref aabbMax1))
{
m_compoundColObj.SetWorldTransform(ref newChildWorldTrans);
m_compoundColObj.SetInterpolationWorldTransform(ref newChildWorldTrans);
//the contactpoint is still projected back using the original inverted worldtrans
CollisionShape tmpShape = m_compoundColObj.GetCollisionShape();
m_compoundColObj.InternalSetTemporaryCollisionShape(childShape);
if (m_childCollisionAlgorithms[index] == null)
{
m_childCollisionAlgorithms[index] = m_dispatcher.FindAlgorithm(m_compoundColObj, m_otherObj, m_sharedManifold);
if (m_childCollisionAlgorithms[index] == m_parent)
{
int ibreak = 0;
}
}
///detect swapping case
if (m_resultOut.GetBody0Internal() == m_compoundColObj)
{
m_resultOut.SetShapeIdentifiersA(-1, index);
}
else
{
m_resultOut.SetShapeIdentifiersB(-1, index);
}
m_childCollisionAlgorithms[index].ProcessCollision(m_compoundColObj, m_otherObj, m_dispatchInfo, m_resultOut);
if (m_dispatchInfo.getDebugDraw() != null && (((m_dispatchInfo.getDebugDraw().GetDebugMode() & DebugDrawModes.DBG_DrawAabb)) != 0))
{
IndexedVector3 worldAabbMin = IndexedVector3.Zero, worldAabbMax = IndexedVector3.Zero;
m_dispatchInfo.getDebugDraw().DrawAabb(aabbMin0, aabbMax0, new IndexedVector3(1, 1, 1));
m_dispatchInfo.getDebugDraw().DrawAabb(aabbMin1, aabbMax1, new IndexedVector3(1, 1, 1));
}
//revert back transform
m_compoundColObj.InternalSetTemporaryCollisionShape(tmpShape);
m_compoundColObj.SetWorldTransform(ref orgTrans);
m_compoundColObj.SetInterpolationWorldTransform(ref orgInterpolationTrans);
}
}
public static void RayTestSingle(ref IndexedMatrix rayFromTrans, ref IndexedMatrix rayToTrans,
CollisionObject collisionObject,
CollisionShape collisionShape,
ref IndexedMatrix colObjWorldTransform,
RayResultCallback resultCallback)
{
SphereShape pointShape = BulletGlobals.SphereShapePool.Get();
pointShape.Initialize(0.0f);
pointShape.SetMargin(0f);
ConvexShape castShape = pointShape;
if (collisionShape.IsConvex())
{
BulletGlobals.StartProfile("rayTestConvex");
CastResult castResult = BulletGlobals.CastResultPool.Get();
castResult.m_fraction = resultCallback.m_closestHitFraction;
ConvexShape convexShape = collisionShape as ConvexShape;
VoronoiSimplexSolver simplexSolver = BulletGlobals.VoronoiSimplexSolverPool.Get();
//#define USE_SUBSIMPLEX_CONVEX_CAST 1
//#ifdef USE_SUBSIMPLEX_CONVEX_CAST
// FIXME - MAN - convexcat here seems to make big difference to forklift.
SubSimplexConvexCast convexCaster = BulletGlobals.SubSimplexConvexCastPool.Get();
convexCaster.Initialize(castShape, convexShape, simplexSolver);
//GjkConvexCast convexCaster = new GjkConvexCast(castShape, convexShape, simplexSolver);
//#else
//btGjkConvexCast convexCaster(castShape,convexShape,&simplexSolver);
//btContinuousConvexCollision convexCaster(castShape,convexShape,&simplexSolver,0);
//#endif //#USE_SUBSIMPLEX_CONVEX_CAST
if (convexCaster.CalcTimeOfImpact(ref rayFromTrans, ref rayToTrans, ref colObjWorldTransform, ref colObjWorldTransform, castResult))
{
//add hit
if (castResult.m_normal.LengthSquared() > 0.0001f)
{
if (castResult.m_fraction < resultCallback.m_closestHitFraction)
{
//if (resultCallback.m_closestHitFraction != 1f)
//{
// int ibreak = 0;
// convexCaster.calcTimeOfImpact(ref rayFromTrans, ref rayToTrans, ref colObjWorldTransform, ref colObjWorldTransform, castResult);
//}
//#ifdef USE_SUBSIMPLEX_CONVEX_CAST
//rotate normal into worldspace
castResult.m_normal = rayFromTrans._basis * castResult.m_normal;
//#endif //USE_SUBSIMPLEX_CONVEX_CAST
castResult.m_normal.Normalize();
LocalRayResult localRayResult = new LocalRayResult(
collisionObject,
//null, // updated to allow different ctor on struct
ref castResult.m_normal,
castResult.m_fraction
);
bool normalInWorldSpace = true;
resultCallback.AddSingleResult(ref localRayResult, normalInWorldSpace);
}
}
}
castResult.Cleanup();
BulletGlobals.SubSimplexConvexCastPool.Free(convexCaster);
BulletGlobals.VoronoiSimplexSolverPool.Free(simplexSolver);
BulletGlobals.StopProfile();
}
else
{
if (collisionShape.IsConcave())
{
BulletGlobals.StartProfile("rayTestConcave");
if (collisionShape.GetShapeType() == BroadphaseNativeTypes.TRIANGLE_MESH_SHAPE_PROXYTYPE && collisionShape is BvhTriangleMeshShape)
{
///optimized version for btBvhTriangleMeshShape
BvhTriangleMeshShape triangleMesh = (BvhTriangleMeshShape)collisionShape;
IndexedMatrix worldTocollisionObject = colObjWorldTransform.Inverse();
IndexedVector3 rayFromLocal = worldTocollisionObject * rayFromTrans._origin;
IndexedVector3 rayToLocal = worldTocollisionObject * rayToTrans._origin;
IndexedMatrix transform = IndexedMatrix.Identity;
using (BridgeTriangleRaycastCallback rcb = BulletGlobals.BridgeTriangleRaycastCallbackPool.Get())
{
rcb.Initialize(ref rayFromLocal, ref rayToLocal, resultCallback, collisionObject, triangleMesh, ref transform);
rcb.m_hitFraction = resultCallback.m_closestHitFraction;
triangleMesh.PerformRaycast(rcb, ref rayFromLocal, ref rayToLocal);
}
}
else if (collisionShape.GetShapeType() == BroadphaseNativeTypes.TERRAIN_SHAPE_PROXYTYPE && collisionShape is HeightfieldTerrainShape)
{
///optimized version for btBvhTriangleMeshShape
HeightfieldTerrainShape heightField = (HeightfieldTerrainShape)collisionShape;
IndexedMatrix worldTocollisionObject = colObjWorldTransform.Inverse();
IndexedVector3 rayFromLocal = worldTocollisionObject * rayFromTrans._origin;
IndexedVector3 rayToLocal = worldTocollisionObject * rayToTrans._origin;
IndexedMatrix transform = IndexedMatrix.Identity;
using (BridgeTriangleConcaveRaycastCallback rcb = BulletGlobals.BridgeTriangleConcaveRaycastCallbackPool.Get())
{
rcb.Initialize(ref rayFromLocal, ref rayToLocal, resultCallback, collisionObject, heightField, ref transform);
rcb.m_hitFraction = resultCallback.m_closestHitFraction;
heightField.PerformRaycast(rcb, ref rayFromLocal, ref rayToLocal);
}
}
else
{
//generic (slower) case
ConcaveShape concaveShape = (ConcaveShape)collisionShape;
IndexedMatrix worldTocollisionObject = colObjWorldTransform.Inverse();
IndexedVector3 rayFromLocal = worldTocollisionObject * rayFromTrans._origin;
IndexedVector3 rayToLocal = worldTocollisionObject * rayToTrans._origin;
//ConvexCast::CastResult
IndexedMatrix transform = IndexedMatrix.Identity;
using (BridgeTriangleConcaveRaycastCallback rcb = BulletGlobals.BridgeTriangleConcaveRaycastCallbackPool.Get())
{
rcb.Initialize(ref rayFromLocal, ref rayToLocal, resultCallback, collisionObject, concaveShape, ref transform);
rcb.m_hitFraction = resultCallback.m_closestHitFraction;
IndexedVector3 rayAabbMinLocal = rayFromLocal;
MathUtil.VectorMin(ref rayToLocal, ref rayAabbMinLocal);
IndexedVector3 rayAabbMaxLocal = rayFromLocal;
MathUtil.VectorMax(ref rayToLocal, ref rayAabbMaxLocal);
concaveShape.ProcessAllTriangles(rcb, ref rayAabbMinLocal, ref rayAabbMaxLocal);
}
}
BulletGlobals.StopProfile();
}
else
{
BulletGlobals.StartProfile("rayTestCompound");
///@todo: use AABB tree or other BVH acceleration structure, see btDbvt
if (collisionShape.IsCompound())
{
CompoundShape compoundShape = collisionShape as CompoundShape;
Dbvt dbvt = compoundShape.GetDynamicAabbTree();
RayTester rayCB = new RayTester(
collisionObject,
compoundShape,
ref colObjWorldTransform,
ref rayFromTrans,
ref rayToTrans,
resultCallback);
#if !DISABLE_DBVT_COMPOUNDSHAPE_RAYCAST_ACCELERATION
if (dbvt != null)
{
IndexedVector3 localRayFrom = colObjWorldTransform.InverseTimes(ref rayFromTrans)._origin;
IndexedVector3 localRayTo = colObjWorldTransform.InverseTimes(ref rayToTrans)._origin;
Dbvt.RayTest(dbvt.m_root, ref localRayFrom, ref localRayTo, rayCB);
}
else
#endif //DISABLE_DBVT_COMPOUNDSHAPE_RAYCAST_ACCELERATION
{
for (int i = 0, n = compoundShape.GetNumChildShapes(); i < n; ++i)
{
rayCB.Process(i);
}
}
rayCB.Cleanup();
BulletGlobals.StopProfile();
}
}
}
BulletGlobals.SphereShapePool.Free(pointShape);
}
//! Use this method for adding children. Only Convex shapes are allowed.
public void AddChildShape(ref IndexedMatrix localTransform, CollisionShape shape)
{
Debug.Assert(shape.IsConvex());
m_childTransforms.Add(localTransform);
m_childShapes.Add(shape);
}
public override void Clear()
{
shape = null;
}
//! Use this method for adding children. Only Convex shapes are allowed.
public void AddChildShape(CollisionShape shape)
{
Debug.Assert(shape.IsConvex());
m_childShapes.Add(shape);
}
///Avoid using this internal API call
///internalSetTemporaryCollisionShape is used to temporary replace the actual collision shape by a child collision shape.
public void InternalSetTemporaryCollisionShape(CollisionShape collisionShape)
{
m_collisionShape = collisionShape;
}
//public void startDraw(GraphicsDevice graphicsDevice,ref IndexedMatrix view, ref IndexedMatrix projection)
//{
// ((DefaultDebugDraw)m_debugDraw).update(graphicsDevice, ref view, ref projection);
//}
public virtual void DrawShadow(IndexedMatrix m, IndexedVector3 extrusion, CollisionShape shape, IndexedVector3 worldBoundsMin, IndexedVector3 worldBoundsMax)
{
DrawShadow(m, extrusion, shape, worldBoundsMin, worldBoundsMax);
}
/// Remove all children shapes that contain the specified shape
public virtual void RemoveChildShape(CollisionShape shape)
{
m_updateRevision++;
// Find the children containing the shape specified, and remove those children.
//note: there might be multiple children using the same shape!
for (int i = m_children.Count - 1; i >= 0; i--)
{
if (m_children[i].m_childShape == shape)
{
RemoveChildShapeByIndex(i);
}
}
RecalculateLocalAabb();
}
public virtual void SetCollisionShape(CollisionShape collisionShape)
{
m_collisionShape = collisionShape;
m_rootCollisionShape = collisionShape;
}
