OPC_TreeCollider.cpp

Go to the documentation of this file.

/*
 *      OPCODE - Optimized Collision Detection
 *      Copyright (C) 2001 Pierre Terdiman
 *      Homepage: http://www.codercorner.com/Opcode.htm
 */







#include"../CollisionPairInserter.h"

// Precompiled Header
#include "Stdafx.h"

#include<iostream>

using namespace Opcode;

#include "OPC_BoxBoxOverlap.h"
//#include "OPC_TriBoxOverlap.h"
#include "OPC_TriTriOverlap.h"



AABBTreeCollider::AABBTreeCollider() :
        mIMesh0                         (null),
        mIMesh1                         (null),
        mNbBVBVTests            (0),
        mNbPrimPrimTests        (0),
        mNbBVPrimTests          (0),
        mFullBoxBoxTest         (true),
        mFullPrimBoxTest        (true),
        collisionPairInserter(0)
{
}




AABBTreeCollider::~AABBTreeCollider()
{
}



const char* AABBTreeCollider::ValidateSettings()
{
        if(TemporalCoherenceEnabled() && !FirstContactEnabled())        return "Temporal coherence only works with ""First contact"" mode!";
        return null;
}



bool AABBTreeCollider::Collide(BVTCache& cache, const Matrix4x4* world0, const Matrix4x4* world1)
{
        // Checkings
        if(!cache.Model0 || !cache.Model1)                                                              return false;
        if(cache.Model0->HasLeafNodes()!=cache.Model1->HasLeafNodes())  return false;
        if(cache.Model0->IsQuantized()!=cache.Model1->IsQuantized())    return false;

        /*
        
          Rules:
                - perform hull test
                - when hulls collide, disable hull test
                - if meshes overlap, reset countdown
                - if countdown reaches 0, enable hull test

        */

#ifdef __MESHMERIZER_H__
        // Handle hulls
        if(cache.HullTest)
        {
                if(cache.Model0->GetHull() && cache.Model1->GetHull())
                {
                        struct Local
                        {
                                static Point* SVCallback(const Point& sv, udword& previndex, udword user_data)
                                {
                                        CollisionHull* Hull = (CollisionHull*)user_data;
                                        previndex = Hull->ComputeSupportingVertex(sv, previndex);
                                        return (Point*)&Hull->GetVerts()[previndex];
                                }
                        };

                        bool Collide;

                        if(0)
                        {
                                static GJKEngine GJK;
                                static bool GJKInitDone=false;
                                if(!GJKInitDone)
                                {
                                        GJK.Enable(GJK_BACKUP_PROCEDURE);
                                        GJK.Enable(GJK_DEGENERATE);
                                        GJK.Enable(GJK_HILLCLIMBING);
                                        GJKInitDone = true;
                                }
                                GJK.SetCallbackObj0(Local::SVCallback);
                                GJK.SetCallbackObj1(Local::SVCallback);
                                GJK.SetUserData0(udword(cache.Model0->GetHull()));
                                GJK.SetUserData1(udword(cache.Model1->GetHull()));
                                Collide = GJK.Collide(*world0, *world1, &cache.SepVector);
                        }
                        else
                        {
                                static SVEngine SVE;
                                SVE.SetCallbackObj0(Local::SVCallback);
                                SVE.SetCallbackObj1(Local::SVCallback);
                                SVE.SetUserData0(udword(cache.Model0->GetHull()));
                                SVE.SetUserData1(udword(cache.Model1->GetHull()));
                                Collide = SVE.Collide(*world0, *world1, &cache.SepVector);
                        }

                        if(!Collide)
                        {
                // Reset stats & contact status
                mFlags &= ~OPC_CONTACT;
                mNbBVBVTests            = 0;
                mNbPrimPrimTests        = 0;
                mNbBVPrimTests          = 0;
                mPairs.Reset();
                return true;
                        }
                }
        }

        // Here, hulls collide
        cache.HullTest = false;
#endif // __MESHMERIZER_H__

        // Checkings
        if(!Setup(cache.Model0->GetMeshInterface(), cache.Model1->GetMeshInterface()))  return false;

        // Simple double-dispatch
        bool Status;
        if(!cache.Model0->HasLeafNodes())
        {
                if(cache.Model0->IsQuantized())
                {
                        const AABBQuantizedNoLeafTree* T0 = (const AABBQuantizedNoLeafTree*)cache.Model0->GetTree();
                        const AABBQuantizedNoLeafTree* T1 = (const AABBQuantizedNoLeafTree*)cache.Model1->GetTree();
                        Status = Collide(T0, T1, world0, world1, &cache);
                }
                else
                {
                        const AABBNoLeafTree* T0 = (const AABBNoLeafTree*)cache.Model0->GetTree();
                        const AABBNoLeafTree* T1 = (const AABBNoLeafTree*)cache.Model1->GetTree();
                        Status = Collide(T0, T1, world0, world1, &cache);
                }
        }
        else
        {
                if(cache.Model0->IsQuantized())
                {
                        const AABBQuantizedTree* T0 = (const AABBQuantizedTree*)cache.Model0->GetTree();
                        const AABBQuantizedTree* T1 = (const AABBQuantizedTree*)cache.Model1->GetTree();
                        Status = Collide(T0, T1, world0, world1, &cache);
                }
                else
                {
                        const AABBCollisionTree* T0 = (const AABBCollisionTree*)cache.Model0->GetTree();
                        const AABBCollisionTree* T1 = (const AABBCollisionTree*)cache.Model1->GetTree();
                        Status = Collide(T0, T1, world0, world1, &cache);
                }
        }

#ifdef __MESHMERIZER_H__
        if(Status)
        {
                // Reset counter as long as overlap occurs
                if(GetContactStatus())  cache.ResetCountDown();

                // Enable hull test again when counter reaches zero
                cache.CountDown--;
                if(!cache.CountDown)
                {
                        cache.ResetCountDown();
                        cache.HullTest = true;
                }
        }
#endif
        return Status;
}



