public static void FindCollision(GImpactQuantizedBvh boxset0, ref IndexedMatrix trans0,
GImpactQuantizedBvh boxset1, ref IndexedMatrix trans1,
PairSet collision_pairs)
{
if (boxset0.GetNodeCount() == 0 || boxset1.GetNodeCount() == 0)
{
return;
}
BT_BOX_BOX_TRANSFORM_CACHE trans_cache_1to0 = new BT_BOX_BOX_TRANSFORM_CACHE();
trans_cache_1to0.CalcFromHomogenic(ref trans0, ref trans1);
#if TRI_COLLISION_PROFILING
BulletGlobals.StartProfile("GIMPACT-TRIMESH");
#endif //TRI_COLLISION_PROFILING
FindQuantizedCollisionPairsRecursive(boxset0, boxset1, collision_pairs, ref trans_cache_1to0, 0, 0, true);
#if TRI_COLLISION_PROFILING
BulletGlobals.StopProfile();
#endif //TRI_COLLISION_PROFILING
}
protected void GImpactVsGImpactFindPairs(
ref IndexedMatrix trans0,
ref IndexedMatrix trans1,
GImpactShapeInterface shape0,
GImpactShapeInterface shape1, PairSet pairset)
{
if (shape0.HasBoxSet() && shape1.HasBoxSet())
{
GImpactQuantizedBvh.FindCollision(shape0.GetBoxSet(), ref trans0, shape1.GetBoxSet(), ref trans1, pairset);
}
else
{
AABB boxshape0 = new AABB();
AABB boxshape1 = new AABB();
int i = shape0.GetNumChildShapes();
while (i-- != 0)
{
shape0.GetChildAabb(i, ref trans0, out boxshape0.m_min, out boxshape0.m_max);
int j = shape1.GetNumChildShapes();
while (j-- != 0)
{
shape1.GetChildAabb(i, ref trans1, out boxshape1.m_min, out boxshape1.m_max);
if (boxshape1.HasCollision(ref boxshape0))
{
pairset.PushPair(i, j);
}
}
}
}
#if DEBUG
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugGimpactAlgo)
{
BulletGlobals.g_streamWriter.WriteLine("GImpactAglo::GImpactVsGImpactFindPairs [{0}]", pairset.Count);
}
#endif
}
public static void FindCollision(GImpactBvh boxset1, ref IndexedMatrix trans1,
GImpactBvh boxset2, ref IndexedMatrix trans2,
PairSet collision_pairs)
{
if (boxset1.GetNodeCount() == 0 || boxset2.GetNodeCount() == 0)
{
return;
}
BT_BOX_BOX_TRANSFORM_CACHE trans_cache_1to0 = new BT_BOX_BOX_TRANSFORM_CACHE();
trans_cache_1to0.CalcFromHomogenic(ref trans1, ref trans2);
#if TRI_COLLISION_PROFILING
bt_begin_gim02_tree_time();
#endif //TRI_COLLISION_PROFILING
FindCollisionPairsRecursive(boxset1, boxset2, collision_pairs, trans_cache_1to0, 0, 0, true);
#if TRI_COLLISION_PROFILING
bt_end_gim02_tree_time();
#endif //TRI_COLLISION_PROFILING
}
public static void FindQuantizedCollisionPairsRecursive(
GImpactQuantizedBvh boxset0, GImpactQuantizedBvh boxset1,
PairSet collision_pairs,
ref BT_BOX_BOX_TRANSFORM_CACHE trans_cache_1to0,
int node0, int node1, bool complete_primitive_tests)
{
if (QuantizedNodeCollision(
boxset0, boxset1, trans_cache_1to0,
node0, node1, complete_primitive_tests) == false)
{
return; //avoid colliding internal nodes
}
if (boxset0.IsLeafNode(node0))
{
if (boxset1.IsLeafNode(node1))
{
// collision result
collision_pairs.PushPair(boxset0.GetNodeData(node0), boxset1.GetNodeData(node1));
return;
}
else
{
//collide left recursive
FindQuantizedCollisionPairsRecursive(
boxset0, boxset1,
collision_pairs, ref trans_cache_1to0,
node0, boxset1.GetLeftNode(node1), false);
//collide right recursive
FindQuantizedCollisionPairsRecursive(
boxset0, boxset1,
collision_pairs, ref trans_cache_1to0,
node0, boxset1.GetRightNode(node1), false);
}
}
else
{
if (boxset1.IsLeafNode(node1))
{
//collide left recursive
FindQuantizedCollisionPairsRecursive(
boxset0, boxset1,
collision_pairs, ref trans_cache_1to0,
boxset0.GetLeftNode(node0), node1, false);
//collide right recursive
FindQuantizedCollisionPairsRecursive(
boxset0, boxset1,
collision_pairs, ref trans_cache_1to0,
boxset0.GetRightNode(node0), node1, false);
}
else
{
//collide left0 left1
FindQuantizedCollisionPairsRecursive(
boxset0, boxset1,
collision_pairs, ref trans_cache_1to0,
boxset0.GetLeftNode(node0), boxset1.GetLeftNode(node1), false);
//collide left0 right1
FindQuantizedCollisionPairsRecursive(
boxset0, boxset1,
collision_pairs, ref trans_cache_1to0,
boxset0.GetLeftNode(node0), boxset1.GetRightNode(node1), false);
//collide right0 left1
FindQuantizedCollisionPairsRecursive(
boxset0, boxset1,
collision_pairs, ref trans_cache_1to0,
boxset0.GetRightNode(node0), boxset1.GetLeftNode(node1), false);
//collide right0 right1
FindQuantizedCollisionPairsRecursive(
boxset0, boxset1,
collision_pairs, ref trans_cache_1to0,
boxset0.GetRightNode(node0), boxset1.GetRightNode(node1), false);
} // else if node1 is not a leaf
} // else if node0 is not a leaf
}
//! Collides two gimpact shapes
/*!
* \pre shape0 and shape1 couldn't be btGImpactMeshShape objects
*/
public void GImpactVsGImpact(CollisionObject body0,
CollisionObject body1,
GImpactShapeInterface shape0,
GImpactShapeInterface shape1)
{
if (shape0.GetGImpactShapeType() == GIMPACT_SHAPE_TYPE.CONST_GIMPACT_TRIMESH_SHAPE)
{
GImpactMeshShape meshshape0 = shape0 as GImpactMeshShape;
m_part0 = meshshape0.GetMeshPartCount();
while (m_part0-- != 0)
{
GImpactVsGImpact(body0, body1, meshshape0.GetMeshPart(m_part0), shape1);
}
return;
}
if (shape1.GetGImpactShapeType() == GIMPACT_SHAPE_TYPE.CONST_GIMPACT_TRIMESH_SHAPE)
{
GImpactMeshShape meshshape1 = shape1 as GImpactMeshShape;
m_part1 = meshshape1.GetMeshPartCount();
while (m_part1-- != 0)
{
GImpactVsGImpact(body0, body1, shape0, meshshape1.GetMeshPart(m_part1));
}
return;
}
IndexedMatrix orgtrans0 = body0.GetWorldTransform();
IndexedMatrix orgtrans1 = body1.GetWorldTransform();
PairSet pairset = new PairSet();
GImpactVsGImpactFindPairs(ref orgtrans0, ref orgtrans1, shape0, shape1, pairset);
if (pairset.Count == 0)
{
return;
}
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugGimpactAlgo)
{
BulletGlobals.g_streamWriter.WriteLine("GImpactAglo::GImpactVsGImpact [{0}]", pairset.Count);
}
if (shape0.GetGImpactShapeType() == GIMPACT_SHAPE_TYPE.CONST_GIMPACT_TRIMESH_SHAPE_PART &&
shape1.GetGImpactShapeType() == GIMPACT_SHAPE_TYPE.CONST_GIMPACT_TRIMESH_SHAPE_PART)
{
GImpactMeshShapePart shapepart0 = shape0 as GImpactMeshShapePart;
GImpactMeshShapePart shapepart1 = shape1 as GImpactMeshShapePart;
//specialized function
#if BULLET_TRIANGLE_COLLISION
CollideGjkTriangles(body0, body1, shapepart0, shapepart1, &pairset[0].m_index1, pairset.size());
#else
CollideSatTriangles(body0, body1, shapepart0, shapepart1, pairset, pairset.Count);
#endif
return;
}
//general function
shape0.LockChildShapes();
shape1.LockChildShapes();
using (GIM_ShapeRetriever retriever0 = BulletGlobals.GIM_ShapeRetrieverPool.Get())
using (GIM_ShapeRetriever retriever1 = BulletGlobals.GIM_ShapeRetrieverPool.Get())
{
retriever0.Initialize(shape0);
retriever1.Initialize(shape1);
bool child_has_transform0 = shape0.ChildrenHasTransform();
bool child_has_transform1 = shape1.ChildrenHasTransform();
int i = pairset.Count;
while (i-- != 0)
{
GIM_PAIR pair = pairset[i];
m_triface0 = pair.m_index1;
m_triface1 = pair.m_index2;
CollisionShape colshape0 = retriever0.GetChildShape(m_triface0);
CollisionShape colshape1 = retriever1.GetChildShape(m_triface1);
if (child_has_transform0)
{
body0.SetWorldTransform(orgtrans0 * shape0.GetChildTransform(m_triface0));
}
if (child_has_transform1)
{
body1.SetWorldTransform(orgtrans1 * shape1.GetChildTransform(m_triface1));
}
//collide two convex shapes
ConvexVsConvexCollision(body0, body1, colshape0, colshape1);
if (child_has_transform0)
{
body0.SetWorldTransform(ref orgtrans0);
}
if (child_has_transform1)
{
body1.SetWorldTransform(ref orgtrans1);
}
}
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();
}