368 lines
13 KiB
C++
368 lines
13 KiB
C++
|
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
||
|
|
/*
|
||
|
|
* OPCODE - Optimized Collision Detection
|
||
|
|
* Copyright (C) 2001 Pierre Terdiman
|
||
|
|
* Homepage: http://www.codercorner.com/Opcode.htm
|
||
|
|
*/
|
||
|
|
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
||
|
|
|
||
|
|
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
||
|
|
/**
|
||
|
|
* Contains code for box pruning.
|
||
|
|
* \file IceBoxPruning.cpp
|
||
|
|
* \author Pierre Terdiman
|
||
|
|
* \date January, 29, 2000
|
||
|
|
*/
|
||
|
|
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
||
|
|
|
||
|
|
/*
|
||
|
|
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
||
|
|
You could use a complex sweep-and-prune as implemented in I-Collide.
|
||
|
|
You could use a complex hashing scheme as implemented in V-Clip or recently in ODE it seems.
|
||
|
|
You could use a "Recursive Dimensional Clustering" algorithm as implemented in GPG2.
|
||
|
|
|
||
|
|
Or you could use this.
|
||
|
|
Faster ? I don't know. Probably not. It would be a shame. But who knows ?
|
||
|
|
Easier ? Definitely. Enjoy the sheer simplicity.
|
||
|
|
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
||
|
|
*/
|
||
|
|
|
||
|
|
|
||
|
|
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
||
|
|
// Precompiled Header
|
||
|
|
#include "Stdafx.h"
|
||
|
|
|
||
|
|
using namespace Opcode;
|
||
|
|
|
||
|
|
inline_ void FindRunningIndex(udword& index, float* array, udword* sorted, int last, float max)
|
||
|
|
{
|
||
|
|
int First=index;
|
||
|
|
while(First<=last)
|
||
|
|
{
|
||
|
|
index = (First+last)>>1;
|
||
|
|
|
||
|
|
if(max>array[sorted[index]]) First = index+1;
|
||
|
|
else last = index-1;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
// ### could be log(n) !
|
||
|
|
// and maybe use cmp integers
|
||
|
|
|
||
|
|
// InsertionSort has better coherence, RadixSort is better for one-shot queries.
|
||
|
|
#define PRUNING_SORTER RadixSort
|
||
|
|
//#define PRUNING_SORTER InsertionSort
|
||
|
|
|
||
|
|
// Static for coherence
|
||
|
|
static PRUNING_SORTER* gCompletePruningSorter = null;
|
||
|
|
static PRUNING_SORTER* gBipartitePruningSorter0 = null;
|
||
|
|
static PRUNING_SORTER* gBipartitePruningSorter1 = null;
|
||
|
|
inline_ PRUNING_SORTER* GetCompletePruningSorter()
|
||
|
|
{
|
||
|
|
if(!gCompletePruningSorter) gCompletePruningSorter = new PRUNING_SORTER;
|
||
|
|
return gCompletePruningSorter;
|
||
|
|
}
|
||
|
|
inline_ PRUNING_SORTER* GetBipartitePruningSorter0()
|
||
|
|
{
|
||
|
|
if(!gBipartitePruningSorter0) gBipartitePruningSorter0 = new PRUNING_SORTER;
|
||
|
|
return gBipartitePruningSorter0;
|
||
|
|
}
|
||
|
|
inline_ PRUNING_SORTER* GetBipartitePruningSorter1()
|
||
|
|
{
|
||
|
|
if(!gBipartitePruningSorter1) gBipartitePruningSorter1 = new PRUNING_SORTER;
|
||
|
|
return gBipartitePruningSorter1;
|
||
|
|
}
|
||
|
|
void ReleasePruningSorters()
|
||
|
|
{
|
||
|
|
DELETESINGLE(gBipartitePruningSorter1);
|
||
|
|
DELETESINGLE(gBipartitePruningSorter0);
|
||
|
|
DELETESINGLE(gCompletePruningSorter);
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
||
|
|
/**
|
||
|
|
* Bipartite box pruning. Returns a list of overlapping pairs of boxes, each box of the pair belongs to a different set.
|
||
|
|
* \param nb0 [in] number of boxes in the first set
|
||
|
|
* \param array0 [in] array of boxes for the first set
|
||
|
|
* \param nb1 [in] number of boxes in the second set
|
||
|
|
* \param array1 [in] array of boxes for the second set
|
||
|
|
* \param pairs [out] array of overlapping pairs
|
||
|
|
* \param axes [in] projection order (0,2,1 is often best)
|
||
|
|
* \return true if success.
|
||
|
|
*/
|
||
|
|
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
||
|
|
bool Opcode::BipartiteBoxPruning(udword nb0, const AABB** array0, udword nb1, const AABB** array1, Pairs& pairs, const Axes& axes)
|
||
|
|
{
|
||
|
|
// Checkings
|
||
|
|
if(!nb0 || !array0 || !nb1 || !array1) return false;
|
||
|
|
|
||
|
|
// Catch axes
|
||
|
|
udword Axis0 = axes.mAxis0;
|
||
|
|
udword Axis1 = axes.mAxis1;
|
||
|
|
udword Axis2 = axes.mAxis2;
|
||
|
|
|
||
|
|
// Allocate some temporary data
|
||
|
|
float* MinPosList0 = new float[nb0];
|
||
|
|
float* MinPosList1 = new float[nb1];
|
||
|
|
|
||
|
|
// 1) Build main lists using the primary axis
|
||
|
|
for(udword i=0;i<nb0;i++) MinPosList0[i] = array0[i]->GetMin(Axis0);
|
||
|
|
for(udword i=0;i<nb1;i++) MinPosList1[i] = array1[i]->GetMin(Axis0);
|
||
|
|
|
||
|
|
// 2) Sort the lists
|
||
|
|
PRUNING_SORTER* RS0 = GetBipartitePruningSorter0();
|
||
|
|
PRUNING_SORTER* RS1 = GetBipartitePruningSorter1();
|
||
|
|
const udword* Sorted0 = RS0->Sort(MinPosList0, nb0).GetRanks();
|
||
|
|
const udword* Sorted1 = RS1->Sort(MinPosList1, nb1).GetRanks();
|
||
|
|
|
||
|
|
// 3) Prune the lists
|
||
|
|
udword Index0, Index1;
|
||
|
|
|
||
|
|
const udword* const LastSorted0 = &Sorted0[nb0];
|
||
|
|
const udword* const LastSorted1 = &Sorted1[nb1];
|
||
|
|
const udword* RunningAddress0 = Sorted0;
|
||
|
|
const udword* RunningAddress1 = Sorted1;
|
||
|
|
|
||
|
|
while(RunningAddress1<LastSorted1 && Sorted0<LastSorted0)
|
||
|
|
{
|
||
|
|
Index0 = *Sorted0++;
|
||
|
|
|
||
|
|
while(RunningAddress1<LastSorted1 && MinPosList1[*RunningAddress1]<MinPosList0[Index0]) RunningAddress1++;
|
||
|
|
|
||
|
|
const udword* RunningAddress2_1 = RunningAddress1;
|
||
|
|
|
||
|
|
while(RunningAddress2_1<LastSorted1 && MinPosList1[Index1 = *RunningAddress2_1++]<=array0[Index0]->GetMax(Axis0))
|
||
|
|
{
|
||
|
|
if(array0[Index0]->Intersect(*array1[Index1], Axis1))
|
||
|
|
{
|
||
|
|
if(array0[Index0]->Intersect(*array1[Index1], Axis2))
|
||
|
|
{
|
||
|
|
pairs.AddPair(Index0, Index1);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
////
|
||
|
|
|
||
|
|
while(RunningAddress0<LastSorted0 && Sorted1<LastSorted1)
|
||
|
|
{
|
||
|
|
Index0 = *Sorted1++;
|
||
|
|
|
||
|
|
while(RunningAddress0<LastSorted0 && MinPosList0[*RunningAddress0]<=MinPosList1[Index0]) RunningAddress0++;
|
||
|
|
|
||
|
|
const udword* RunningAddress2_0 = RunningAddress0;
|
||
|
|
|
||
|
|
while(RunningAddress2_0<LastSorted0 && MinPosList0[Index1 = *RunningAddress2_0++]<=array1[Index0]->GetMax(Axis0))
|
||
|
|
{
|
||
|
|
if(array0[Index1]->Intersect(*array1[Index0], Axis1))
|
||
|
|
{
|
||
|
|
if(array0[Index1]->Intersect(*array1[Index0], Axis2))
|
||
|
|
{
|
||
|
|
pairs.AddPair(Index1, Index0);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
DELETEARRAY(MinPosList1);
|
||
|
|
DELETEARRAY(MinPosList0);
|
||
|
|
|
||
|
|
return true;
|
||
|
|
}
|
||
|
|
|
||
|
|
#define ORIGINAL_VERSION
|
||
|
|
//#define JOAKIM
|
||
|
|
|
||
|
|
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
||
|
|
/**
|
||
|
|
* Complete box pruning. Returns a list of overlapping pairs of boxes, each box of the pair belongs to the same set.
|
||
|
|
* \param nb [in] number of boxes
|
||
|
|
* \param array [in] array of boxes
|
||
|
|
* \param pairs [out] array of overlapping pairs
|
||
|
|
* \param axes [in] projection order (0,2,1 is often best)
|
||
|
|
* \return true if success.
|
||
|
|
*/
|
||
|
|
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
||
|
|
bool Opcode::CompleteBoxPruning(udword nb, const AABB** array, Pairs& pairs, const Axes& axes)
|
||
|
|
{
|
||
|
|
// Checkings
|
||
|
|
if(!nb || !array) return false;
|
||
|
|
|
||
|
|
// Catch axes
|
||
|
|
udword Axis0 = axes.mAxis0;
|
||
|
|
udword Axis1 = axes.mAxis1;
|
||
|
|
udword Axis2 = axes.mAxis2;
|
||
|
|
|
||
|
|
#ifdef ORIGINAL_VERSION
|
||
|
|
// Allocate some temporary data
|
||
|
|
// float* PosList = new float[nb];
|
||
|
|
float* PosList = new float[nb+1];
|
||
|
|
|
||
|
|
// 1) Build main list using the primary axis
|
||
|
|
for(udword i=0;i<nb;i++) PosList[i] = array[i]->GetMin(Axis0);
|
||
|
|
PosList[nb++] = MAX_FLOAT;
|
||
|
|
|
||
|
|
// 2) Sort the list
|
||
|
|
PRUNING_SORTER* RS = GetCompletePruningSorter();
|
||
|
|
const udword* Sorted = RS->Sort(PosList, nb).GetRanks();
|
||
|
|
|
||
|
|
// 3) Prune the list
|
||
|
|
const udword* const LastSorted = &Sorted[nb];
|
||
|
|
const udword* RunningAddress = Sorted;
|
||
|
|
udword Index0, Index1;
|
||
|
|
while(RunningAddress<LastSorted && Sorted<LastSorted)
|
||
|
|
{
|
||
|
|
Index0 = *Sorted++;
|
||
|
|
|
||
|
|
// while(RunningAddress<LastSorted && PosList[*RunningAddress++]<PosList[Index0]);
|
||
|
|
while(PosList[*RunningAddress++]<PosList[Index0]);
|
||
|
|
|
||
|
|
if(RunningAddress<LastSorted)
|
||
|
|
{
|
||
|
|
const udword* RunningAddress2 = RunningAddress;
|
||
|
|
|
||
|
|
// while(RunningAddress2<LastSorted && PosList[Index1 = *RunningAddress2++]<=array[Index0]->GetMax(Axis0))
|
||
|
|
while(PosList[Index1 = *RunningAddress2++]<=array[Index0]->GetMax(Axis0))
|
||
|
|
{
|
||
|
|
// if(Index0!=Index1)
|
||
|
|
// {
|
||
|
|
if(array[Index0]->Intersect(*array[Index1], Axis1))
|
||
|
|
{
|
||
|
|
if(array[Index0]->Intersect(*array[Index1], Axis2))
|
||
|
|
{
|
||
|
|
pairs.AddPair(Index0, Index1);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
// }
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
DELETEARRAY(PosList);
|
||
|
|
#endif
|
||
|
|
|
||
|
|
#ifdef JOAKIM
|
||
|
|
// Allocate some temporary data
|
||
|
|
// float* PosList = new float[nb];
|
||
|
|
float* MinList = new float[nb+1];
|
||
|
|
|
||
|
|
// 1) Build main list using the primary axis
|
||
|
|
for(udword i=0;i<nb;i++) MinList[i] = array[i]->GetMin(Axis0);
|
||
|
|
MinList[nb] = MAX_FLOAT;
|
||
|
|
|
||
|
|
// 2) Sort the list
|
||
|
|
PRUNING_SORTER* RS = GetCompletePruningSorter();
|
||
|
|
udword* Sorted = RS->Sort(MinList, nb+1).GetRanks();
|
||
|
|
|
||
|
|
// 3) Prune the list
|
||
|
|
// const udword* const LastSorted = &Sorted[nb];
|
||
|
|
// const udword* const LastSorted = &Sorted[nb-1];
|
||
|
|
const udword* RunningAddress = Sorted;
|
||
|
|
udword Index0, Index1;
|
||
|
|
|
||
|
|
// while(RunningAddress<LastSorted && Sorted<LastSorted)
|
||
|
|
// while(RunningAddress<LastSorted)
|
||
|
|
while(RunningAddress<&Sorted[nb])
|
||
|
|
// while(Sorted<LastSorted)
|
||
|
|
{
|
||
|
|
// Index0 = *Sorted++;
|
||
|
|
Index0 = *RunningAddress++;
|
||
|
|
|
||
|
|
// while(RunningAddress<LastSorted && PosList[*RunningAddress++]<PosList[Index0]);
|
||
|
|
// while(PosList[*RunningAddress++]<PosList[Index0]);
|
||
|
|
//RunningAddress = Sorted;
|
||
|
|
// if(RunningAddress<LastSorted)
|
||
|
|
{
|
||
|
|
const udword* RunningAddress2 = RunningAddress;
|
||
|
|
|
||
|
|
// while(RunningAddress2<LastSorted && PosList[Index1 = *RunningAddress2++]<=array[Index0]->GetMax(Axis0))
|
||
|
|
|
||
|
|
// float CurrentMin = array[Index0]->GetMin(Axis0);
|
||
|
|
float CurrentMax = array[Index0]->GetMax(Axis0);
|
||
|
|
|
||
|
|
while(MinList[Index1 = *RunningAddress2] <= CurrentMax)
|
||
|
|
// while(PosList[Index1 = *RunningAddress] <= CurrentMax)
|
||
|
|
{
|
||
|
|
// if(Index0!=Index1)
|
||
|
|
// {
|
||
|
|
if(array[Index0]->Intersect(*array[Index1], Axis1))
|
||
|
|
{
|
||
|
|
if(array[Index0]->Intersect(*array[Index1], Axis2))
|
||
|
|
{
|
||
|
|
pairs.AddPair(Index0, Index1);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
// }
|
||
|
|
|
||
|
|
RunningAddress2++;
|
||
|
|
// RunningAddress++;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
DELETEARRAY(MinList);
|
||
|
|
#endif
|
||
|
|
|
||
|
|
return true;
|
||
|
|
}
|
||
|
|
|
||
|
|
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
||
|
|
// Brute-force versions are kept:
|
||
|
|
// - to check the optimized versions return the correct list of intersections
|
||
|
|
// - to check the speed of the optimized code against the brute-force one
|
||
|
|
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
||
|
|
|
||
|
|
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
||
|
|
/**
|
||
|
|
* Brute-force bipartite box pruning. Returns a list of overlapping pairs of boxes, each box of the pair belongs to a different set.
|
||
|
|
* \param nb0 [in] number of boxes in the first set
|
||
|
|
* \param array0 [in] array of boxes for the first set
|
||
|
|
* \param nb1 [in] number of boxes in the second set
|
||
|
|
* \param array1 [in] array of boxes for the second set
|
||
|
|
* \param pairs [out] array of overlapping pairs
|
||
|
|
* \return true if success.
|
||
|
|
*/
|
||
|
|
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
||
|
|
bool Opcode::BruteForceBipartiteBoxTest(udword nb0, const AABB** array0, udword nb1, const AABB** array1, Pairs& pairs)
|
||
|
|
{
|
||
|
|
// Checkings
|
||
|
|
if(!nb0 || !array0 || !nb1 || !array1) return false;
|
||
|
|
|
||
|
|
// Brute-force nb0*nb1 overlap tests
|
||
|
|
for(udword i=0;i<nb0;i++)
|
||
|
|
{
|
||
|
|
for(udword j=0;j<nb1;j++)
|
||
|
|
{
|
||
|
|
if(array0[i]->Intersect(*array1[j])) pairs.AddPair(i, j);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
return true;
|
||
|
|
}
|
||
|
|
|
||
|
|
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
||
|
|
/**
|
||
|
|
* Complete box pruning. Returns a list of overlapping pairs of boxes, each box of the pair belongs to the same set.
|
||
|
|
* \param nb [in] number of boxes
|
||
|
|
* \param array [in] array of boxes
|
||
|
|
* \param pairs [out] array of overlapping pairs
|
||
|
|
* \return true if success.
|
||
|
|
*/
|
||
|
|
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
||
|
|
bool Opcode::BruteForceCompleteBoxTest(udword nb, const AABB** array, Pairs& pairs)
|
||
|
|
{
|
||
|
|
// Checkings
|
||
|
|
if(!nb || !array) return false;
|
||
|
|
|
||
|
|
// Brute-force n(n-1)/2 overlap tests
|
||
|
|
for(udword i=0;i<nb;i++)
|
||
|
|
{
|
||
|
|
for(udword j=i+1;j<nb;j++)
|
||
|
|
{
|
||
|
|
if(array[i]->Intersect(*array[j])) pairs.AddPair(i, j);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
return true;
|
||
|
|
}
|