void AABBTreeCollider::InitQuery(const Matrix4x4* world0, const Matrix4x4* world1)
{
        // Reset stats & contact status
        Collider::InitQuery();
        mNbBVBVTests            = 0;
        mNbPrimPrimTests        = 0;
        mNbBVPrimTests          = 0;
        mPairs.Reset();

        // Setup matrices
        Matrix4x4 InvWorld0, InvWorld1;
        if(world0)      InvertPRMatrix(InvWorld0, *world0);
        else            InvWorld0.Identity();

        if(world1)      InvertPRMatrix(InvWorld1, *world1);
        else            InvWorld1.Identity();

        Matrix4x4 World0to1 = world0 ? (*world0 * InvWorld1) : InvWorld1;
        Matrix4x4 World1to0 = world1 ? (*world1 * InvWorld0) : InvWorld0;

        mR0to1 = World0to1;             World0to1.GetTrans(mT0to1);
        mR1to0 = World1to0;             World1to0.GetTrans(mT1to0);

        // Precompute absolute 1-to-0 rotation matrix
        for(udword i=0;i<3;i++)
        {
                for(udword j=0;j<3;j++)
                {
                        // Epsilon value prevents floating-point inaccuracies (strategy borrowed from CD)
                        mAR.m[i][j] = 1e-6f + fabsf(mR1to0.m[i][j]);
                }
        }


        //Modified by S-cubed, Inc.
        //$B9TNs$r(B Boost $B$N7A<0$K$"$o$;$k!#(B
        //  Prim-Prim $B%F%9%H$r(B TriOverlap.cpp $B$G9T$&$?$a!#(B
        if(collisionPairInserter){
            
            for(udword i=0; i<3; i++){
                for(udword j=0; j<3; j++){
                    collisionPairInserter->CD_Rot1(i,j) = (double) world0->m[j][i];
                    collisionPairInserter->CD_Rot2(i,j) = (double) world1->m[j][i];
                }
            }
 
            IceMaths::Point t0;
            IceMaths::Point t1;
            world0->GetTrans(t0);
            world1->GetTrans(t1);
            //  Matrix3x3 w1 = world1;
            //  mT0to1 = w1 * (t1 - t0)
            collisionPairInserter->CD_Trans1[0] = (double) t0.x;
            collisionPairInserter->CD_Trans1[1] = (double) t0.y;
            collisionPairInserter->CD_Trans1[2] = (double) t0.z;
            
            collisionPairInserter->CD_Trans2[0] = (double) t1.x;
            collisionPairInserter->CD_Trans2[1] = (double) t1.y;
            collisionPairInserter->CD_Trans2[2] = (double) t1.z;
            
            //s1, s2 $B$O(B 1.0 $B$N$_$J$N$G!"8GDj!#(B
            collisionPairInserter->CD_s1 = 1.0;
            collisionPairInserter->CD_s2 = 1.0;
        }
}
        



