1 files changed, 367 insertions, 0 deletions

diff --git a/libraries/ode-0.9/OPCODE/OPC_BoxPruning.cpp b/libraries/ode-0.9/OPCODE/OPC_BoxPruning.cpp
new file mode 100644
index 0000000..6906160
--- /dev/null
+++ b/libraries/ode-0.9/OPCODE/OPC_BoxPruning.cpp
@@ -0,0 +1,367 @@
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/*
+ *      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;
+}