one more for the gipper

1 files changed, 466 insertions, 0 deletions

diff --git a/libraries/ode-0.9/OPCODE/OPC_HybridModel.cpp b/libraries/ode-0.9/OPCODE/OPC_HybridModel.cpp
new file mode 100644
index 0000000..7016219
--- /dev/null
+++ b/libraries/ode-0.9/OPCODE/OPC_HybridModel.cpp
@@ -0,0 +1,466 @@
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/*
+ *      OPCODE - Optimized Collision Detection
+ *      Copyright (C) 2001 Pierre Terdiman
+ *      Homepage: http://www.codercorner.com/Opcode.htm
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *      Contains code for hybrid models.
+ *      \file           OPC_HybridModel.cpp
+ *      \author         Pierre Terdiman
+ *      \date           May, 18, 2003
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *      An hybrid collision model.
+ *      
+ *      The problem :
+ *      
+ *      Opcode really shines for mesh-mesh collision, especially when meshes are deeply overlapping
+ *      (it typically outperforms RAPID in those cases).
+ *      
+ *      Unfortunately this is not the typical scenario in games.
+ *      
+ *      For close-proximity cases, especially for volume-mesh queries, it's relatively easy to run faster
+ *      than Opcode, that suffers from a relatively high setup time.
+ *      
+ *      In particular, Opcode's "vanilla" trees in those cases -can- run faster. They can also use -less-
+ *      memory than the optimized ones, when you let the system stop at ~10 triangles / leaf for example
+ *      (i.e. when you don't use "complete" trees). However, those trees tend to fragment memory quite a
+ *      lot, increasing cache misses : since they're not "complete", we can't predict the final number of
+ *      nodes and we have to allocate nodes on-the-fly. For the same reasons we can't use Opcode's "optimized"
+ *      trees here, since they rely on a known layout to perform the "optimization".
+ *      
+ *      Hybrid trees :
+ *      
+ *      Hybrid trees try to combine best of both worlds :
+ *      
+ *      - they use a maximum limit of 16 triangles/leaf. "16" is used so that we'll be able to save the
+ *      number of triangles using 4 bits only.
+ *      
+ *      - they're still "complete" trees thanks to a two-passes building phase. First we create a "vanilla"
+ *      AABB-tree with Opcode, limited to 16 triangles/leaf. Then we create a *second* vanilla tree, this
+ *      time using the leaves of the first one. The trick is : this second tree is now "complete"... so we
+ *      can further transform it into an Opcode's optimized tree.
+ *      
+ *      - then we run the collision queries on that standard Opcode tree. The only difference is that leaf
+ *      nodes contain indices to leaf nodes of another tree. Also, we have to skip all primitive tests in
+ *      Opcode optimized trees, since our leaves don't contain triangles anymore.
+ *      
+ *      - finally, for each collided leaf, we simply loop through 16 triangles max, and collide them with
+ *      the bounding volume used in the query (we only support volume-vs-mesh queries here, not mesh-vs-mesh)
+ *      
+ *      All of that is wrapped in this "hybrid model" that contains the minimal data required for this to work.
+ *      It's a mix between old "vanilla" trees, and old "optimized" trees.
+ *
+ *      Extra advantages:
+ *
+ *      - If we use them for dynamic models, we're left with a very small number of leaf nodes to refit. It
+ *      might be a bit faster since we have less nodes to write back.
+ *
+ *      - In rigid body simulation, using temporal coherence and sleeping objects greatly reduce the actual
+ *      influence of one tree over another (i.e. the speed difference is often invisible). So memory is really
+ *      the key element to consider, and in this regard hybrid trees are just better.
+ *      
+ *      Information to take home:
+ *      - they use less ram
+ *      - they're not slower (they're faster or slower depending on cases, overall there's no significant
+ *      difference *as long as objects don't interpenetrate too much* - in which case Opcode's optimized trees
+ *      are still notably faster)
+ *
+ *      \class          HybridModel
+ *      \author         Pierre Terdiman
+ *      \version        1.3
+ *      \date           May, 18, 2003
+*/
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// Precompiled Header
+#include "Stdafx.h"
+
+using namespace Opcode;
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *      Constructor.
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+HybridModel::HybridModel() :
+        mNbLeaves               (0),
+        mNbPrimitives   (0),
+        mTriangles              (null),
+        mIndices                (null)
+{
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *      Destructor.
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+HybridModel::~HybridModel()
+{
+        Release();
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *      Releases everything.
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void HybridModel::Release()
+{
+        ReleaseBase();
+        DELETEARRAY(mIndices);
+        DELETEARRAY(mTriangles);
+        mNbLeaves               = 0;
+        mNbPrimitives   = 0;
+}
+
+        struct Internal
+        {
+                Internal()
+                {
+                        mNbLeaves       = 0;
+                        mLeaves         = null;
+                        mTriangles      = null;
+                        mBase           = null;
+                }
+                ~Internal()
+                {
+                        DELETEARRAY(mLeaves);
+                }
+
+                udword                  mNbLeaves;
+                AABB*                   mLeaves;
+                LeafTriangles*  mTriangles;
+                const udword*   mBase;
+        };
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *      Builds a collision model.
+ *      \param          create          [in] model creation structure
+ *      \return         true if success
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+bool HybridModel::Build(const OPCODECREATE& create)
+{
+        // 1) Checkings
+        if(!create.mIMesh || !create.mIMesh->IsValid()) return false;
+
+        // Look for degenerate faces.
+        udword NbDegenerate = create.mIMesh->CheckTopology();
+        if(NbDegenerate)        Log("OPCODE WARNING: found %d degenerate faces in model! Collision might report wrong results!\n", NbDegenerate);
+        // We continue nonetheless.... 
+
+        Release();      // Make sure previous tree has been discarded
+
+        // 1-1) Setup mesh interface automatically
+        SetMeshInterface(create.mIMesh);
+
+        bool Status = false;
+        AABBTree* LeafTree = null;
+        Internal Data;
+
+        // 2) Build a generic AABB Tree.
+        mSource = new AABBTree;
+        CHECKALLOC(mSource);
+
+        // 2-1) Setup a builder. Our primitives here are triangles from input mesh,
+        // so we use an AABBTreeOfTrianglesBuilder.....
+        {
+                AABBTreeOfTrianglesBuilder TB;
+                TB.mIMesh                       = create.mIMesh;
+                TB.mNbPrimitives        = create.mIMesh->GetNbTriangles();
+                TB.mSettings            = create.mSettings;
+                TB.mSettings.mLimit     = 16;   // ### Hardcoded, but maybe we could let the user choose 8 / 16 / 32 ...
+                if(!mSource->Build(&TB))        goto FreeAndExit;
+        }
+
+        // 2-2) Here's the trick : create *another* AABB tree using the leaves of the first one (which are boxes, this time)
+        struct Local
+        {
+                // A callback to count leaf nodes
+                static bool CountLeaves(const AABBTreeNode* current, udword depth, void* user_data)
+                {
+                        if(current->IsLeaf())
+                        {
+                                Internal* Data = (Internal*)user_data;
+                                Data->mNbLeaves++;
+                        }
+                        return true;
+                }
+
+                // A callback to setup leaf nodes in our internal structures
+                static bool SetupLeafData(const AABBTreeNode* current, udword depth, void* user_data)
+                {
+                        if(current->IsLeaf())
+                        {
+                                Internal* Data = (Internal*)user_data;
+
+                                // Get current leaf's box
+                                Data->mLeaves[Data->mNbLeaves] = *current->GetAABB();
+
+                                // Setup leaf data
+                                udword Index = udword((size_t(current->GetPrimitives()) - size_t(Data->mBase)) / sizeof(udword));
+                                Data->mTriangles[Data->mNbLeaves].SetData(current->GetNbPrimitives(), Index);
+
+                                Data->mNbLeaves++;
+                        }
+                        return true;
+                }
+        };
+
+        // Walk the tree & count number of leaves
+        Data.mNbLeaves = 0;
+        mSource->Walk(Local::CountLeaves, &Data);
+        mNbLeaves = Data.mNbLeaves;     // Keep track of it
+
+        // Special case for 1-leaf meshes
+        if(mNbLeaves==1)
+        {
+                mModelCode |= OPC_SINGLE_NODE;
+                Status = true;
+                goto FreeAndExit;
+        }
+
+        // Allocate our structures
+        Data.mLeaves = new AABB[Data.mNbLeaves];                CHECKALLOC(Data.mLeaves);
+        mTriangles = new LeafTriangles[Data.mNbLeaves]; CHECKALLOC(mTriangles);
+
+        // Walk the tree again & setup leaf data
+        Data.mTriangles = mTriangles;
+        Data.mBase              = mSource->GetIndices();
+        Data.mNbLeaves  = 0;    // Reset for incoming walk
+        mSource->Walk(Local::SetupLeafData, &Data);
+
+        // Handle source indices
+        {
+                bool MustKeepIndices = true;
+                if(create.mCanRemap)
+                {
+                        // We try to get rid of source indices (saving more ram!) by reorganizing triangle arrays...
+                        // Remap can fail when we use callbacks => keep track of indices in that case (it still
+                        // works, only using more memory)
+                        if(create.mIMesh->RemapClient(mSource->GetNbPrimitives(), mSource->GetIndices()))
+                        {
+                                MustKeepIndices = false;
+                        }
+                }
+
+                if(MustKeepIndices)
+                {
+                        // Keep track of source indices (from vanilla tree)
+                        mNbPrimitives = mSource->GetNbPrimitives();
+                        mIndices = new udword[mNbPrimitives];
+                        CopyMemory(mIndices, mSource->GetIndices(), mNbPrimitives*sizeof(udword));
+                }
+        }
+
+        // Now, create our optimized tree using previous leaf nodes
+        LeafTree = new AABBTree;
+        CHECKALLOC(LeafTree);
+        {
+                AABBTreeOfAABBsBuilder TB;      // Now using boxes !
+                TB.mSettings            = create.mSettings;
+                TB.mSettings.mLimit     = 1;    // We now want a complete tree so that we can "optimize" it
+                TB.mNbPrimitives        = Data.mNbLeaves;
+                TB.mAABBArray           = Data.mLeaves;
+                if(!LeafTree->Build(&TB))       goto FreeAndExit;
+        }
+
+        // 3) Create an optimized tree according to user-settings
+        if(!CreateTree(create.mNoLeaf, create.mQuantized))      goto FreeAndExit;
+
+        // 3-2) Create optimized tree
+        if(!mTree->Build(LeafTree))     goto FreeAndExit;
+
+        // Finally ok...
+        Status = true;
+
+FreeAndExit:    // Allow me this one...
+        DELETESINGLE(LeafTree);
+
+        // 3-3) Delete generic tree if needed
+        if(!create.mKeepOriginal)       DELETESINGLE(mSource);
+
+        return Status;
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *      Gets the number of bytes used by the tree.
+ *      \return         amount of bytes used
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+udword HybridModel::GetUsedBytes() const
+{
+        udword UsedBytes = 0;
+        if(mTree)               UsedBytes += mTree->GetUsedBytes();
+        if(mIndices)    UsedBytes += mNbPrimitives * sizeof(udword);    // mIndices
+        if(mTriangles)  UsedBytes += mNbLeaves * sizeof(LeafTriangles); // mTriangles
+        return UsedBytes;
+}
+
+inline_ void ComputeMinMax(Point& min, Point& max, const VertexPointers& vp)
+{
+        // Compute triangle's AABB = a leaf box
+#ifdef OPC_USE_FCOMI    // a 15% speedup on my machine, not much
+        min.x = FCMin3(vp.Vertex[0]->x, vp.Vertex[1]->x, vp.Vertex[2]->x);
+        max.x = FCMax3(vp.Vertex[0]->x, vp.Vertex[1]->x, vp.Vertex[2]->x);
+
+        min.y = FCMin3(vp.Vertex[0]->y, vp.Vertex[1]->y, vp.Vertex[2]->y);
+        max.y = FCMax3(vp.Vertex[0]->y, vp.Vertex[1]->y, vp.Vertex[2]->y);
+
+        min.z = FCMin3(vp.Vertex[0]->z, vp.Vertex[1]->z, vp.Vertex[2]->z);
+        max.z = FCMax3(vp.Vertex[0]->z, vp.Vertex[1]->z, vp.Vertex[2]->z);
+#else
+        min = *vp.Vertex[0];
+        max = *vp.Vertex[0];
+        min.Min(*vp.Vertex[1]);
+        max.Max(*vp.Vertex[1]);
+        min.Min(*vp.Vertex[2]);
+        max.Max(*vp.Vertex[2]);
+#endif
+}
+
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/**
+ *      Refits the collision model. This can be used to handle dynamic meshes. Usage is:
+ *      1. modify your mesh vertices (keep the topology constant!)
+ *      2. refit the tree (call this method)
+ *      \return         true if success
+ */
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+bool HybridModel::Refit()
+{
+        if(!mIMesh)     return false;
+        if(!mTree)      return false;
+
+        if(IsQuantized())       return false;
+        if(HasLeafNodes())      return false;
+
+        const LeafTriangles* LT = GetLeafTriangles();
+        const udword* Indices = GetIndices();
+
+        // Bottom-up update
+        VertexPointers VP;
+        Point Min,Max;
+        Point Min_,Max_;
+        udword Index = mTree->GetNbNodes();
+        AABBNoLeafNode* Nodes = (AABBNoLeafNode*)((AABBNoLeafTree*)mTree)->GetNodes();
+        while(Index--)
+        {
+                AABBNoLeafNode& Current = Nodes[Index];
+
+                if(Current.HasPosLeaf())
+                {
+                        const LeafTriangles& CurrentLeaf = LT[Current.GetPosPrimitive()];
+
+                        Min.SetPlusInfinity();
+                        Max.SetMinusInfinity();
+
+                        Point TmpMin, TmpMax;
+
+                        // Each leaf box has a set of triangles
+                        udword NbTris = CurrentLeaf.GetNbTriangles();
+                        if(Indices)
+                        {
+                                const udword* T = &Indices[CurrentLeaf.GetTriangleIndex()];
+
+                                // Loop through triangles and test each of them
+                                while(NbTris--)
+                                {
+                                        mIMesh->GetTriangle(VP, *T++);
+                                        ComputeMinMax(TmpMin, TmpMax, VP);
+                                        Min.Min(TmpMin);
+                                        Max.Max(TmpMax);
+                                }
+                        }
+                        else
+                        {
+                                udword BaseIndex = CurrentLeaf.GetTriangleIndex();
+
+                                // Loop through triangles and test each of them
+                                while(NbTris--)
+                                {
+                                        mIMesh->GetTriangle(VP, BaseIndex++);
+                                        ComputeMinMax(TmpMin, TmpMax, VP);
+                                        Min.Min(TmpMin);
+                                        Max.Max(TmpMax);
+                                }
+                        }
+                }
+                else
+                {
+                        const CollisionAABB& CurrentBox = Current.GetPos()->mAABB;
+                        CurrentBox.GetMin(Min);
+                        CurrentBox.GetMax(Max);
+                }
+
+                if(Current.HasNegLeaf())
+                {
+                        const LeafTriangles& CurrentLeaf = LT[Current.GetNegPrimitive()];
+
+                        Min_.SetPlusInfinity();
+                        Max_.SetMinusInfinity();
+
+                        Point TmpMin, TmpMax;
+
+                        // Each leaf box has a set of triangles
+                        udword NbTris = CurrentLeaf.GetNbTriangles();
+                        if(Indices)
+                        {
+                                const udword* T = &Indices[CurrentLeaf.GetTriangleIndex()];
+
+                                // Loop through triangles and test each of them
+                                while(NbTris--)
+                                {
+                                        mIMesh->GetTriangle(VP, *T++);
+                                        ComputeMinMax(TmpMin, TmpMax, VP);
+                                        Min_.Min(TmpMin);
+                                        Max_.Max(TmpMax);
+                                }
+                        }
+                        else
+                        {
+                                udword BaseIndex = CurrentLeaf.GetTriangleIndex();
+
+                                // Loop through triangles and test each of them
+                                while(NbTris--)
+                                {
+                                        mIMesh->GetTriangle(VP, BaseIndex++);
+                                        ComputeMinMax(TmpMin, TmpMax, VP);
+                                        Min_.Min(TmpMin);
+                                        Max_.Max(TmpMax);
+                                }
+                        }
+                }
+                else
+                {
+                        const CollisionAABB& CurrentBox = Current.GetNeg()->mAABB;
+                        CurrentBox.GetMin(Min_);
+                        CurrentBox.GetMax(Max_);
+                }
+#ifdef OPC_USE_FCOMI
+                Min.x = FCMin2(Min.x, Min_.x);
+                Max.x = FCMax2(Max.x, Max_.x);
+                Min.y = FCMin2(Min.y, Min_.y);
+                Max.y = FCMax2(Max.y, Max_.y);
+                Min.z = FCMin2(Min.z, Min_.z);
+                Max.z = FCMax2(Max.z, Max_.z);
+#else
+                Min.Min(Min_);
+                Max.Max(Max_);
+#endif
+                Current.mAABB.SetMinMax(Min, Max);
+        }
+        return true;
+}