bool AABBTreeCollider::CheckTemporalCoherence(Pair* cache)
{
        // Checkings
        if(!cache)      return false;

        // Test previously colliding primitives first
        if(TemporalCoherenceEnabled() && FirstContactEnabled())
        {
                PrimTest(cache->id0, cache->id1);
                if(GetContactStatus())  return true;
        }
        return false;
}

#define UPDATE_CACHE                                            \
        if(cache && GetContactStatus())                 \
        {                                                                               \
                cache->id0 = mPairs.GetEntry(0);        \
                cache->id1 = mPairs.GetEntry(1);        \
        }



bool AABBTreeCollider::Collide(const AABBCollisionTree* tree0, const AABBCollisionTree* tree1, const Matrix4x4* world0, const Matrix4x4* world1, Pair* cache)
{
        // Init collision query
        InitQuery(world0, world1);

        // Check previous state
        if(CheckTemporalCoherence(cache))               return true;

        // Perform collision query
        _Collide(tree0->GetNodes(), tree1->GetNodes());

        UPDATE_CACHE

        return true;
}



bool AABBTreeCollider::Collide(const AABBNoLeafTree* tree0, const AABBNoLeafTree* tree1, const Matrix4x4* world0, const Matrix4x4* world1, Pair* cache)
{
        // Init collision query
        InitQuery(world0, world1);

        // Check previous state
        if(CheckTemporalCoherence(cache))               return true;

        // Perform collision query
        _Collide(tree0->GetNodes(), tree1->GetNodes());

        UPDATE_CACHE

        return true;
}



bool AABBTreeCollider::Collide(const AABBQuantizedTree* tree0, const AABBQuantizedTree* tree1, const Matrix4x4* world0, const Matrix4x4* world1, Pair* cache)
{
        // Init collision query
        InitQuery(world0, world1);

        // Check previous state
        if(CheckTemporalCoherence(cache))               return true;

        // Setup dequantization coeffs
        mCenterCoeff0   = tree0->mCenterCoeff;
        mExtentsCoeff0  = tree0->mExtentsCoeff;
        mCenterCoeff1   = tree1->mCenterCoeff;
        mExtentsCoeff1  = tree1->mExtentsCoeff;

        // Dequantize box A
        const AABBQuantizedNode* N0 = tree0->GetNodes();
        const Point a(float(N0->mAABB.mExtents[0]) * mExtentsCoeff0.x, float(N0->mAABB.mExtents[1]) * mExtentsCoeff0.y, float(N0->mAABB.mExtents[2]) * mExtentsCoeff0.z);
        const Point Pa(float(N0->mAABB.mCenter[0]) * mCenterCoeff0.x, float(N0->mAABB.mCenter[1]) * mCenterCoeff0.y, float(N0->mAABB.mCenter[2]) * mCenterCoeff0.z);
        // Dequantize box B
        const AABBQuantizedNode* N1 = tree1->GetNodes();
        const Point b(float(N1->mAABB.mExtents[0]) * mExtentsCoeff1.x, float(N1->mAABB.mExtents[1]) * mExtentsCoeff1.y, float(N1->mAABB.mExtents[2]) * mExtentsCoeff1.z);
        const Point Pb(float(N1->mAABB.mCenter[0]) * mCenterCoeff1.x, float(N1->mAABB.mCenter[1]) * mCenterCoeff1.y, float(N1->mAABB.mCenter[2]) * mCenterCoeff1.z);

        // Perform collision query
        _Collide(N0, N1, a, Pa, b, Pb);

        UPDATE_CACHE

        return true;
}



