| 1 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
|---|
| 2 | /* |
|---|
| 3 | * OPCODE - Optimized Collision Detection |
|---|
| 4 | * Copyright (C) 2001 Pierre Terdiman |
|---|
| 5 | * Homepage: http://www.codercorner.com/Opcode.htm |
|---|
| 6 | */ |
|---|
| 7 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
|---|
| 8 | |
|---|
| 9 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
|---|
| 10 | /** |
|---|
| 11 | * Contains code for box pruning. |
|---|
| 12 | * \file IceBoxPruning.cpp |
|---|
| 13 | * \author Pierre Terdiman |
|---|
| 14 | * \date January, 29, 2000 |
|---|
| 15 | */ |
|---|
| 16 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
|---|
| 17 | |
|---|
| 18 | /* |
|---|
| 19 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
|---|
| 20 | You could use a complex sweep-and-prune as implemented in I-Collide. |
|---|
| 21 | You could use a complex hashing scheme as implemented in V-Clip or recently in ODE it seems. |
|---|
| 22 | You could use a "Recursive Dimensional Clustering" algorithm as implemented in GPG2. |
|---|
| 23 | |
|---|
| 24 | Or you could use this. |
|---|
| 25 | Faster ? I don't know. Probably not. It would be a shame. But who knows ? |
|---|
| 26 | Easier ? Definitely. Enjoy the sheer simplicity. |
|---|
| 27 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
|---|
| 28 | */ |
|---|
| 29 | |
|---|
| 30 | |
|---|
| 31 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
|---|
| 32 | // Precompiled Header |
|---|
| 33 | #include "Stdafx.h" |
|---|
| 34 | |
|---|
| 35 | using namespace Opcode; |
|---|
| 36 | |
|---|
| 37 | inline_ void FindRunningIndex(udword& index, float* array, udword* sorted, int last, float max) |
|---|
| 38 | { |
|---|
| 39 | int First=index; |
|---|
| 40 | while(First<=last) |
|---|
| 41 | { |
|---|
| 42 | index = (First+last)>>1; |
|---|
| 43 | |
|---|
| 44 | if(max>array[sorted[index]]) First = index+1; |
|---|
| 45 | else last = index-1; |
|---|
| 46 | } |
|---|
| 47 | } |
|---|
| 48 | // ### could be log(n) ! |
|---|
| 49 | // and maybe use cmp integers |
|---|
| 50 | |
|---|
| 51 | // InsertionSort has better coherence, RadixSort is better for one-shot queries. |
|---|
| 52 | #define PRUNING_SORTER RadixSort |
|---|
| 53 | //#define PRUNING_SORTER InsertionSort |
|---|
| 54 | |
|---|
| 55 | // Static for coherence |
|---|
| 56 | static PRUNING_SORTER* gCompletePruningSorter = null; |
|---|
| 57 | static PRUNING_SORTER* gBipartitePruningSorter0 = null; |
|---|
| 58 | static PRUNING_SORTER* gBipartitePruningSorter1 = null; |
|---|
| 59 | inline_ PRUNING_SORTER* GetCompletePruningSorter() |
|---|
| 60 | { |
|---|
| 61 | if(!gCompletePruningSorter) gCompletePruningSorter = new PRUNING_SORTER; |
|---|
| 62 | return gCompletePruningSorter; |
|---|
| 63 | } |
|---|
| 64 | inline_ PRUNING_SORTER* GetBipartitePruningSorter0() |
|---|
| 65 | { |
|---|
| 66 | if(!gBipartitePruningSorter0) gBipartitePruningSorter0 = new PRUNING_SORTER; |
|---|
| 67 | return gBipartitePruningSorter0; |
|---|
| 68 | } |
|---|
| 69 | inline_ PRUNING_SORTER* GetBipartitePruningSorter1() |
|---|
| 70 | { |
|---|
| 71 | if(!gBipartitePruningSorter1) gBipartitePruningSorter1 = new PRUNING_SORTER; |
|---|
| 72 | return gBipartitePruningSorter1; |
|---|
| 73 | } |
|---|
| 74 | void ReleasePruningSorters() |
|---|
| 75 | { |
|---|
| 76 | DELETESINGLE(gBipartitePruningSorter1); |
|---|
| 77 | DELETESINGLE(gBipartitePruningSorter0); |
|---|
| 78 | DELETESINGLE(gCompletePruningSorter); |
|---|
| 79 | } |
|---|
| 80 | |
|---|
| 81 | |
|---|
| 82 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
|---|
| 83 | /** |
|---|
| 84 | * Bipartite box pruning. Returns a list of overlapping pairs of boxes, each box of the pair belongs to a different set. |
|---|
| 85 | * \param nb0 [in] number of boxes in the first set |
|---|
| 86 | * \param array0 [in] array of boxes for the first set |
|---|
| 87 | * \param nb1 [in] number of boxes in the second set |
|---|
| 88 | * \param array1 [in] array of boxes for the second set |
|---|
| 89 | * \param pairs [out] array of overlapping pairs |
|---|
| 90 | * \param axes [in] projection order (0,2,1 is often best) |
|---|
| 91 | * \return true if success. |
|---|
| 92 | */ |
|---|
| 93 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
|---|
| 94 | bool Opcode::BipartiteBoxPruning(udword nb0, const AABB** array0, udword nb1, const AABB** array1, Pairs& pairs, const Axes& axes) |
|---|
| 95 | { |
|---|
| 96 | // Checkings |
|---|
| 97 | if(!nb0 || !array0 || !nb1 || !array1) return false; |
|---|
| 98 | |
|---|
| 99 | // Catch axes |
|---|
| 100 | udword Axis0 = axes.mAxis0; |
|---|
| 101 | udword Axis1 = axes.mAxis1; |
|---|
| 102 | udword Axis2 = axes.mAxis2; |
|---|
| 103 | |
|---|
| 104 | // Allocate some temporary data |
|---|
| 105 | float* MinPosList0 = new float[nb0]; |
|---|
| 106 | float* MinPosList1 = new float[nb1]; |
|---|
| 107 | |
|---|
| 108 | // 1) Build main lists using the primary axis |
|---|
| 109 | for(udword i=0;i<nb0;i++) MinPosList0[i] = array0[i]->GetMin(Axis0); |
|---|
| 110 | for(udword i=0;i<nb1;i++) MinPosList1[i] = array1[i]->GetMin(Axis0); |
|---|
| 111 | |
|---|
| 112 | // 2) Sort the lists |
|---|
| 113 | PRUNING_SORTER* RS0 = GetBipartitePruningSorter0(); |
|---|
| 114 | PRUNING_SORTER* RS1 = GetBipartitePruningSorter1(); |
|---|
| 115 | const udword* Sorted0 = RS0->Sort(MinPosList0, nb0).GetRanks(); |
|---|
| 116 | const udword* Sorted1 = RS1->Sort(MinPosList1, nb1).GetRanks(); |
|---|
| 117 | |
|---|
| 118 | // 3) Prune the lists |
|---|
| 119 | udword Index0, Index1; |
|---|
| 120 | |
|---|
| 121 | const udword* const LastSorted0 = &Sorted0[nb0]; |
|---|
| 122 | const udword* const LastSorted1 = &Sorted1[nb1]; |
|---|
| 123 | const udword* RunningAddress0 = Sorted0; |
|---|
| 124 | const udword* RunningAddress1 = Sorted1; |
|---|
| 125 | |
|---|
| 126 | while(RunningAddress1<LastSorted1 && Sorted0<LastSorted0) |
|---|
| 127 | { |
|---|
| 128 | Index0 = *Sorted0++; |
|---|
| 129 | |
|---|
| 130 | while(RunningAddress1<LastSorted1 && MinPosList1[*RunningAddress1]<MinPosList0[Index0]) RunningAddress1++; |
|---|
| 131 | |
|---|
| 132 | const udword* RunningAddress2_1 = RunningAddress1; |
|---|
| 133 | |
|---|
| 134 | while(RunningAddress2_1<LastSorted1 && MinPosList1[Index1 = *RunningAddress2_1++]<=array0[Index0]->GetMax(Axis0)) |
|---|
| 135 | { |
|---|
| 136 | if(array0[Index0]->Intersect(*array1[Index1], Axis1)) |
|---|
| 137 | { |
|---|
| 138 | if(array0[Index0]->Intersect(*array1[Index1], Axis2)) |
|---|
| 139 | { |
|---|
| 140 | pairs.AddPair(Index0, Index1); |
|---|
| 141 | } |
|---|
| 142 | } |
|---|
| 143 | } |
|---|
| 144 | } |
|---|
| 145 | |
|---|
| 146 | //// |
|---|
| 147 | |
|---|
| 148 | while(RunningAddress0<LastSorted0 && Sorted1<LastSorted1) |
|---|
| 149 | { |
|---|
| 150 | Index0 = *Sorted1++; |
|---|
| 151 | |
|---|
| 152 | while(RunningAddress0<LastSorted0 && MinPosList0[*RunningAddress0]<=MinPosList1[Index0]) RunningAddress0++; |
|---|
| 153 | |
|---|
| 154 | const udword* RunningAddress2_0 = RunningAddress0; |
|---|
| 155 | |
|---|
| 156 | while(RunningAddress2_0<LastSorted0 && MinPosList0[Index1 = *RunningAddress2_0++]<=array1[Index0]->GetMax(Axis0)) |
|---|
| 157 | { |
|---|
| 158 | if(array0[Index1]->Intersect(*array1[Index0], Axis1)) |
|---|
| 159 | { |
|---|
| 160 | if(array0[Index1]->Intersect(*array1[Index0], Axis2)) |
|---|
| 161 | { |
|---|
| 162 | pairs.AddPair(Index1, Index0); |
|---|
| 163 | } |
|---|
| 164 | } |
|---|
| 165 | |
|---|
| 166 | } |
|---|
| 167 | } |
|---|
| 168 | |
|---|
| 169 | DELETEARRAY(MinPosList1); |
|---|
| 170 | DELETEARRAY(MinPosList0); |
|---|
| 171 | |
|---|
| 172 | return true; |
|---|
| 173 | } |
|---|
| 174 | |
|---|
| 175 | #define ORIGINAL_VERSION |
|---|
| 176 | //#define JOAKIM |
|---|
| 177 | |
|---|
| 178 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
|---|
| 179 | /** |
|---|
| 180 | * Complete box pruning. Returns a list of overlapping pairs of boxes, each box of the pair belongs to the same set. |
|---|
| 181 | * \param nb [in] number of boxes |
|---|
| 182 | * \param array [in] array of boxes |
|---|
| 183 | * \param pairs [out] array of overlapping pairs |
|---|
| 184 | * \param axes [in] projection order (0,2,1 is often best) |
|---|
| 185 | * \return true if success. |
|---|
| 186 | */ |
|---|
| 187 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
|---|
| 188 | bool Opcode::CompleteBoxPruning(udword nb, const AABB** array, Pairs& pairs, const Axes& axes) |
|---|
| 189 | { |
|---|
| 190 | // Checkings |
|---|
| 191 | if(!nb || !array) return false; |
|---|
| 192 | |
|---|
| 193 | // Catch axes |
|---|
| 194 | udword Axis0 = axes.mAxis0; |
|---|
| 195 | udword Axis1 = axes.mAxis1; |
|---|
| 196 | udword Axis2 = axes.mAxis2; |
|---|
| 197 | |
|---|
| 198 | #ifdef ORIGINAL_VERSION |
|---|
| 199 | // Allocate some temporary data |
|---|
| 200 | // float* PosList = new float[nb]; |
|---|
| 201 | float* PosList = new float[nb+1]; |
|---|
| 202 | |
|---|
| 203 | // 1) Build main list using the primary axis |
|---|
| 204 | for(udword i=0;i<nb;i++) PosList[i] = array[i]->GetMin(Axis0); |
|---|
| 205 | PosList[nb++] = MAX_FLOAT; |
|---|
| 206 | |
|---|
| 207 | // 2) Sort the list |
|---|
| 208 | PRUNING_SORTER* RS = GetCompletePruningSorter(); |
|---|
| 209 | const udword* Sorted = RS->Sort(PosList, nb).GetRanks(); |
|---|
| 210 | |
|---|
| 211 | // 3) Prune the list |
|---|
| 212 | const udword* const LastSorted = &Sorted[nb]; |
|---|
| 213 | const udword* RunningAddress = Sorted; |
|---|
| 214 | udword Index0, Index1; |
|---|
| 215 | while(RunningAddress<LastSorted && Sorted<LastSorted) |
|---|
| 216 | { |
|---|
| 217 | Index0 = *Sorted++; |
|---|
| 218 | |
|---|
| 219 | // while(RunningAddress<LastSorted && PosList[*RunningAddress++]<PosList[Index0]); |
|---|
| 220 | while(PosList[*RunningAddress++]<PosList[Index0]); |
|---|
| 221 | |
|---|
| 222 | if(RunningAddress<LastSorted) |
|---|
| 223 | { |
|---|
| 224 | const udword* RunningAddress2 = RunningAddress; |
|---|
| 225 | |
|---|
| 226 | // while(RunningAddress2<LastSorted && PosList[Index1 = *RunningAddress2++]<=array[Index0]->GetMax(Axis0)) |
|---|
| 227 | while(PosList[Index1 = *RunningAddress2++]<=array[Index0]->GetMax(Axis0)) |
|---|
| 228 | { |
|---|
| 229 | // if(Index0!=Index1) |
|---|
| 230 | // { |
|---|
| 231 | if(array[Index0]->Intersect(*array[Index1], Axis1)) |
|---|
| 232 | { |
|---|
| 233 | if(array[Index0]->Intersect(*array[Index1], Axis2)) |
|---|
| 234 | { |
|---|
| 235 | pairs.AddPair(Index0, Index1); |
|---|
| 236 | } |
|---|
| 237 | } |
|---|
| 238 | // } |
|---|
| 239 | } |
|---|
| 240 | } |
|---|
| 241 | } |
|---|
| 242 | |
|---|
| 243 | DELETEARRAY(PosList); |
|---|
| 244 | #endif |
|---|
| 245 | |
|---|
| 246 | #ifdef JOAKIM |
|---|
| 247 | // Allocate some temporary data |
|---|
| 248 | // float* PosList = new float[nb]; |
|---|
| 249 | float* MinList = new float[nb+1]; |
|---|
| 250 | |
|---|
| 251 | // 1) Build main list using the primary axis |
|---|
| 252 | for(udword i=0;i<nb;i++) MinList[i] = array[i]->GetMin(Axis0); |
|---|
| 253 | MinList[nb] = MAX_FLOAT; |
|---|
| 254 | |
|---|
| 255 | // 2) Sort the list |
|---|
| 256 | PRUNING_SORTER* RS = GetCompletePruningSorter(); |
|---|
| 257 | udword* Sorted = RS->Sort(MinList, nb+1).GetRanks(); |
|---|
| 258 | |
|---|
| 259 | // 3) Prune the list |
|---|
| 260 | // const udword* const LastSorted = &Sorted[nb]; |
|---|
| 261 | // const udword* const LastSorted = &Sorted[nb-1]; |
|---|
| 262 | const udword* RunningAddress = Sorted; |
|---|
| 263 | udword Index0, Index1; |
|---|
| 264 | |
|---|
| 265 | // while(RunningAddress<LastSorted && Sorted<LastSorted) |
|---|
| 266 | // while(RunningAddress<LastSorted) |
|---|
| 267 | while(RunningAddress<&Sorted[nb]) |
|---|
| 268 | // while(Sorted<LastSorted) |
|---|
| 269 | { |
|---|
| 270 | // Index0 = *Sorted++; |
|---|
| 271 | Index0 = *RunningAddress++; |
|---|
| 272 | |
|---|
| 273 | // while(RunningAddress<LastSorted && PosList[*RunningAddress++]<PosList[Index0]); |
|---|
| 274 | // while(PosList[*RunningAddress++]<PosList[Index0]); |
|---|
| 275 | //RunningAddress = Sorted; |
|---|
| 276 | // if(RunningAddress<LastSorted) |
|---|
| 277 | { |
|---|
| 278 | const udword* RunningAddress2 = RunningAddress; |
|---|
| 279 | |
|---|
| 280 | // while(RunningAddress2<LastSorted && PosList[Index1 = *RunningAddress2++]<=array[Index0]->GetMax(Axis0)) |
|---|
| 281 | |
|---|
| 282 | // float CurrentMin = array[Index0]->GetMin(Axis0); |
|---|
| 283 | float CurrentMax = array[Index0]->GetMax(Axis0); |
|---|
| 284 | |
|---|
| 285 | while(MinList[Index1 = *RunningAddress2] <= CurrentMax) |
|---|
| 286 | // while(PosList[Index1 = *RunningAddress] <= CurrentMax) |
|---|
| 287 | { |
|---|
| 288 | // if(Index0!=Index1) |
|---|
| 289 | // { |
|---|
| 290 | if(array[Index0]->Intersect(*array[Index1], Axis1)) |
|---|
| 291 | { |
|---|
| 292 | if(array[Index0]->Intersect(*array[Index1], Axis2)) |
|---|
| 293 | { |
|---|
| 294 | pairs.AddPair(Index0, Index1); |
|---|
| 295 | } |
|---|
| 296 | } |
|---|
| 297 | // } |
|---|
| 298 | |
|---|
| 299 | RunningAddress2++; |
|---|
| 300 | // RunningAddress++; |
|---|
| 301 | } |
|---|
| 302 | } |
|---|
| 303 | } |
|---|
| 304 | |
|---|
| 305 | DELETEARRAY(MinList); |
|---|
| 306 | #endif |
|---|
| 307 | |
|---|
| 308 | return true; |
|---|
| 309 | } |
|---|
| 310 | |
|---|
| 311 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
|---|
| 312 | // Brute-force versions are kept: |
|---|
| 313 | // - to check the optimized versions return the correct list of intersections |
|---|
| 314 | // - to check the speed of the optimized code against the brute-force one |
|---|
| 315 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
|---|
| 316 | |
|---|
| 317 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
|---|
| 318 | /** |
|---|
| 319 | * Brute-force bipartite box pruning. Returns a list of overlapping pairs of boxes, each box of the pair belongs to a different set. |
|---|
| 320 | * \param nb0 [in] number of boxes in the first set |
|---|
| 321 | * \param array0 [in] array of boxes for the first set |
|---|
| 322 | * \param nb1 [in] number of boxes in the second set |
|---|
| 323 | * \param array1 [in] array of boxes for the second set |
|---|
| 324 | * \param pairs [out] array of overlapping pairs |
|---|
| 325 | * \return true if success. |
|---|
| 326 | */ |
|---|
| 327 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
|---|
| 328 | bool Opcode::BruteForceBipartiteBoxTest(udword nb0, const AABB** array0, udword nb1, const AABB** array1, Pairs& pairs) |
|---|
| 329 | { |
|---|
| 330 | // Checkings |
|---|
| 331 | if(!nb0 || !array0 || !nb1 || !array1) return false; |
|---|
| 332 | |
|---|
| 333 | // Brute-force nb0*nb1 overlap tests |
|---|
| 334 | for(udword i=0;i<nb0;i++) |
|---|
| 335 | { |
|---|
| 336 | for(udword j=0;j<nb1;j++) |
|---|
| 337 | { |
|---|
| 338 | if(array0[i]->Intersect(*array1[j])) pairs.AddPair(i, j); |
|---|
| 339 | } |
|---|
| 340 | } |
|---|
| 341 | return true; |
|---|
| 342 | } |
|---|
| 343 | |
|---|
| 344 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
|---|
| 345 | /** |
|---|
| 346 | * Complete box pruning. Returns a list of overlapping pairs of boxes, each box of the pair belongs to the same set. |
|---|
| 347 | * \param nb [in] number of boxes |
|---|
| 348 | * \param array [in] array of boxes |
|---|
| 349 | * \param pairs [out] array of overlapping pairs |
|---|
| 350 | * \return true if success. |
|---|
| 351 | */ |
|---|
| 352 | /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// |
|---|
| 353 | bool Opcode::BruteForceCompleteBoxTest(udword nb, const AABB** array, Pairs& pairs) |
|---|
| 354 | { |
|---|
| 355 | // Checkings |
|---|
| 356 | if(!nb || !array) return false; |
|---|
| 357 | |
|---|
| 358 | // Brute-force n(n-1)/2 overlap tests |
|---|
| 359 | for(udword i=0;i<nb;i++) |
|---|
| 360 | { |
|---|
| 361 | for(udword j=i+1;j<nb;j++) |
|---|
| 362 | { |
|---|
| 363 | if(array[i]->Intersect(*array[j])) pairs.AddPair(i, j); |
|---|
| 364 | } |
|---|
| 365 | } |
|---|
| 366 | return true; |
|---|
| 367 | } |
|---|
