Extras/CDTestFramework/Opcode/OPC_OptimizedTree.cpp

   1 /*
   2  *      OPCODE - Optimized Collision Detection
   3  * http://www.codercorner.com/Opcode.htm
   4  *
   5  * Copyright (c) 2001-2008 Pierre Terdiman,  pierre@codercorner.com
   6
   7 This software is provided 'as-is', without any express or implied warranty.
   8 In no event will the authors be held liable for any damages arising from the use of this software.
   9 Permission is granted to anyone to use this software for any purpose,
  10 including commercial applications, and to alter it and redistribute it freely,
  11 subject to the following restrictions:
  12
  13 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
  14 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
  15 3. This notice may not be removed or altered from any source distribution.
  16 */
  17
  18 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  19 /**
  20  *      Contains code for optimized trees. Implements 4 trees:
  21  *      - normal
  22  *      - no leaf
  23  *      - quantized
  24  *      - no leaf / quantized
  25  *
  26  *      \file           OPC_OptimizedTree.cpp
  27  *      \author         Pierre Terdiman
  28  *      \date           March, 20, 2001
  29  */
  30 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  31
  32 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  33 /**
  34  *      A standard AABB tree.
  35  *
  36  *      \class          AABBCollisionTree
  37  *      \author         Pierre Terdiman
  38  *      \version        1.3
  39  *      \date           March, 20, 2001
  40 */
  41 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  42
  43 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  44 /**
  45  *      A no-leaf AABB tree.
  46  *
  47  *      \class          AABBNoLeafTree
  48  *      \author         Pierre Terdiman
  49  *      \version        1.3
  50  *      \date           March, 20, 2001
  51 */
  52 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  53
  54 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  55 /**
  56  *      A quantized AABB tree.
  57  *
  58  *      \class          AABBQuantizedTree
  59  *      \author         Pierre Terdiman
  60  *      \version        1.3
  61  *      \date           March, 20, 2001
  62 */
  63 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  64
  65 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  66 /**
  67  *      A quantized no-leaf AABB tree.
  68  *
  69  *      \class          AABBQuantizedNoLeafTree
  70  *      \author         Pierre Terdiman
  71  *      \version        1.3
  72  *      \date           March, 20, 2001
  73 */
  74 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  75
  76 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  77 // Precompiled Header
  78 #include "Stdafx.h"
  79
  80 using namespace Opcode;
  81
  82 //! Compilation flag:
  83 //! - true to fix quantized boxes (i.e. make sure they enclose the original ones)
  84 //! - false to see the effects of quantization errors (faster, but wrong results in some cases)
  85 static bool gFixQuantized = true;
  86
  87 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  88 /**
  89  *      Builds an implicit tree from a standard one. An implicit tree is a complete tree (2*N-1 nodes) whose negative
  90  *      box pointers and primitive pointers have been made implicit, hence packing 3 pointers in one.
  91  *
  92  *      Layout for implicit trees:
  93  *      Node:
  94  *                      - box
  95  *                      - data (32-bits value)
  96  *
  97  *      if data's LSB = 1 =>    remaining bits are a primitive pointer
  98  *      else                                    remaining bits are a P-node pointer, and N = P + 1
  99  *
 100  *      \relates        AABBCollisionNode
 101  *      \fn                     _BuildCollisionTree(AABBCollisionNode* linear, const udword box_id, udword& current_id, const AABBTreeNode* current_node)
 102  *      \param          linear                  [in] base address of destination nodes
 103  *      \param          box_id                  [in] index of destination node
 104  *      \param          current_id              [in] current running index
 105  *      \param          current_node    [in] current node from input tree
 106  */
 107 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 108 static void _BuildCollisionTree(AABBCollisionNode* linear, const udword box_id, udword& current_id, const AABBTreeNode* current_node)
 109 {
 110         // Current node from input tree is "current_node". Must be flattened into "linear[boxid]".
 111
 112         // Store the AABB
 113         current_node->GetAABB()->GetCenter(linear[box_id].mAABB.mCenter);
 114         current_node->GetAABB()->GetExtents(linear[box_id].mAABB.mExtents);
 115         // Store remaining info
 116         if(current_node->IsLeaf())
 117         {
 118                 // The input tree must be complete => i.e. one primitive/leaf
 119                 ASSERT(current_node->GetNbPrimitives()==1);
 120                 // Get the primitive index from the input tree
 121                 udword PrimitiveIndex = current_node->GetPrimitives()[0];
 122                 // Setup box data as the primitive index, marked as leaf
 123                 linear[box_id].mData = (PrimitiveIndex<<1)|1;
 124         }
 125         else
 126         {
 127                 // To make the negative one implicit, we must store P and N in successive order
 128                 udword PosID = current_id++;    // Get a new id for positive child
 129                 udword NegID = current_id++;    // Get a new id for negative child
 130                 // Setup box data as the forthcoming new P pointer
 131                 linear[box_id].mData = (udword)&linear[PosID];
 132                 // Make sure it's not marked as leaf
 133                 ASSERT(!(linear[box_id].mData&1));
 134                 // Recurse with new IDs
 135                 _BuildCollisionTree(linear, PosID, current_id, current_node->GetPos());
 136                 _BuildCollisionTree(linear, NegID, current_id, current_node->GetNeg());
 137         }
 138 }
 139
 140 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 141 /**
 142  *      Builds a "no-leaf" tree from a standard one. This is a tree whose leaf nodes have been removed.
 143  *
 144  *      Layout for no-leaf trees:
 145  *
 146  *      Node:
 147  *                      - box
 148  *                      - P pointer => a node (LSB=0) or a primitive (LSB=1)
 149  *                      - N pointer => a node (LSB=0) or a primitive (LSB=1)
 150  *
 151  *      \relates        AABBNoLeafNode
 152  *      \fn                     _BuildNoLeafTree(AABBNoLeafNode* linear, const udword box_id, udword& current_id, const AABBTreeNode* current_node)
 153  *      \param          linear                  [in] base address of destination nodes
 154  *      \param          box_id                  [in] index of destination node
 155  *      \param          current_id              [in] current running index
 156  *      \param          current_node    [in] current node from input tree
 157  */
 158 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 159 static void _BuildNoLeafTree(AABBNoLeafNode* linear, const udword box_id, udword& current_id, const AABBTreeNode* current_node)
 160 {
 161         const AABBTreeNode* P = current_node->GetPos();
 162         const AABBTreeNode* N = current_node->GetNeg();
 163         // Leaf nodes here?!
 164         ASSERT(P);
 165         ASSERT(N);
 166         // Internal node => keep the box
 167         current_node->GetAABB()->GetCenter(linear[box_id].mAABB.mCenter);
 168         current_node->GetAABB()->GetExtents(linear[box_id].mAABB.mExtents);
 169
 170         if(P->IsLeaf())
 171         {
 172                 // The input tree must be complete => i.e. one primitive/leaf
 173                 ASSERT(P->GetNbPrimitives()==1);
 174                 // Get the primitive index from the input tree
 175                 udword PrimitiveIndex = P->GetPrimitives()[0];
 176                 // Setup prev box data as the primitive index, marked as leaf
 177                 linear[box_id].mPosData = (PrimitiveIndex<<1)|1;
 178         }
 179         else
 180         {
 181                 // Get a new id for positive child
 182                 udword PosID = current_id++;
 183                 // Setup box data
 184                 linear[box_id].mPosData = (udword)&linear[PosID];
 185                 // Make sure it's not marked as leaf
 186                 ASSERT(!(linear[box_id].mPosData&1));
 187                 // Recurse
 188                 _BuildNoLeafTree(linear, PosID, current_id, P);
 189         }
 190
 191         if(N->IsLeaf())
 192         {
 193                 // The input tree must be complete => i.e. one primitive/leaf
 194                 ASSERT(N->GetNbPrimitives()==1);
 195                 // Get the primitive index from the input tree
 196                 udword PrimitiveIndex = N->GetPrimitives()[0];
 197                 // Setup prev box data as the primitive index, marked as leaf
 198                 linear[box_id].mNegData = (PrimitiveIndex<<1)|1;
 199         }
 200         else
 201         {
 202                 // Get a new id for negative child
 203                 udword NegID = current_id++;
 204                 // Setup box data
 205                 linear[box_id].mNegData = (udword)&linear[NegID];
 206                 // Make sure it's not marked as leaf
 207                 ASSERT(!(linear[box_id].mNegData&1));
 208                 // Recurse
 209                 _BuildNoLeafTree(linear, NegID, current_id, N);
 210         }
 211 }
 212
 213 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 214 /**
 215  *      Constructor.
 216  */
 217 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 218 AABBCollisionTree::AABBCollisionTree() : mNodes(null)
 219 {
 220 }
 221
 222 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 223 /**
 224  *      Destructor.
 225  */
 226 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 227 AABBCollisionTree::~AABBCollisionTree()
 228 {
 229         DELETEARRAY(mNodes);
 230 }
 231
 232 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 233 /**
 234  *      Builds the collision tree from a generic AABB tree.
 235  *      \param          tree                    [in] generic AABB tree
 236  *      \return         true if success
 237  */
 238 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 239 bool AABBCollisionTree::Build(AABBTree* tree)
 240 {
 241         // Checkings
 242         if(!tree)       return false;
 243         // Check the input tree is complete
 244         udword NbTriangles      = tree->GetNbPrimitives();
 245         udword NbNodes          = tree->GetNbNodes();
 246         if(NbNodes!=NbTriangles*2-1)    return false;
 247
 248         // Get nodes
 249         if(mNbNodes!=NbNodes)   // Same number of nodes => keep moving
 250         {
 251                 mNbNodes = NbNodes;
 252                 DELETEARRAY(mNodes);
 253                 mNodes = new AABBCollisionNode[mNbNodes];
 254                 CHECKALLOC(mNodes);
 255         }
 256
 257         // Build the tree
 258         udword CurID = 1;
 259         _BuildCollisionTree(mNodes, 0, CurID, tree);
 260         ASSERT(CurID==mNbNodes);
 261
 262         return true;
 263 }
 264
 265 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 266 /**
 267  *      Refits the collision tree after vertices have been modified.
 268  *      \param          mesh_interface  [in] mesh interface for current model
 269  *      \return         true if success
 270  */
 271 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 272 bool AABBCollisionTree::Refit(const MeshInterface* mesh_interface)
 273 {
 274         ASSERT(!"Not implemented since AABBCollisionTrees have twice as more nodes to refit as AABBNoLeafTrees!");
 275         return false;
 276 }
 277
 278 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 279 /**
 280  *      Walks the tree and call the user back for each node.
 281  *      \param          callback        [in] walking callback
 282  *      \param          user_data       [in] callback's user data
 283  *      \return         true if success
 284  */
 285 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 286 bool AABBCollisionTree::Walk(GenericWalkingCallback callback, void* user_data) const
 287 {
 288         if(!callback)   return false;
 289
 290         struct Local
 291         {
 292                 static void _Walk(const AABBCollisionNode* current_node, GenericWalkingCallback callback, void* user_data)
 293                 {
 294                         if(!current_node || !(callback)(current_node, user_data))       return;
 295
 296                         if(!current_node->IsLeaf())
 297                         {
 298                                 _Walk(current_node->GetPos(), callback, user_data);
 299                                 _Walk(current_node->GetNeg(), callback, user_data);
 300                         }
 301                 }
 302         };
 303         Local::_Walk(mNodes, callback, user_data);
 304         return true;
 305 }
 306
 307
 308 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 309 /**
 310  *      Constructor.
 311  */
 312 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 313 AABBNoLeafTree::AABBNoLeafTree() : mNodes(null)
 314 {
 315 }
 316
 317 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 318 /**
 319  *      Destructor.
 320  */
 321 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 322 AABBNoLeafTree::~AABBNoLeafTree()
 323 {
 324         DELETEARRAY(mNodes);
 325 }
 326
 327 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 328 /**
 329  *      Builds the collision tree from a generic AABB tree.
 330  *      \param          tree                    [in] generic AABB tree
 331  *      \return         true if success
 332  */
 333 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 334 bool AABBNoLeafTree::Build(AABBTree* tree)
 335 {
 336         // Checkings
 337         if(!tree)       return false;
 338         // Check the input tree is complete
 339         udword NbTriangles      = tree->GetNbPrimitives();
 340         udword NbNodes          = tree->GetNbNodes();
 341         if(NbNodes!=NbTriangles*2-1)    return false;
 342
 343         // Get nodes
 344         if(mNbNodes!=NbTriangles-1)     // Same number of nodes => keep moving
 345         {
 346                 mNbNodes = NbTriangles-1;
 347                 DELETEARRAY(mNodes);
 348                 mNodes = new AABBNoLeafNode[mNbNodes];
 349                 CHECKALLOC(mNodes);
 350         }
 351
 352         // Build the tree
 353         udword CurID = 1;
 354         _BuildNoLeafTree(mNodes, 0, CurID, tree);
 355         ASSERT(CurID==mNbNodes);
 356
 357         return true;
 358 }
 359
 360 inline_ void ComputeMinMax_OT(Point& min, Point& max, const VertexPointers& vp)
 361 {
 362         // Compute triangle's AABB = a leaf box
 363 #ifdef OPC_USE_FCOMI    // a 15% speedup on my machine, not much
 364         min.x = FCMin3(vp.Vertex[0]->x, vp.Vertex[1]->x, vp.Vertex[2]->x);
 365         max.x = FCMax3(vp.Vertex[0]->x, vp.Vertex[1]->x, vp.Vertex[2]->x);
 366
 367         min.y = FCMin3(vp.Vertex[0]->y, vp.Vertex[1]->y, vp.Vertex[2]->y);
 368         max.y = FCMax3(vp.Vertex[0]->y, vp.Vertex[1]->y, vp.Vertex[2]->y);
 369
 370         min.z = FCMin3(vp.Vertex[0]->z, vp.Vertex[1]->z, vp.Vertex[2]->z);
 371         max.z = FCMax3(vp.Vertex[0]->z, vp.Vertex[1]->z, vp.Vertex[2]->z);
 372 #else
 373         min = *vp.Vertex[0];
 374         max = *vp.Vertex[0];
 375         min.Min(*vp.Vertex[1]);
 376         max.Max(*vp.Vertex[1]);
 377         min.Min(*vp.Vertex[2]);
 378         max.Max(*vp.Vertex[2]);
 379 #endif
 380 }
 381
 382 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 383 /**
 384  *      Refits the collision tree after vertices have been modified.
 385  *      \param          mesh_interface  [in] mesh interface for current model
 386  *      \return         true if success
 387  */
 388 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 389 bool AABBNoLeafTree::Refit(const MeshInterface* mesh_interface)
 390 {
 391         // Checkings
 392         if(!mesh_interface)     return false;
 393
 394         // Bottom-up update
 395         VertexPointers VP;
 396         Point Min,Max;
 397         Point Min_,Max_;
 398         udword Index = mNbNodes;
 399         while(Index--)
 400         {
 401                 AABBNoLeafNode& Current = mNodes[Index];
 402
 403                 if(Current.HasPosLeaf())
 404                 {
 405                         mesh_interface->GetTriangle(VP, Current.GetPosPrimitive());
 406                         ComputeMinMax_OT(Min, Max, VP);
 407                 }
 408                 else
 409                 {
 410                         const CollisionAABB& CurrentBox = Current.GetPos()->mAABB;
 411                         CurrentBox.GetMin(Min);
 412                         CurrentBox.GetMax(Max);
 413                 }
 414
 415                 if(Current.HasNegLeaf())
 416                 {
 417                         mesh_interface->GetTriangle(VP, Current.GetNegPrimitive());
 418                         ComputeMinMax_OT(Min_, Max_, VP);
 419                 }
 420                 else
 421                 {
 422                         const CollisionAABB& CurrentBox = Current.GetNeg()->mAABB;
 423                         CurrentBox.GetMin(Min_);
 424                         CurrentBox.GetMax(Max_);
 425                 }
 426 #ifdef OPC_USE_FCOMI
 427                 Min.x = FCMin2(Min.x, Min_.x);
 428                 Max.x = FCMax2(Max.x, Max_.x);
 429                 Min.y = FCMin2(Min.y, Min_.y);
 430                 Max.y = FCMax2(Max.y, Max_.y);
 431                 Min.z = FCMin2(Min.z, Min_.z);
 432                 Max.z = FCMax2(Max.z, Max_.z);
 433 #else
 434                 Min.Min(Min_);
 435                 Max.Max(Max_);
 436 #endif
 437                 Current.mAABB.SetMinMax(Min, Max);
 438         }
 439         return true;
 440 }
 441
 442 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 443 /**
 444  *      Walks the tree and call the user back for each node.
 445  *      \param          callback        [in] walking callback
 446  *      \param          user_data       [in] callback's user data
 447  *      \return         true if success
 448  */
 449 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 450 bool AABBNoLeafTree::Walk(GenericWalkingCallback callback, void* user_data) const
 451 {
 452         if(!callback)   return false;
 453
 454         struct Local
 455         {
 456                 static void _Walk(const AABBNoLeafNode* current_node, GenericWalkingCallback callback, void* user_data)
 457                 {
 458                         if(!current_node || !(callback)(current_node, user_data))       return;
 459
 460                         if(!current_node->HasPosLeaf()) _Walk(current_node->GetPos(), callback, user_data);
 461                         if(!current_node->HasNegLeaf()) _Walk(current_node->GetNeg(), callback, user_data);
 462                 }
 463         };
 464         Local::_Walk(mNodes, callback, user_data);
 465         return true;
 466 }
 467
 468 // Quantization notes:
 469 // - We could use the highest bits of mData to store some more quantized bits. Dequantization code
 470 //   would be slightly more complex, but number of overlap tests would be reduced (and anyhow those
 471 //   bits are currently wasted). Of course it's not possible if we move to 16 bits mData.
 472 // - Something like "16 bits floats" could be tested, to bypass the int-to-float conversion.
 473 // - A dedicated BV-BV test could be used, dequantizing while testing for overlap. (i.e. it's some
 474 //   lazy-dequantization which may save some work in case of early exits). At the very least some
 475 //   muls could be saved by precomputing several more matrices. But maybe not worth the pain.
 476 // - Do we need to dequantize anyway? Not doing the extents-related muls only implies the box has
 477 //   been scaled, for example.
 478 // - The deeper we move into the hierarchy, the smaller the extents should be. May not need a fixed
 479 //   number of quantization bits. Even better, could probably be best delta-encoded.
 480
 481
 482 // Find max values. Some people asked why I wasn't simply using the first node. Well, I can't.
 483 // I'm not looking for (min, max) values like in a standard AABB, I'm looking for the extremal
 484 // centers/extents in order to quantize them. The first node would only give a single center and
 485 // a single extents. While extents would be the biggest, the center wouldn't.
 486 #define FIND_MAX_VALUES                                                                                                                                                 \
 487         /* Get max values */                                                                                                                                            \
 488         Point CMax(MIN_FLOAT, MIN_FLOAT, MIN_FLOAT);                                                                                            \
 489         Point EMax(MIN_FLOAT, MIN_FLOAT, MIN_FLOAT);                                                                                            \
 490         for(udword i=0;i<mNbNodes;i++)                                                                                                                          \
 491         {                                                                                                                                                                                       \
 492                 if(fabsf(Nodes[i].mAABB.mCenter.x)>CMax.x)      CMax.x = fabsf(Nodes[i].mAABB.mCenter.x);       \
 493                 if(fabsf(Nodes[i].mAABB.mCenter.y)>CMax.y)      CMax.y = fabsf(Nodes[i].mAABB.mCenter.y);       \
 494                 if(fabsf(Nodes[i].mAABB.mCenter.z)>CMax.z)      CMax.z = fabsf(Nodes[i].mAABB.mCenter.z);       \
 495                 if(fabsf(Nodes[i].mAABB.mExtents.x)>EMax.x)     EMax.x = fabsf(Nodes[i].mAABB.mExtents.x);      \
 496                 if(fabsf(Nodes[i].mAABB.mExtents.y)>EMax.y)     EMax.y = fabsf(Nodes[i].mAABB.mExtents.y);      \
 497                 if(fabsf(Nodes[i].mAABB.mExtents.z)>EMax.z)     EMax.z = fabsf(Nodes[i].mAABB.mExtents.z);      \
 498         }
 499
 500 #define INIT_QUANTIZATION                                                                                                       \
 501         udword nbc=15;  /* Keep one bit for sign */                                                             \
 502         udword nbe=15;  /* Keep one bit for fix */                                                              \
 503         if(!gFixQuantized) nbe++;                                                                                               \
 504                                                                                                                                                         \
 505         /* Compute quantization coeffs */                                                                               \
 506         Point CQuantCoeff, EQuantCoeff;                                                                                 \
 507         CQuantCoeff.x = CMax.x!=0.0f ? float((1<<nbc)-1)/CMax.x : 0.0f;                 \
 508         CQuantCoeff.y = CMax.y!=0.0f ? float((1<<nbc)-1)/CMax.y : 0.0f;                 \
 509         CQuantCoeff.z = CMax.z!=0.0f ? float((1<<nbc)-1)/CMax.z : 0.0f;                 \
 510         EQuantCoeff.x = EMax.x!=0.0f ? float((1<<nbe)-1)/EMax.x : 0.0f;                 \
 511         EQuantCoeff.y = EMax.y!=0.0f ? float((1<<nbe)-1)/EMax.y : 0.0f;                 \
 512         EQuantCoeff.z = EMax.z!=0.0f ? float((1<<nbe)-1)/EMax.z : 0.0f;                 \
 513         /* Compute and save dequantization coeffs */                                                    \
 514         mCenterCoeff.x = CQuantCoeff.x!=0.0f ? 1.0f / CQuantCoeff.x : 0.0f;             \
 515         mCenterCoeff.y = CQuantCoeff.y!=0.0f ? 1.0f / CQuantCoeff.y : 0.0f;             \
 516         mCenterCoeff.z = CQuantCoeff.z!=0.0f ? 1.0f / CQuantCoeff.z : 0.0f;             \
 517         mExtentsCoeff.x = EQuantCoeff.x!=0.0f ? 1.0f / EQuantCoeff.x : 0.0f;    \
 518         mExtentsCoeff.y = EQuantCoeff.y!=0.0f ? 1.0f / EQuantCoeff.y : 0.0f;    \
 519         mExtentsCoeff.z = EQuantCoeff.z!=0.0f ? 1.0f / EQuantCoeff.z : 0.0f;    \
 520
 521 #define PERFORM_QUANTIZATION                                                                                                            \
 522         /* Quantize */                                                                                                                                  \
 523         mNodes[i].mAABB.mCenter[0] = sword(Nodes[i].mAABB.mCenter.x * CQuantCoeff.x);   \
 524         mNodes[i].mAABB.mCenter[1] = sword(Nodes[i].mAABB.mCenter.y * CQuantCoeff.y);   \
 525         mNodes[i].mAABB.mCenter[2] = sword(Nodes[i].mAABB.mCenter.z * CQuantCoeff.z);   \
 526         mNodes[i].mAABB.mExtents[0] = uword(Nodes[i].mAABB.mExtents.x * EQuantCoeff.x); \
 527         mNodes[i].mAABB.mExtents[1] = uword(Nodes[i].mAABB.mExtents.y * EQuantCoeff.y); \
 528         mNodes[i].mAABB.mExtents[2] = uword(Nodes[i].mAABB.mExtents.z * EQuantCoeff.z); \
 529         /* Fix quantized boxes */                                                                                                               \
 530         if(gFixQuantized)                                                                                                                               \
 531         {                                                                                                                                                               \
 532                 /* Make sure the quantized box is still valid */                                                        \
 533                 Point Max = Nodes[i].mAABB.mCenter + Nodes[i].mAABB.mExtents;                           \
 534                 Point Min = Nodes[i].mAABB.mCenter - Nodes[i].mAABB.mExtents;                           \
 535                 /* For each axis */                                                                                                                     \
 536                 for(udword j=0;j<3;j++)                                                                                                         \
 537                 {       /* Dequantize the box center */                                                                                 \
 538                         float qc = float(mNodes[i].mAABB.mCenter[j]) * mCenterCoeff[j];                 \
 539                         bool FixMe=true;                                                                                                                \
 540                         do                                                                                                                                              \
 541                         {       /* Dequantize the box extent */                                                                         \
 542                                 float qe = float(mNodes[i].mAABB.mExtents[j]) * mExtentsCoeff[j];       \
 543                                 /* Compare real & dequantized values */                                                         \
 544                                 if(qc+qe<Max[j] || qc-qe>Min[j])        mNodes[i].mAABB.mExtents[j]++;  \
 545                                 else                                                            FixMe=false;                                    \
 546                                 /* Prevent wrapping */                                                                                          \
 547                                 if(!mNodes[i].mAABB.mExtents[j])                                                                        \
 548                                 {                                                                                                                                       \
 549                                         mNodes[i].mAABB.mExtents[j]=0xffff;                                                             \
 550                                         FixMe=false;                                                                                                    \
 551                                 }                                                                                                                                       \
 552                         }while(FixMe);                                                                                                                  \
 553                 }                                                                                                                                                       \
 554         }
 555
 556 #define REMAP_DATA(member)                                                                                      \
 557         /* Fix data */                                                                                                  \
 558         Data = Nodes[i].member;                                                                                 \
 559         if(!(Data&1))                                                                                                   \
 560         {                                                                                                                               \
 561                 /* Compute box number */                                                                        \
 562                 udword Nb = (Data - udword(Nodes))/Nodes[i].GetNodeSize();      \
 563                 Data = udword(&mNodes[Nb]);                                                                     \
 564         }                                                                                                                               \
 565         /* ...remapped */                                                                                               \
 566         mNodes[i].member = Data;
 567
 568 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 569 /**
 570  *      Constructor.
 571  */
 572 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 573 AABBQuantizedTree::AABBQuantizedTree() : mNodes(null)
 574 {
 575 }
 576
 577 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 578 /**
 579  *      Destructor.
 580  */
 581 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 582 AABBQuantizedTree::~AABBQuantizedTree()
 583 {
 584         DELETEARRAY(mNodes);
 585 }
 586
 587 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 588 /**
 589  *      Builds the collision tree from a generic AABB tree.
 590  *      \param          tree                    [in] generic AABB tree
 591  *      \return         true if success
 592  */
 593 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 594 bool AABBQuantizedTree::Build(AABBTree* tree)
 595 {
 596         // Checkings
 597         if(!tree)       return false;
 598         // Check the input tree is complete
 599         udword NbTriangles      = tree->GetNbPrimitives();
 600         udword NbNodes          = tree->GetNbNodes();
 601         if(NbNodes!=NbTriangles*2-1)    return false;
 602
 603         // Get nodes
 604         mNbNodes = NbNodes;
 605         DELETEARRAY(mNodes);
 606         AABBCollisionNode* Nodes = new AABBCollisionNode[mNbNodes];
 607         CHECKALLOC(Nodes);
 608
 609         // Build the tree
 610         udword CurID = 1;
 611         _BuildCollisionTree(Nodes, 0, CurID, tree);
 612
 613         // Quantize
 614         {
 615                 mNodes = new AABBQuantizedNode[mNbNodes];
 616                 CHECKALLOC(mNodes);
 617
 618                 // Get max values
 619                 FIND_MAX_VALUES
 620
 621                 // Quantization
 622                 INIT_QUANTIZATION
 623
 624                 // Quantize
 625                 udword Data;
 626                 for(udword i=0;i<mNbNodes;i++)
 627                 {
 628                         PERFORM_QUANTIZATION
 629                         REMAP_DATA(mData)
 630                 }
 631
 632                 DELETEARRAY(Nodes);
 633         }
 634
 635         return true;
 636 }
 637
 638 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 639 /**
 640  *      Refits the collision tree after vertices have been modified.
 641  *      \param          mesh_interface  [in] mesh interface for current model
 642  *      \return         true if success
 643  */
 644 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 645 bool AABBQuantizedTree::Refit(const MeshInterface* mesh_interface)
 646 {
 647         ASSERT(!"Not implemented since requantizing is painful !");
 648         return false;
 649 }
 650
 651 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 652 /**
 653  *      Walks the tree and call the user back for each node.
 654  *      \param          callback        [in] walking callback
 655  *      \param          user_data       [in] callback's user data
 656  *      \return         true if success
 657  */
 658 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 659 bool AABBQuantizedTree::Walk(GenericWalkingCallback callback, void* user_data) const
 660 {
 661         if(!callback)   return false;
 662
 663         struct Local
 664         {
 665                 static void _Walk(const AABBQuantizedNode* current_node, GenericWalkingCallback callback, void* user_data)
 666                 {
 667                         if(!current_node || !(callback)(current_node, user_data))       return;
 668
 669                         if(!current_node->IsLeaf())
 670                         {
 671                                 _Walk(current_node->GetPos(), callback, user_data);
 672                                 _Walk(current_node->GetNeg(), callback, user_data);
 673                         }
 674                 }
 675         };
 676         Local::_Walk(mNodes, callback, user_data);
 677         return true;
 678 }
 679
 680
 681
 682 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 683 /**
 684  *      Constructor.
 685  */
 686 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 687 AABBQuantizedNoLeafTree::AABBQuantizedNoLeafTree() : mNodes(null)
 688 {
 689 }
 690
 691 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 692 /**
 693  *      Destructor.
 694  */
 695 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 696 AABBQuantizedNoLeafTree::~AABBQuantizedNoLeafTree()
 697 {
 698         DELETEARRAY(mNodes);
 699 }
 700
 701 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 702 /**
 703  *      Builds the collision tree from a generic AABB tree.
 704  *      \param          tree                    [in] generic AABB tree
 705  *      \return         true if success
 706  */
 707 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 708 bool AABBQuantizedNoLeafTree::Build(AABBTree* tree)
 709 {
 710         // Checkings
 711         if(!tree)       return false;
 712         // Check the input tree is complete
 713         udword NbTriangles      = tree->GetNbPrimitives();
 714         udword NbNodes          = tree->GetNbNodes();
 715         if(NbNodes!=NbTriangles*2-1)    return false;
 716
 717         // Get nodes
 718         mNbNodes = NbTriangles-1;
 719         DELETEARRAY(mNodes);
 720         AABBNoLeafNode* Nodes = new AABBNoLeafNode[mNbNodes];
 721         CHECKALLOC(Nodes);
 722
 723         // Build the tree
 724         udword CurID = 1;
 725         _BuildNoLeafTree(Nodes, 0, CurID, tree);
 726         ASSERT(CurID==mNbNodes);
 727
 728         // Quantize
 729         {
 730                 mNodes = new AABBQuantizedNoLeafNode[mNbNodes];
 731                 CHECKALLOC(mNodes);
 732
 733                 // Get max values
 734                 FIND_MAX_VALUES
 735
 736                 // Quantization
 737                 INIT_QUANTIZATION
 738
 739                 // Quantize
 740                 udword Data;
 741                 for(udword i=0;i<mNbNodes;i++)
 742                 {
 743                         PERFORM_QUANTIZATION
 744                         REMAP_DATA(mPosData)
 745                         REMAP_DATA(mNegData)
 746                 }
 747
 748                 DELETEARRAY(Nodes);
 749         }
 750
 751         return true;
 752 }
 753
 754 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 755 /**
 756  *      Refits the collision tree after vertices have been modified.
 757  *      \param          mesh_interface  [in] mesh interface for current model
 758  *      \return         true if success
 759  */
 760 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 761 bool AABBQuantizedNoLeafTree::Refit(const MeshInterface* mesh_interface)
 762 {
 763         ASSERT(!"Not implemented since requantizing is painful !");
 764         return false;
 765 }
 766
 767 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 768 /**
 769  *      Walks the tree and call the user back for each node.
 770  *      \param          callback        [in] walking callback
 771  *      \param          user_data       [in] callback's user data
 772  *      \return         true if success
 773  */
 774 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 775 bool AABBQuantizedNoLeafTree::Walk(GenericWalkingCallback callback, void* user_data) const
 776 {
 777         if(!callback)   return false;
 778
 779         struct Local
 780         {
 781                 static void _Walk(const AABBQuantizedNoLeafNode* current_node, GenericWalkingCallback callback, void* user_data)
 782                 {
 783                         if(!current_node || !(callback)(current_node, user_data))       return;
 784
 785                         if(!current_node->HasPosLeaf()) _Walk(current_node->GetPos(), callback, user_data);
 786                         if(!current_node->HasNegLeaf()) _Walk(current_node->GetNeg(), callback, user_data);
 787                 }
 788         };
 789         Local::_Walk(mNodes, callback, user_data);
 790         return true;
 791 }