bool AABBTreeCollider::Collide(const AABBQuantizedNoLeafTree* tree0, const AABBQuantizedNoLeafTree* tree1, const Matrix4x4* world0, const Matrix4x4* world1, Pair* cache)
{
        // Init collision query
        InitQuery(world0, world1);

        // Check previous state
        if(CheckTemporalCoherence(cache))               return true;

        // Setup dequantization coeffs
        mCenterCoeff0   = tree0->mCenterCoeff;
        mExtentsCoeff0  = tree0->mExtentsCoeff;
        mCenterCoeff1   = tree1->mCenterCoeff;
        mExtentsCoeff1  = tree1->mExtentsCoeff;

        // Perform collision query
        _Collide(tree0->GetNodes(), tree1->GetNodes());

        UPDATE_CACHE

        return true;
}

// Standard trees

// The normal AABB tree can use 2 different descent rules (with different performances)
//#define ORIGINAL_CODE                 //!< UNC-like descent rules
#define ALTERNATIVE_CODE                //!< Alternative descent rules

#ifdef ORIGINAL_CODE



void AABBTreeCollider::_Collide(const AABBCollisionNode* b0, const AABBCollisionNode* b1)
{
  mNowNode0 = b0;
  mNowNode1 = b1;
        // Perform BV-BV overlap test
        if(!BoxBoxOverlap(b0->mAABB.mExtents, b0->mAABB.mCenter, b1->mAABB.mExtents, b1->mAABB.mCenter))        return;

        if(b0->IsLeaf() && b1->IsLeaf()) { PrimTest(b0->GetPrimitive(), b1->GetPrimitive()); return; }

        if(b1->IsLeaf() || (!b0->IsLeaf() && (b0->GetSize() > b1->GetSize())))
        {
                _Collide(b0->GetNeg(), b1);
                if(ContactFound()) return;
                _Collide(b0->GetPos(), b1);
        }
        else
        {
                _Collide(b0, b1->GetNeg());
                if(ContactFound()) return;
                _Collide(b0, b1->GetPos());
        }
}
#endif

#ifdef ALTERNATIVE_CODE



void AABBTreeCollider::_Collide(const AABBCollisionNode* b0, const AABBCollisionNode* b1)
{
        // Perform BV-BV overlap test
        if(!BoxBoxOverlap(b0->mAABB.mExtents, b0->mAABB.mCenter, b1->mAABB.mExtents, b1->mAABB.mCenter))
        {
                return;
        }

        if(b0->IsLeaf())
        {
                if(b1->IsLeaf())
                {
                  mNowNode0 = b0;
                  mNowNode1 = b1;
                        PrimTest(b0->GetPrimitive(), b1->GetPrimitive());
                }
                else
                {
                        _Collide(b0, b1->GetNeg());
                        if(ContactFound()) return;
                        _Collide(b0, b1->GetPos());
                }
        }
        else if(b1->IsLeaf())
        {
                _Collide(b0->GetNeg(), b1);
                if(ContactFound()) return;
                _Collide(b0->GetPos(), b1);
        }
        else
        {
                _Collide(b0->GetNeg(), b1->GetNeg());
                if(ContactFound()) return;
                _Collide(b0->GetNeg(), b1->GetPos());
                if(ContactFound()) return;
                _Collide(b0->GetPos(), b1->GetNeg());
                if(ContactFound()) return;
                _Collide(b0->GetPos(), b1->GetPos());
        }
}
#endif

// No-leaf trees



