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 DEBUG
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);
}
#endif
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);
}
}
}
}
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
}
//! returns the indices of the primitives in the m_primitive_manager
public bool BoxQuery(ref AABB box, ObjectArray <int> collided_results)
{
int curIndex = 0;
int numNodes = GetNodeCount();
while (curIndex < numNodes)
{
AABB bound = new AABB();
GetNodeBound(curIndex, out bound);
//catch bugs in tree data
bool aabbOverlap = bound.HasCollision(ref box);
bool isleafnode = IsLeafNode(curIndex);
if (isleafnode && aabbOverlap)
{
collided_results.Add(GetNodeData(curIndex));
}
if (aabbOverlap || isleafnode)
{
//next subnode
curIndex++;
}
else
{
//skip node
curIndex += GetEscapeNodeIndex(curIndex);
}
}
if (collided_results.Count > 0)
{
return(true);
}
return(false);
}
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);
}
}
}
}
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 (BulletGlobals.g_streamWriter != null && BulletGlobals.debugGimpactAlgo)
{
BulletGlobals.g_streamWriter.WriteLine("GImpactAglo::GImpactVsGImpactFindPairs [{0}]", pairset.Count);
}
}
