protected void GImpactTrimeshpartVsPlaneCollision(
CollisionObject body0,
CollisionObject body1,
GImpactMeshShapePart shape0,
StaticPlaneShape shape1, bool swapped)
{
IndexedMatrix orgtrans0 = body0.GetWorldTransform();
IndexedMatrix orgtrans1 = body1.GetWorldTransform();
IndexedVector4 plane;
PlaneShape.GetPlaneEquationTransformed(shape1, ref orgtrans1, out plane);
//test box against plane
AABB tribox = new AABB();
shape0.GetAabb(ref orgtrans0, out tribox.m_min, out tribox.m_max);
tribox.IncrementMargin(shape1.GetMargin());
if (tribox.PlaneClassify(ref plane) != BT_PLANE_INTERSECTION_TYPE.BT_CONST_COLLIDE_PLANE)
{
return;
}
shape0.LockChildShapes();
float margin = shape0.GetMargin() + shape1.GetMargin();
IndexedVector3 vertex;
int vi = shape0.GetVertexCount();
while (vi-- != 0)
{
shape0.GetVertex(vi, out vertex);
vertex = orgtrans0 * vertex;
float distance = IndexedVector3.Dot(vertex, MathUtil.Vector4ToVector3(ref plane)) - plane.W - margin;
if (distance < 0.0f)//add contact
{
if (swapped)
{
AddContactPoint(body1, body0,
vertex,
MathUtil.Vector4ToVector3(-plane),
distance);
}
else
{
AddContactPoint(body0, body1,
vertex,
MathUtil.Vector4ToVector3(ref plane),
distance);
}
}
}
shape0.UnlockChildShapes();
}
//! Collision routines
//!@{
protected void CollideGjkTriangles(CollisionObject body0,
CollisionObject body1,
GImpactMeshShapePart shape0,
GImpactMeshShapePart shape1,
ObjectArray <int> pairs, int pair_count)
{
TriangleShapeEx tri0 = new TriangleShapeEx();
TriangleShapeEx tri1 = new TriangleShapeEx();
shape0.LockChildShapes();
shape1.LockChildShapes();
int pair_pointer = 0;
#if DEBUG
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugGimpactAlgo)
{
BulletGlobals.g_streamWriter.WriteLine("GImpactAglo::CollideGjkTriangles [{0}]", pair_count);
}
#endif
while (pair_count-- != 0)
{
m_triface0 = pairs[pair_pointer];
m_triface1 = pairs[pair_pointer + 1];
pair_pointer += 2;
shape0.GetBulletTriangle(m_triface0, tri0);
shape1.GetBulletTriangle(m_triface1, tri1);
//collide two convex shapes
if (tri0.OverlapTestConservative(tri1))
{
ConvexVsConvexCollision(body0, body1, tri0, tri1);
}
}
shape0.UnlockChildShapes();
shape1.UnlockChildShapes();
}
protected void GImpactTrimeshpartVsPlaneCollision(
CollisionObject body0,
CollisionObject body1,
GImpactMeshShapePart shape0,
StaticPlaneShape shape1, bool swapped)
{
IndexedMatrix orgtrans0 = body0.GetWorldTransform();
IndexedMatrix orgtrans1 = body1.GetWorldTransform();
IndexedVector4 plane;
PlaneShape.GetPlaneEquationTransformed(shape1,ref orgtrans1, out plane);
//test box against plane
AABB tribox = new AABB();
shape0.GetAabb(ref orgtrans0, out tribox.m_min, out tribox.m_max);
tribox.IncrementMargin(shape1.GetMargin());
if (tribox.PlaneClassify(ref plane) != BT_PLANE_INTERSECTION_TYPE.BT_CONST_COLLIDE_PLANE) return;
shape0.LockChildShapes();
float margin = shape0.GetMargin() + shape1.GetMargin();
IndexedVector3 vertex;
int vi = shape0.GetVertexCount();
while (vi-- != 0)
{
shape0.GetVertex(vi, out vertex);
vertex = orgtrans0 * vertex;
float distance = IndexedVector3.Dot(vertex, MathUtil.Vector4ToVector3(ref plane)) - plane.W - margin;
if (distance < 0.0f)//add contact
{
if (swapped)
{
AddContactPoint(body1, body0,
vertex,
MathUtil.Vector4ToVector3(-plane),
distance);
}
else
{
AddContactPoint(body0, body1,
vertex,
MathUtil.Vector4ToVector3(ref plane),
distance);
}
}
}
shape0.UnlockChildShapes();
}
protected void CollideSatTriangles(CollisionObject body0,
CollisionObject body1,
GImpactMeshShapePart shape0,
GImpactMeshShapePart shape1,
PairSet pairs, int pair_count)
{
IndexedMatrix orgtrans0 = body0.GetWorldTransform();
IndexedMatrix orgtrans1 = body1.GetWorldTransform();
PrimitiveTriangle ptri0 = new PrimitiveTriangle();
PrimitiveTriangle ptri1 = new PrimitiveTriangle();
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugGimpactAlgo)
{
BulletGlobals.g_streamWriter.WriteLine("GImpactAglo::CollideSatTriangles [{0}]", pair_count);
}
shape0.LockChildShapes();
shape1.LockChildShapes();
int pair_pointer = 0;
while (pair_count-- != 0)
{
m_triface0 = pairs[pair_pointer].m_index1;
m_triface1 = pairs[pair_pointer].m_index2;
pair_pointer += 1;
shape0.GetPrimitiveTriangle(m_triface0, ptri0);
shape1.GetPrimitiveTriangle(m_triface1, ptri1);
#if TRI_COLLISION_PROFILING
BulletGlobal.StartProfile("gim02_tri_time");
#endif
ptri0.ApplyTransform(ref orgtrans0);
ptri1.ApplyTransform(ref orgtrans1);
//build planes
ptri0.BuildTriPlane();
ptri1.BuildTriPlane();
// test conservative
if (ptri0.OverlapTestConservative(ptri1))
{
if (ptri0.FindTriangleCollisionClipMethod(ptri1, m_contact_data))
{
int j = m_contact_data.m_point_count;
while (j-- != 0)
{
AddContactPoint(body0, body1,
m_contact_data.m_points[j],
MathUtil.Vector4ToVector3(ref m_contact_data.m_separating_normal),
-m_contact_data.m_penetration_depth);
}
}
}
#if TRI_COLLISION_PROFILING
BulletGlobals.StopProfile();
#endif
}
shape0.UnlockChildShapes();
shape1.UnlockChildShapes();
}
//! Collision routines
//!@{
protected void CollideGjkTriangles(CollisionObject body0,
CollisionObject body1,
GImpactMeshShapePart shape0,
GImpactMeshShapePart shape1,
ObjectArray<int> pairs, int pair_count)
{
TriangleShapeEx tri0 = new TriangleShapeEx();
TriangleShapeEx tri1 = new TriangleShapeEx();
shape0.LockChildShapes();
shape1.LockChildShapes();
int pair_pointer = 0;
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugGimpactAlgo)
{
BulletGlobals.g_streamWriter.WriteLine("GImpactAglo::CollideGjkTriangles [{0}]", pair_count);
}
while (pair_count-- != 0)
{
m_triface0 = pairs[pair_pointer];
m_triface1 = pairs[pair_pointer + 1];
pair_pointer += 2;
shape0.GetBulletTriangle(m_triface0, tri0);
shape1.GetBulletTriangle(m_triface1, tri1);
//collide two convex shapes
if (tri0.OverlapTestConservative(tri1))
{
ConvexVsConvexCollision(body0, body1, tri0, tri1);
}
}
shape0.UnlockChildShapes();
shape1.UnlockChildShapes();
}
protected void CollideSatTriangles(CollisionObject body0,
CollisionObject body1,
GImpactMeshShapePart shape0,
GImpactMeshShapePart shape1,
PairSet pairs, int pair_count)
{
IndexedMatrix orgtrans0 = body0.GetWorldTransform();
IndexedMatrix orgtrans1 = body1.GetWorldTransform();
PrimitiveTriangle ptri0 = new PrimitiveTriangle();
PrimitiveTriangle ptri1 = new PrimitiveTriangle();
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugGimpactAlgo)
{
BulletGlobals.g_streamWriter.WriteLine("GImpactAglo::CollideSatTriangles [{0}]", pair_count);
}
shape0.LockChildShapes();
shape1.LockChildShapes();
int pair_pointer = 0;
while (pair_count-- != 0)
{
m_triface0 = pairs[pair_pointer].m_index1;
m_triface1 = pairs[pair_pointer].m_index2;
pair_pointer += 1;
shape0.GetPrimitiveTriangle(m_triface0, ptri0);
shape1.GetPrimitiveTriangle(m_triface1, ptri1);
#if TRI_COLLISION_PROFILING
BulletGlobal.StartProfile("gim02_tri_time");
#endif
ptri0.ApplyTransform(ref orgtrans0);
ptri1.ApplyTransform(ref orgtrans1);
//build planes
ptri0.BuildTriPlane();
ptri1.BuildTriPlane();
// test conservative
if (ptri0.OverlapTestConservative(ptri1))
{
if (ptri0.FindTriangleCollisionClipMethod(ptri1, m_contact_data))
{
int j = m_contact_data.m_point_count;
while (j-- != 0)
{
AddContactPoint(body0, body1,
m_contact_data.m_points[j],
MathUtil.Vector4ToVector3(ref m_contact_data.m_separating_normal),
-m_contact_data.m_penetration_depth);
}
}
}
#if TRI_COLLISION_PROFILING
BulletGlobals.StopProfile();
#endif
}
shape0.UnlockChildShapes();
shape1.UnlockChildShapes();
}