void AABBTreeCollider::PrimTest(udword id0, udword id1)
{
    mId0 = id0;
    mId1 = id1;
        // Request vertices from the app
        VertexPointers VP0;
        VertexPointers VP1;
        mIMesh0->GetTriangle(VP0, id0);
        mIMesh1->GetTriangle(VP1, id1);

        // Modified by S-cubed, Inc.
        // Transform from space 0 (old : 1) to space 1 (old : 0)
        // CD $B$G$OJQ49$,5U$J$N$G$"$o$;$k!#(B
        Point u0,u1,u2;
        TransformPoint(u0, *VP0.Vertex[0], mR0to1, mT0to1);
        TransformPoint(u1, *VP0.Vertex[1], mR0to1, mT0to1);
        TransformPoint(u2, *VP0.Vertex[2], mR0to1, mT0to1);

        // Perform triangle-triangle overlap test
        if(TriTriOverlap(u0, u1, u2,
                                         *VP1.Vertex[0], *VP1.Vertex[1], *VP1.Vertex[2]))
        {
                // Keep track of colliding pairs
                mPairs.Add(id0).Add(id1);
                // Set contact status
                mFlags |= OPC_CONTACT;
        }
}



inline_ void AABBTreeCollider::PrimTestTriIndex(udword id1)
{
        // Request vertices from the app
        VertexPointers VP;
        mIMesh1->GetTriangle(VP, id1);

        // Perform triangle-triangle overlap test
        if(TriTriOverlap(mLeafVerts[0], mLeafVerts[1], mLeafVerts[2], *VP.Vertex[0], *VP.Vertex[1], *VP.Vertex[2]))
        {
                // Keep track of colliding pairs
                mPairs.Add(mLeafIndex).Add(id1);
                // Set contact status
                mFlags |= OPC_CONTACT;
        }
}



inline_ void AABBTreeCollider::PrimTestIndexTri(udword id0)
{
        // Request vertices from the app
        VertexPointers VP;
        mIMesh0->GetTriangle(VP, id0);

        // Perform triangle-triangle overlap test
        if(TriTriOverlap(mLeafVerts[0], mLeafVerts[1], mLeafVerts[2], *VP.Vertex[0], *VP.Vertex[1], *VP.Vertex[2]))
        {
                // Keep track of colliding pairs
                mPairs.Add(id0).Add(mLeafIndex);
                // Set contact status
                mFlags |= OPC_CONTACT;
        }
}



void AABBTreeCollider::_CollideTriBox(const AABBNoLeafNode* b)
{
        // Perform triangle-box overlap test
  // Modified by S-cubed, Inc.
  //NoleafNode $B$O;HMQ$7$J$$(B
  //    if(!TriBoxOverlap(b->mAABB.mCenter, b->mAABB.mExtents)) return;

        // Keep same triangle, deal with first child
        if(b->HasPosLeaf())     PrimTestTriIndex(b->GetPosPrimitive());
        else                            _CollideTriBox(b->GetPos());

        if(ContactFound()) return;

        // Keep same triangle, deal with second child
        if(b->HasNegLeaf())     PrimTestTriIndex(b->GetNegPrimitive());
        else                            _CollideTriBox(b->GetNeg());
}



void AABBTreeCollider::_CollideBoxTri(const AABBNoLeafNode* b)
{
  // Modified by S-cubed, Inc.
  //NoleafNode $B$O;HMQ$7$J$$(B
        // Perform triangle-box overlap test
        //if(!TriBoxOverlap(b->mAABB.mCenter, b->mAABB.mExtents))       return;

        // Keep same triangle, deal with first child
        if(b->HasPosLeaf())     PrimTestIndexTri(b->GetPosPrimitive());
        else                            _CollideBoxTri(b->GetPos());

        if(ContactFound()) return;

        // Keep same triangle, deal with second child
        if(b->HasNegLeaf())     PrimTestIndexTri(b->GetNegPrimitive());
        else                            _CollideBoxTri(b->GetNeg());
}

#define FETCH_LEAF(prim_index, imesh, rot, trans)                               \
        mLeafIndex = prim_index;                                                                        \
        /* Request vertices from the app */                                                     \
        VertexPointers VP;      imesh->GetTriangle(VP, prim_index);             \
        /* Transform them in a common space */                                          \
        TransformPoint(mLeafVerts[0], *VP.Vertex[0], rot, trans);       \
        TransformPoint(mLeafVerts[1], *VP.Vertex[1], rot, trans);       \
        TransformPoint(mLeafVerts[2], *VP.Vertex[2], rot, trans);



