add ode

1 files changed, 609 insertions, 0 deletions

diff --git a/libs/ode-0.16.1/ode/src/collision_quadtreespace.cpp b/libs/ode-0.16.1/ode/src/collision_quadtreespace.cpp
new file mode 100644
index 0000000..200b20f
--- /dev/null
+++ b/libs/ode-0.16.1/ode/src/collision_quadtreespace.cpp
@@ -0,0 +1,609 @@
+/*************************************************************************
+ *                                                                       *
+ * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith.       *
+ * All rights reserved.  Email: russ@q12.org   Web: www.q12.org          *
+ *                                                                       *
+ * This library is free software; you can redistribute it and/or         *
+ * modify it under the terms of EITHER:                                  *
+ *   (1) The GNU Lesser General Public License as published by the Free  *
+ *       Software Foundation; either version 2.1 of the License, or (at  *
+ *       your option) any later version. The text of the GNU Lesser      *
+ *       General Public License is included with this library in the     *
+ *       file LICENSE.TXT.                                               *
+ *   (2) The BSD-style license that is included with this library in     *
+ *       the file LICENSE-BSD.TXT.                                       *
+ *                                                                       *
+ * This library is distributed in the hope that it will be useful,       *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files    *
+ * LICENSE.TXT and LICENSE-BSD.TXT for more details.                     *
+ *                                                                       *
+ *************************************************************************/
+
+// QuadTreeSpace by Erwin de Vries.
+// With math corrections by Oleh Derevenko. ;)
+
+#include <ode/common.h>
+#include <ode/collision_space.h>
+#include <ode/collision.h>
+#include "config.h"
+#include "matrix.h"
+#include "collision_kernel.h"
+
+#include "collision_space_internal.h"
+
+
+#define AXIS0 0
+#define AXIS1 1
+#define UP 2
+
+//#define DRAWBLOCKS
+
+const int SPLITAXIS = 2;
+const int SPLITS = SPLITAXIS * SPLITAXIS;
+
+#define GEOM_ENABLED(g) (((g)->gflags & GEOM_ENABLE_TEST_MASK) == GEOM_ENABLE_TEST_VALUE)
+
+class Block{
+public:
+    dReal mMinX, mMaxX;
+    dReal mMinZ, mMaxZ;
+
+    dGeomID mFirst;
+    int mGeomCount;
+
+    Block* mParent;
+    Block* mChildren;
+
+    void Create(const dReal MinX, const dReal MaxX, const dReal MinZ, const dReal MaxZ, Block* Parent, int Depth, Block*& Blocks);
+
+    void Collide(void* UserData, dNearCallback* Callback);
+    void Collide(dGeomID g1, dGeomID g2, void* UserData, dNearCallback* Callback);
+
+    void CollideLocal(dGeomID g2, void* UserData, dNearCallback* Callback);
+
+    void AddObject(dGeomID Object);
+    void DelObject(dGeomID Object);
+    void Traverse(dGeomID Object);
+
+    bool Inside(const dReal* AABB);
+
+    Block* GetBlock(const dReal* AABB);
+    Block* GetBlockChild(const dReal* AABB);
+};
+
+
+#ifdef DRAWBLOCKS
+#include "..\..\Include\drawstuff\\drawstuff.h"
+
+static void DrawBlock(Block* Block){
+    dVector3 v[8];
+    v[0][AXIS0] = Block->mMinX;
+    v[0][UP] = REAL(-1.0);
+    v[0][AXIS1] = Block->mMinZ;
+
+    v[1][AXIS0] = Block->mMinX;
+    v[1][UP] = REAL(-1.0);
+    v[1][AXIS1] = Block->mMaxZ;
+
+    v[2][AXIS0] = Block->mMaxX;
+    v[2][UP] = REAL(-1.0);
+    v[2][AXIS1] = Block->mMinZ;
+
+    v[3][AXIS0] = Block->mMaxX;
+    v[3][UP] = REAL(-1.0);
+    v[3][AXIS1] = Block->mMaxZ;
+
+    v[4][AXIS0] = Block->mMinX;
+    v[4][UP] = REAL(1.0);
+    v[4][AXIS1] = Block->mMinZ;
+
+    v[5][AXIS0] = Block->mMinX;
+    v[5][UP] = REAL(1.0);
+    v[5][AXIS1] = Block->mMaxZ;
+
+    v[6][AXIS0] = Block->mMaxX;
+    v[6][UP] = REAL(1.0);
+    v[6][AXIS1] = Block->mMinZ;
+
+    v[7][AXIS0] = Block->mMaxX;
+    v[7][UP] = REAL(1.0);
+    v[7][AXIS1] = Block->mMaxZ;
+
+    // Bottom
+    dsDrawLine(v[0], v[1]);
+    dsDrawLine(v[1], v[3]);
+    dsDrawLine(v[3], v[2]);
+    dsDrawLine(v[2], v[0]);
+
+    // Top
+    dsDrawLine(v[4], v[5]);
+    dsDrawLine(v[5], v[7]);
+    dsDrawLine(v[7], v[6]);
+    dsDrawLine(v[6], v[4]);
+
+    // Sides
+    dsDrawLine(v[0], v[4]);
+    dsDrawLine(v[1], v[5]);
+    dsDrawLine(v[2], v[6]);
+    dsDrawLine(v[3], v[7]);
+}
+#endif	//DRAWBLOCKS
+
+
+void Block::Create(const dReal MinX, const dReal MaxX, const dReal MinZ, const dReal MaxZ, Block* Parent, int Depth, Block*& Blocks){
+    dIASSERT(MinX <= MaxX);
+    dIASSERT(MinZ <= MaxZ);
+
+    mGeomCount = 0;
+    mFirst = 0;
+
+    mMinX = MinX;
+    mMaxX = MaxX;
+
+    mMinZ = MinZ;
+    mMaxZ = MaxZ;
+
+    this->mParent = Parent;
+
+    if (Depth > 0){
+        mChildren = Blocks;
+        Blocks += SPLITS;
+
+        const dReal ChildExtentX = (MaxX - MinX) / SPLITAXIS;
+        const dReal ChildExtentZ = (MaxZ - MinZ) / SPLITAXIS;
+
+        const int ChildDepth = Depth - 1;
+        int Index = 0;
+
+        dReal ChildRightX = MinX;
+        for (int i = 0; i < SPLITAXIS; i++){
+            const dReal ChildLeftX = ChildRightX;
+            ChildRightX = (i != SPLITAXIS - 1) ? ChildLeftX + ChildExtentX : MaxX;
+
+            dReal ChildRightZ = MinZ;
+            for (int j = 0; j < SPLITAXIS; j++){
+                const dReal ChildLeftZ = ChildRightZ;
+                ChildRightZ = (j != SPLITAXIS - 1) ? ChildLeftZ + ChildExtentZ : MaxZ;
+
+                mChildren[Index].Create(ChildLeftX, ChildRightX, ChildLeftZ, ChildRightZ, this, ChildDepth, Blocks);
+                ++Index;
+            }
+        }
+    }
+    else mChildren = 0;
+}
+
+void Block::Collide(void* UserData, dNearCallback* Callback){
+#ifdef DRAWBLOCKS
+    DrawBlock(this);
+#endif
+    // Collide the local list
+    dxGeom* g = mFirst;
+    while (g){
+        if (GEOM_ENABLED(g)){
+            Collide(g, g->next_ex, UserData, Callback);
+        }
+        g = g->next_ex;
+    }
+
+    // Recurse for children
+    if (mChildren){
+        for (int i = 0; i < SPLITS; i++){
+            Block &CurrentChild = mChildren[i];
+            if (CurrentChild.mGeomCount <= 1){	// Early out
+                continue;
+            }
+            CurrentChild.Collide(UserData, Callback);
+        }
+    }
+}
+
+// Note: g2 is assumed to be in this Block
+void Block::Collide(dxGeom* g1, dxGeom* g2, void* UserData, dNearCallback* Callback){
+#ifdef DRAWBLOCKS
+    DrawBlock(this);
+#endif
+    // Collide against local list
+    while (g2){
+        if (GEOM_ENABLED(g2)){
+            collideAABBs (g1, g2, UserData, Callback);
+        }
+        g2 = g2->next_ex;
+    }
+
+    // Collide against children
+    if (mChildren){
+        for (int i = 0; i < SPLITS; i++){
+            Block &CurrentChild = mChildren[i];
+            // Early out for empty blocks
+            if (CurrentChild.mGeomCount == 0){
+                continue;
+            }
+
+            // Does the geom's AABB collide with the block?
+            // Don't do AABB tests for single geom blocks.
+            if (CurrentChild.mGeomCount == 1){
+                //
+            }
+            else if (true){
+                if (g1->aabb[AXIS0 * 2 + 0] >= CurrentChild.mMaxX ||
+                    g1->aabb[AXIS0 * 2 + 1] < CurrentChild.mMinX ||
+                    g1->aabb[AXIS1 * 2 + 0] >= CurrentChild.mMaxZ ||
+                    g1->aabb[AXIS1 * 2 + 1] < CurrentChild.mMinZ) continue;
+            }
+            CurrentChild.Collide(g1, CurrentChild.mFirst, UserData, Callback);
+        }
+    }
+}
+
+void Block::CollideLocal(dxGeom* g2, void* UserData, dNearCallback* Callback){
+    // Collide against local list
+    dxGeom* g1 = mFirst;
+    while (g1){
+        if (GEOM_ENABLED(g1)){
+            collideAABBs (g1, g2, UserData, Callback);
+        }
+        g1 = g1->next_ex;
+    }
+}
+
+void Block::AddObject(dGeomID Object){
+    // Add the geom
+    Object->next_ex = mFirst;
+    mFirst = Object;
+    Object->tome_ex = (dxGeom**)this;
+
+    // Now traverse upwards to tell that we have a geom
+    Block* Block = this;
+    do{
+        Block->mGeomCount++;
+        Block = Block->mParent;
+    }
+    while (Block);
+}
+
+void Block::DelObject(dGeomID Object){
+    // Del the geom
+    dxGeom* g = mFirst;
+    dxGeom* Last = 0;
+    while (g){
+        if (g == Object){
+            if (Last){
+                Last->next_ex = g->next_ex;
+            }
+            else mFirst = g->next_ex;
+
+            break;
+        }
+        Last = g;
+        g = g->next_ex;
+    }
+
+    Object->tome_ex = 0;
+
+    // Now traverse upwards to tell that we have lost a geom
+    Block* Block = this;
+    do{
+        Block->mGeomCount--;
+        Block = Block->mParent;
+    }
+    while (Block);
+}
+
+void Block::Traverse(dGeomID Object){
+    Block* NewBlock = GetBlock(Object->aabb);
+
+    if (NewBlock != this){
+        // Remove the geom from the old block and add it to the new block.
+        // This could be more optimal, but the loss should be very small.
+        DelObject(Object);
+        NewBlock->AddObject(Object);
+    }
+}
+
+bool Block::Inside(const dReal* AABB){
+    return AABB[AXIS0 * 2 + 0] >= mMinX && AABB[AXIS0 * 2 + 1] < mMaxX && AABB[AXIS1 * 2 + 0] >= mMinZ && AABB[AXIS1 * 2 + 1] < mMaxZ;
+}
+
+Block* Block::GetBlock(const dReal* AABB){
+    if (Inside(AABB)){
+        return GetBlockChild(AABB);	// Child or this will have a good block
+    }
+    else if (mParent){
+        return mParent->GetBlock(AABB);	// Parent has a good block
+    }
+    else return this;	// We are at the root, so we have little choice
+}
+
+Block* Block::GetBlockChild(const dReal* AABB){
+    if (mChildren){
+        for (int i = 0; i < SPLITS; i++){
+            Block &CurrentChild = mChildren[i];
+            if (CurrentChild.Inside(AABB)){
+                return CurrentChild.GetBlockChild(AABB);	// Child will have good block
+            }
+        }
+    }
+    return this;	// This is the best block
+}
+
+//****************************************************************************
+// quadtree space
+
+struct dxQuadTreeSpace : public dxSpace{
+    Block* Blocks;	// Blocks[0] is the root
+
+    dArray<dxGeom*> DirtyList;
+
+    dxQuadTreeSpace(dSpaceID _space, const dVector3 Center, const dVector3 Extents, int Depth);
+    ~dxQuadTreeSpace();
+
+    dxGeom* getGeom(int i);
+
+    void add(dxGeom* g);
+    void remove(dxGeom* g);
+    void dirty(dxGeom* g);
+
+    void computeAABB();
+
+    void cleanGeoms();
+    void collide(void* UserData, dNearCallback* Callback);
+    void collide2(void* UserData, dxGeom* g1, dNearCallback* Callback);
+
+    // Temp data
+    Block* CurrentBlock;	// Only used while enumerating
+    int* CurrentChild;	// Only used while enumerating
+    int CurrentLevel;	// Only used while enumerating
+    dxGeom* CurrentObject;	// Only used while enumerating
+    int CurrentIndex;
+};
+
+namespace {
+
+    inline
+    sizeint numNodes(int depth) 
+    {
+        // A 4-ary tree has (4^(depth+1) - 1)/3 nodes
+        // Note: split up into multiple constant expressions for readability
+        const int k = depth+1;
+        const sizeint fourToNthPlusOne = (sizeint)1 << (2*k); // 4^k = 2^(2k)
+        return (fourToNthPlusOne - 1) / 3;
+    }
+
+}
+
+
+
+dxQuadTreeSpace::dxQuadTreeSpace(dSpaceID _space, const dVector3 Center, const dVector3 Extents, int Depth) : dxSpace(_space){
+    type = dQuadTreeSpaceClass;
+
+    sizeint BlockCount = numNodes(Depth);
+
+    Blocks = (Block*)dAlloc(BlockCount * sizeof(Block));
+    Block* Blocks = this->Blocks + 1;	// This pointer gets modified!
+
+    dReal MinX = Center[AXIS0] - Extents[AXIS0];
+    dReal MaxX = dNextAfter((Center[AXIS0] + Extents[AXIS0]), (dReal)dInfinity);
+    dReal MinZ = Center[AXIS1] - Extents[AXIS1];
+    dReal MaxZ = dNextAfter((Center[AXIS1] + Extents[AXIS1]), (dReal)dInfinity);
+    this->Blocks[0].Create(MinX, MaxX, MinZ, MaxZ, 0, Depth, Blocks);
+
+    CurrentBlock = 0;
+    CurrentChild = (int*)dAlloc((Depth + 1) * sizeof(int));
+    CurrentLevel = 0;
+    CurrentObject = 0;
+    CurrentIndex = -1;
+
+    // Init AABB. We initialize to infinity because it is not illegal for an object to be outside of the tree. Its simply inserted in the root block
+    aabb[0] = -dInfinity;
+    aabb[1] = dInfinity;
+    aabb[2] = -dInfinity;
+    aabb[3] = dInfinity;
+    aabb[4] = -dInfinity;
+    aabb[5] = dInfinity;
+}
+
+dxQuadTreeSpace::~dxQuadTreeSpace(){
+    int Depth = 0;
+    Block* Current = &Blocks[0];
+    while (Current){
+        Depth++;
+        Current = Current->mChildren;
+    }
+
+    sizeint BlockCount = numNodes(Depth);
+
+    dFree(Blocks, BlockCount * sizeof(Block));
+    dFree(CurrentChild, (Depth + 1) * sizeof(int));
+}
+
+dxGeom* dxQuadTreeSpace::getGeom(int Index){
+    dUASSERT(Index >= 0 && Index < count, "index out of range");
+
+    //@@@
+    dDebug (0,"dxQuadTreeSpace::getGeom() not yet implemented");
+
+    return 0;
+
+    // This doesnt work
+/*
+    if (CurrentIndex == Index){
+        // Loop through all objects in the local list
+CHILDRECURSE:
+        if (CurrentObject){
+            dGeomID g = CurrentObject;
+            CurrentObject = CurrentObject->next_ex;
+            CurrentIndex++;
+
+#ifdef DRAWBLOCKS
+            DrawBlock(CurrentBlock);
+#endif	//DRAWBLOCKS
+            return g;
+        }
+        else{
+            // Now lets loop through our children. Starting at index 0.
+            if (CurrentBlock->Children){
+                CurrentChild[CurrentLevel] = 0;
+PARENTRECURSE:
+                for (int& i = CurrentChild[CurrentLevel]; i < SPLITS; i++){
+                    if (CurrentBlock->Children[i].GeomCount == 0){
+                        continue;
+                    }
+                    CurrentBlock = &CurrentBlock->Children[i];
+                    CurrentObject = CurrentBlock->First;
+
+                    i++;
+
+                    CurrentLevel++;
+                    goto CHILDRECURSE;
+                }
+            }
+        }
+
+        // Now lets go back to the parent so it can continue processing its other children.
+        if (CurrentBlock->Parent){
+            CurrentBlock = CurrentBlock->Parent;
+            CurrentLevel--;
+            goto PARENTRECURSE;
+        }
+    }
+    else{
+        CurrentBlock = &Blocks[0];
+        CurrentLevel = 0;
+        CurrentObject = CurrentObject;
+        CurrentIndex = 0;
+
+        // Other states are already set
+        CurrentObject = CurrentBlock->First;
+    }
+
+
+    if (current_geom && current_index == Index - 1){
+        //current_geom = current_geom->next_ex; // next
+        current_index = Index;
+        return current_geom;
+    }
+    else for (int i = 0; i < Index; i++){	// this will be verrrrrrry slow
+        getGeom(i);
+    }
+*/
+
+    return 0;
+}
+
+void dxQuadTreeSpace::add(dxGeom* g){
+    CHECK_NOT_LOCKED (this);
+    dAASSERT(g);
+    dUASSERT(g->tome_ex == 0 && g->next_ex == 0, "geom is already in a space");
+
+    DirtyList.push(g);
+    Blocks[0].GetBlock(g->aabb)->AddObject(g);	// Add to best block
+
+    dxSpace::add(g);
+}
+
+void dxQuadTreeSpace::remove(dxGeom* g){
+    CHECK_NOT_LOCKED(this);
+    dAASSERT(g);
+    dUASSERT(g->parent_space == this,"object is not in this space");
+
+    // remove
+    ((Block*)g->tome_ex)->DelObject(g);
+
+    for (int i = 0; i < DirtyList.size(); i++){
+        if (DirtyList[i] == g){
+            DirtyList.remove(i);
+            // (mg) there can be multiple instances of a dirty object on stack  be sure to remove ALL and not just first, for this we decrement i
+            --i;
+        }
+    }
+
+    dxSpace::remove(g);
+}
+
+void dxQuadTreeSpace::dirty(dxGeom* g){
+    DirtyList.push(g);
+}
+
+void dxQuadTreeSpace::computeAABB(){
+    //
+}
+
+void dxQuadTreeSpace::cleanGeoms(){
+    // compute the AABBs of all dirty geoms, and clear the dirty flags
+    lock_count++;
+
+    for (int i = 0; i < DirtyList.size(); i++){
+        dxGeom* g = DirtyList[i];
+        if (IS_SPACE(g)){
+            ((dxSpace*)g)->cleanGeoms();
+        }
+        
+        g->recomputeAABB();
+        dIASSERT((g->gflags & GEOM_AABB_BAD) == 0);
+
+        g->gflags &= ~GEOM_DIRTY;
+
+        ((Block*)g->tome_ex)->Traverse(g);
+    }
+    DirtyList.setSize(0);
+
+    lock_count--;
+}
+
+void dxQuadTreeSpace::collide(void* UserData, dNearCallback* Callback){
+    dAASSERT(Callback);
+
+    lock_count++;
+    cleanGeoms();
+
+    Blocks[0].Collide(UserData, Callback);
+
+    lock_count--;
+}
+
+
+struct DataCallback {
+    void *data;
+    dNearCallback *callback;
+};
+// Invokes the callback with arguments swapped
+static void swap_callback(void *data, dxGeom *g1, dxGeom *g2)
+{
+    DataCallback *dc = (DataCallback*)data;
+    dc->callback(dc->data, g2, g1);
+}
+
+
+void dxQuadTreeSpace::collide2(void* UserData, dxGeom* g2, dNearCallback* Callback){
+    dAASSERT(g2 && Callback);
+
+    lock_count++;
+    cleanGeoms();
+    g2->recomputeAABB();
+
+    if (g2->parent_space == this){
+        // The block the geom is in
+        Block* CurrentBlock = (Block*)g2->tome_ex;
+
+        // Collide against block and its children
+        DataCallback dc = {UserData, Callback};
+        CurrentBlock->Collide(g2, CurrentBlock->mFirst, &dc, swap_callback);
+
+        // Collide against parents
+        while ((CurrentBlock = CurrentBlock->mParent))
+            CurrentBlock->CollideLocal(g2, UserData, Callback);
+
+    }
+    else {
+        DataCallback dc = {UserData, Callback};
+        Blocks[0].Collide(g2, Blocks[0].mFirst, &dc, swap_callback);
+    }
+
+    lock_count--;
+}
+
+dSpaceID dQuadTreeSpaceCreate(dxSpace* space, const dVector3 Center, const dVector3 Extents, int Depth){
+    return new dxQuadTreeSpace(space, Center, Extents, Depth);
+}