void AABBTreeCollider::_Collide(const AABBNoLeafNode* a, const AABBNoLeafNode* b)
{
        // Perform BV-BV overlap test
        if(!BoxBoxOverlap(a->mAABB.mExtents, a->mAABB.mCenter, b->mAABB.mExtents, b->mAABB.mCenter))    return;

        // Catch leaf status
        BOOL BHasPosLeaf = b->HasPosLeaf();
        BOOL BHasNegLeaf = b->HasNegLeaf();

        if(a->HasPosLeaf())
        {
                FETCH_LEAF(a->GetPosPrimitive(), mIMesh0, mR0to1, mT0to1)

                if(BHasPosLeaf) PrimTestTriIndex(b->GetPosPrimitive());
                else                    _CollideTriBox(b->GetPos());

                if(ContactFound()) return;

                if(BHasNegLeaf) PrimTestTriIndex(b->GetNegPrimitive());
                else                    _CollideTriBox(b->GetNeg());
        }
        else
        {
                if(BHasPosLeaf)
                {
                        FETCH_LEAF(b->GetPosPrimitive(), mIMesh1, mR1to0, mT1to0)

                        _CollideBoxTri(a->GetPos());
                }
                else _Collide(a->GetPos(), b->GetPos());

                if(ContactFound()) return;

                if(BHasNegLeaf)
                {
                        FETCH_LEAF(b->GetNegPrimitive(), mIMesh1, mR1to0, mT1to0)

                        _CollideBoxTri(a->GetPos());
                }
                else _Collide(a->GetPos(), b->GetNeg());
        }

        if(ContactFound()) return;

        if(a->HasNegLeaf())
        {
                FETCH_LEAF(a->GetNegPrimitive(), mIMesh0, mR0to1, mT0to1)

                if(BHasPosLeaf) PrimTestTriIndex(b->GetPosPrimitive());
                else                    _CollideTriBox(b->GetPos());

                if(ContactFound()) return;

                if(BHasNegLeaf) PrimTestTriIndex(b->GetNegPrimitive());
                else                    _CollideTriBox(b->GetNeg());
        }
        else
        {
                if(BHasPosLeaf)
                {
                        // ### That leaf has possibly already been fetched
                        FETCH_LEAF(b->GetPosPrimitive(), mIMesh1, mR1to0, mT1to0)

                        _CollideBoxTri(a->GetNeg());
                }
                else _Collide(a->GetNeg(), b->GetPos());

                if(ContactFound()) return;

                if(BHasNegLeaf)
                {
                        // ### That leaf has possibly already been fetched
                        FETCH_LEAF(b->GetNegPrimitive(), mIMesh1, mR1to0, mT1to0)

                        _CollideBoxTri(a->GetNeg());
                }
                else _Collide(a->GetNeg(), b->GetNeg());
        }
}

// Quantized trees



void AABBTreeCollider::_Collide(const AABBQuantizedNode* b0, const AABBQuantizedNode* b1, const Point& a, const Point& Pa, const Point& b, const Point& Pb)
{
        // Perform BV-BV overlap test
        if(!BoxBoxOverlap(a, Pa, b, Pb))        return;

        if(b0->IsLeaf() && b1->IsLeaf()) { PrimTest(b0->GetPrimitive(), b1->GetPrimitive()); return; }

        if(b1->IsLeaf() || (!b0->IsLeaf() && (b0->GetSize() > b1->GetSize())))
        {
                // Dequantize box
                const QuantizedAABB* Box = &b0->GetNeg()->mAABB;
                const Point negPa(float(Box->mCenter[0]) * mCenterCoeff0.x, float(Box->mCenter[1]) * mCenterCoeff0.y, float(Box->mCenter[2]) * mCenterCoeff0.z);
                const Point nega(float(Box->mExtents[0]) * mExtentsCoeff0.x, float(Box->mExtents[1]) * mExtentsCoeff0.y, float(Box->mExtents[2]) * mExtentsCoeff0.z);
                _Collide(b0->GetNeg(), b1, nega, negPa, b, Pb);

                if(ContactFound()) return;

                // Dequantize box
                Box = &b0->GetPos()->mAABB;
                const Point posPa(float(Box->mCenter[0]) * mCenterCoeff0.x, float(Box->mCenter[1]) * mCenterCoeff0.y, float(Box->mCenter[2]) * mCenterCoeff0.z);
                const Point posa(float(Box->mExtents[0]) * mExtentsCoeff0.x, float(Box->mExtents[1]) * mExtentsCoeff0.y, float(Box->mExtents[2]) * mExtentsCoeff0.z);
                _Collide(b0->GetPos(), b1, posa, posPa, b, Pb);
        }
        else
        {
                // Dequantize box
                const QuantizedAABB* Box = &b1->GetNeg()->mAABB;
                const Point negPb(float(Box->mCenter[0]) * mCenterCoeff1.x, float(Box->mCenter[1]) * mCenterCoeff1.y, float(Box->mCenter[2]) * mCenterCoeff1.z);
                const Point negb(float(Box->mExtents[0]) * mExtentsCoeff1.x, float(Box->mExtents[1]) * mExtentsCoeff1.y, float(Box->mExtents[2]) * mExtentsCoeff1.z);
                _Collide(b0, b1->GetNeg(), a, Pa, negb, negPb);

                if(ContactFound()) return;

                // Dequantize box
                Box = &b1->GetPos()->mAABB;
                const Point posPb(float(Box->mCenter[0]) * mCenterCoeff1.x, float(Box->mCenter[1]) * mCenterCoeff1.y, float(Box->mCenter[2]) * mCenterCoeff1.z);
                const Point posb(float(Box->mExtents[0]) * mExtentsCoeff1.x, float(Box->mExtents[1]) * mExtentsCoeff1.y, float(Box->mExtents[2]) * mExtentsCoeff1.z);
                _Collide(b0, b1->GetPos(), a, Pa, posb, posPb);
        }
}

// Quantized no-leaf trees



void AABBTreeCollider::_CollideTriBox(const AABBQuantizedNoLeafNode* b)
{

        // Dequantize box
        const QuantizedAABB* bb = &b->mAABB;
        const Point Pb(float(bb->mCenter[0]) * mCenterCoeff1.x, float(bb->mCenter[1]) * mCenterCoeff1.y, float(bb->mCenter[2]) * mCenterCoeff1.z);
        const Point eb(float(bb->mExtents[0]) * mExtentsCoeff1.x, float(bb->mExtents[1]) * mExtentsCoeff1.y, float(bb->mExtents[2]) * mExtentsCoeff1.z);

        // Perform triangle-box overlap test
        // Modified by S-cubed, Inc.
        //NoleafNode $B$O;HMQ$7$J$$(B
        //if(!TriBoxOverlap(Pb, eb))    return;

        if(b->HasPosLeaf())     PrimTestTriIndex(b->GetPosPrimitive());
        else                            _CollideTriBox(b->GetPos());

        if(ContactFound()) return;

        if(b->HasNegLeaf())     PrimTestTriIndex(b->GetNegPrimitive());
        else                            _CollideTriBox(b->GetNeg());
}



void AABBTreeCollider::_CollideBoxTri(const AABBQuantizedNoLeafNode* b)
{
        // Dequantize box
        const QuantizedAABB* bb = &b->mAABB;
        const Point Pa(float(bb->mCenter[0]) * mCenterCoeff0.x, float(bb->mCenter[1]) * mCenterCoeff0.y, float(bb->mCenter[2]) * mCenterCoeff0.z);
        const Point ea(float(bb->mExtents[0]) * mExtentsCoeff0.x, float(bb->mExtents[1]) * mExtentsCoeff0.y, float(bb->mExtents[2]) * mExtentsCoeff0.z);

 
        // Modified by S-cubed, Inc.
        //NoleafNode $B$O;HMQ$7$J$$(B
        // Perform triangle-box overlap test
        //      if(!TriBoxOverlap(Pa, ea))      return;

        if(b->HasPosLeaf())     PrimTestIndexTri(b->GetPosPrimitive());
        else                            _CollideBoxTri(b->GetPos());

        if(ContactFound()) return;

        if(b->HasNegLeaf())     PrimTestIndexTri(b->GetNegPrimitive());
        else                            _CollideBoxTri(b->GetNeg());
}



void AABBTreeCollider::_Collide(const AABBQuantizedNoLeafNode* a, const AABBQuantizedNoLeafNode* b)
{
        // Dequantize box A
        const QuantizedAABB* ab = &a->mAABB;
        const Point Pa(float(ab->mCenter[0]) * mCenterCoeff0.x, float(ab->mCenter[1]) * mCenterCoeff0.y, float(ab->mCenter[2]) * mCenterCoeff0.z);
        const Point ea(float(ab->mExtents[0]) * mExtentsCoeff0.x, float(ab->mExtents[1]) * mExtentsCoeff0.y, float(ab->mExtents[2]) * mExtentsCoeff0.z);
        // Dequantize box B
        const QuantizedAABB* bb = &b->mAABB;
        const Point Pb(float(bb->mCenter[0]) * mCenterCoeff1.x, float(bb->mCenter[1]) * mCenterCoeff1.y, float(bb->mCenter[2]) * mCenterCoeff1.z);
        const Point eb(float(bb->mExtents[0]) * mExtentsCoeff1.x, float(bb->mExtents[1]) * mExtentsCoeff1.y, float(bb->mExtents[2]) * mExtentsCoeff1.z);

        // Perform BV-BV overlap test
        if(!BoxBoxOverlap(ea, Pa, eb, Pb))      return;

        // Catch leaf status
        BOOL BHasPosLeaf = b->HasPosLeaf();
        BOOL BHasNegLeaf = b->HasNegLeaf();

        if(a->HasPosLeaf())
        {
                FETCH_LEAF(a->GetPosPrimitive(), mIMesh0, mR0to1, mT0to1)

                if(BHasPosLeaf) PrimTestTriIndex(b->GetPosPrimitive());
                else                    _CollideTriBox(b->GetPos());

                if(ContactFound()) return;

                if(BHasNegLeaf) PrimTestTriIndex(b->GetNegPrimitive());
                else                    _CollideTriBox(b->GetNeg());
        }
        else
        {
                if(BHasPosLeaf)
                {
                        FETCH_LEAF(b->GetPosPrimitive(), mIMesh1, mR1to0, mT1to0)

                        _CollideBoxTri(a->GetPos());
                }
                else _Collide(a->GetPos(), b->GetPos());

                if(ContactFound()) return;

                if(BHasNegLeaf)
                {
                        FETCH_LEAF(b->GetNegPrimitive(), mIMesh1, mR1to0, mT1to0)

                        _CollideBoxTri(a->GetPos());
                }
                else _Collide(a->GetPos(), b->GetNeg());
        }

        if(ContactFound()) return;

        if(a->HasNegLeaf())
        {
                FETCH_LEAF(a->GetNegPrimitive(), mIMesh0, mR0to1, mT0to1)

                if(BHasPosLeaf) PrimTestTriIndex(b->GetPosPrimitive());
                else                    _CollideTriBox(b->GetPos());

                if(ContactFound()) return;

                if(BHasNegLeaf) PrimTestTriIndex(b->GetNegPrimitive());
                else                    _CollideTriBox(b->GetNeg());
        }
        else
        {
                if(BHasPosLeaf)
                {
                        // ### That leaf has possibly already been fetched
                        FETCH_LEAF(b->GetPosPrimitive(), mIMesh1, mR1to0, mT1to0)

                        _CollideBoxTri(a->GetNeg());
                }
                else _Collide(a->GetNeg(), b->GetPos());

                if(ContactFound()) return;

                if(BHasNegLeaf)
                {
                        // ### That leaf has possibly already been fetched
                        FETCH_LEAF(b->GetNegPrimitive(), mIMesh1, mR1to0, mT1to0)

                        _CollideBoxTri(a->GetNeg());
                }
                else _Collide(a->GetNeg(), b->GetNeg());
        }
}