Index: trunk/src/lib/collision_detection/bounding_sphere.cc
===================================================================
--- trunk/src/lib/collision_detection/bounding_sphere.cc	(revision 7677)
+++ trunk/src/lib/collision_detection/bounding_sphere.cc	(revision 7711)
@@ -1,3 +1,3 @@
-/* 
+/*
    orxonox - the future of 3D-vertical-scrollers
 
@@ -14,5 +14,5 @@
 */
 
-#define DEBUG_SPECIAL_MODULE DEBUG_MODULE_COLLISION
+#define DEBUG_SPECIAL_MODULE DEBUG_MODULE_COLLISION_DETECTION
 
 #include "bounding_sphere.h"
@@ -24,7 +24,7 @@
  *  standard constructor
 */
-BoundingSphere::BoundingSphere () 
+BoundingSphere::BoundingSphere ()
 {
-   this->setClassID(CL_BOUNDING_SPHERE, "BoundingSphere"); 
+   this->setClassID(CL_BOUNDING_SPHERE, "BoundingSphere");
 }
 
@@ -34,5 +34,5 @@
 
 */
-BoundingSphere::~BoundingSphere () 
+BoundingSphere::~BoundingSphere ()
 {
   // delete what has to be deleted here
Index: trunk/src/lib/collision_detection/bounding_volume.cc
===================================================================
--- trunk/src/lib/collision_detection/bounding_volume.cc	(revision 7677)
+++ trunk/src/lib/collision_detection/bounding_volume.cc	(revision 7711)
@@ -14,8 +14,7 @@
 */
 
-#define DEBUG_SPECIAL_MODULE DEBUG_MODULE_COLLISION
+#define DEBUG_SPECIAL_MODULE DEBUG_MODULE_COLLISION_DETECTION
 
 #include "bounding_volume.h"
-#include "vector.h"
 
 using namespace std;
@@ -27,8 +26,8 @@
 BoundingVolume::BoundingVolume ()
 {
-   this->setClassID(CL_BOUNDING_VOLUME, "BoundingVolume");
-   this->center = new Vector();
-   this->bOrigVertices = true;
-   this->vertices = NULL;
+  this->setClassID(CL_BOUNDING_VOLUME, "BoundingVolume");
+  this->modelInf = NULL;
+  this->triangleIndexes = NULL;
+  this->triangleIndexesLength = 0;
 }
 
@@ -40,8 +39,5 @@
 BoundingVolume::~BoundingVolume ()
 {
-  // delete what has to be deleted here
-  delete this->center;
-
-  if( this->vertices && !this->bOrigVertices)
-    delete[] this->vertices;
+  if( this->triangleIndexes)
+    delete[] this->triangleIndexes;
 }
Index: trunk/src/lib/collision_detection/bounding_volume.h
===================================================================
--- trunk/src/lib/collision_detection/bounding_volume.h	(revision 7677)
+++ trunk/src/lib/collision_detection/bounding_volume.h	(revision 7711)
@@ -11,5 +11,5 @@
 #include "model.h"
 
-class Vector;
+
 template<class T> class tList;
 
@@ -22,7 +22,7 @@
     virtual ~BoundingVolume();
 
-    inline const Vector* getCenter() const { return this->center; }
+    inline const Vector& getCenter() const { return this->center; }
+    inline const modelInfo* getModelInfo() const { return this->modelInf; }
 
-    const sVec3D* getVertices() const { return this->vertices; }
     virtual void mergeWith(const BoundingVolume &bv) = 0;
 
@@ -32,9 +32,10 @@
 
   public:
-    Vector*             center;                     //!< Center point of box
+    Vector              center;                     //!< Weighter average center point of box
+    Vector              arithCenter;                //!< Arithmetical center of the box
 
-    const sVec3D*       vertices;                   //!< if CD_STORE_VERTICES enabled, this is the place, where the vert. will be sotred
-    int                 numOfVertices;              //!< number of vertices in the vertices buffer
-    bool                bOrigVertices;              //!< is true if the vertices pointer points to the original model data - only important for deleting
+    const modelInfo*    modelInf;                   //!< Reference to the model's ModelInfo
+    const int*          triangleIndexes;            //!< Array with the triangle indexes in modelInfo
+    int                 triangleIndexesLength;      //!< length of the indexes array
 };
 
Index: trunk/src/lib/collision_detection/bv_tree.cc
===================================================================
--- trunk/src/lib/collision_detection/bv_tree.cc	(revision 7677)
+++ trunk/src/lib/collision_detection/bv_tree.cc	(revision 7711)
@@ -1,3 +1,3 @@
-/* 
+/*
    orxonox - the future of 3D-vertical-scrollers
 
@@ -14,5 +14,5 @@
 */
 
-#define DEBUG_SPECIAL_MODULE DEBUG_MODULE_COLLISION
+#define DEBUG_SPECIAL_MODULE DEBUG_MODULE_COLLISION_DETECTION
 
 #include "bv_tree.h"
@@ -25,7 +25,7 @@
  *  standard constructor
 */
-BVTree::BVTree () 
+BVTree::BVTree ()
 {
-   this->setClassID(CL_BV_TREE, "BVTree"); 
+   this->setClassID(CL_BV_TREE, "BVTree");
 
 }
@@ -36,5 +36,5 @@
 
 */
-BVTree::~BVTree () 
+BVTree::~BVTree ()
 {
   // delete what has to be deleted here
Index: trunk/src/lib/collision_detection/bv_tree.h
===================================================================
--- trunk/src/lib/collision_detection/bv_tree.h	(revision 7677)
+++ trunk/src/lib/collision_detection/bv_tree.h	(revision 7711)
@@ -18,4 +18,5 @@
 class WorldEntity;
 
+//! draw mode for the bounding volume
 typedef enum DrawMode
 {
@@ -33,27 +34,17 @@
 
 //! A class that represents a bounding volume tree
-class BVTree : public BaseObject {
+class BVTree : public BaseObject
+{
 
- public:
-  BVTree();
-  virtual ~BVTree();
+  public:
+    BVTree();
+    virtual ~BVTree();
 
-  virtual void spawnBVTree(int depth, sVec3D *verticesList, const int length) = 0;
-  virtual void spawnBVTree(int depth, const modelInfo& modInfo) = 0;
-  virtual void flushTree() = 0;
+    virtual void spawnBVTree(const modelInfo& modelInf) = 0;
+    virtual void flushTree() = 0;
 
-  virtual void collideWith(BVTree* tree, WorldEntity* nodeA, WorldEntity* nodeB) = 0;
-  virtual void collideWith(WorldEntity* entity1, WorldEntity* entity2) = 0;
+    virtual void collideWith(WorldEntity* entity1, WorldEntity* entity2) = 0;
 
-  /** @param depth: depth, @param drawMode: how to draw the Model */
-  virtual void drawBV(int depth, int drawMode) const = 0;
-
-
- protected:
-  int numberOfVertices;
-
- private:
-
-
+    virtual void drawBV(int depth, int drawMode) const = 0;
 };
 
Index: trunk/src/lib/collision_detection/bv_tree_node.cc
===================================================================
--- trunk/src/lib/collision_detection/bv_tree_node.cc	(revision 7677)
+++ trunk/src/lib/collision_detection/bv_tree_node.cc	(revision 7711)
@@ -14,5 +14,5 @@
 */
 
-#define DEBUG_SPECIAL_MODULE DEBUG_MODULE_COLLISION
+#define DEBUG_SPECIAL_MODULE DEBUG_MODULE_COLLISION_DETECTION
 
 #include "bv_tree_node.h"
Index: trunk/src/lib/collision_detection/bv_tree_node.h
===================================================================
--- trunk/src/lib/collision_detection/bv_tree_node.h	(revision 7677)
+++ trunk/src/lib/collision_detection/bv_tree_node.h	(revision 7711)
@@ -8,9 +8,11 @@
 #define _BV_TREE_NODE_H
 
+
 #include "base_object.h"
 #include "model.h"
 #include "vector.h"
 
-// FORWARD DECLARATION
+
+// forward declarations
 class BoundingVolume;
 class BVTree;
@@ -19,6 +21,8 @@
 template<class T> class tList;
 
+
 //! A class that represents a bounding volume tree
 class BVTreeNode : public BaseObject {
+
 
  public:
@@ -26,12 +30,10 @@
   virtual ~BVTreeNode();
 
-  virtual void spawnBVTree(const int depth, const sVec3D *verticesList, unsigned int length ) = 0;
-  virtual void spawnBVTree(const int depth, const modelInfo& modInfo) = 0;
+  virtual const BoundingVolume* getBV() const = 0;
+  /** returns the index of this bounding volume tree node @returns index of this index */
+  inline const int getIndex() const { return this->treeIndex; }
 
-  virtual BoundingVolume* getBV(int index) const = 0;
-  inline const int getIndex() { return this->treeIndex; }
-
+  virtual void spawnBVTree(const modelInfo& modInfo, const int* triangleIndexes, int length) = 0;
   virtual void collideWith(BVTreeNode* treeNode, WorldEntity* nodeA, WorldEntity* nodeB) = 0;
-
   virtual void drawBV(int depth, int drawMode, const Vector& color = Vector(1,0,0), bool top = true) const = 0;
 
Index: trunk/src/lib/collision_detection/cd_engine.cc
===================================================================
--- trunk/src/lib/collision_detection/cd_engine.cc	(revision 7677)
+++ trunk/src/lib/collision_detection/cd_engine.cc	(revision 7711)
@@ -14,5 +14,5 @@
 */
 
-#define DEBUG_SPECIAL_MODULE DEBUG_MODULE_COLLISION
+#define DEBUG_SPECIAL_MODULE DEBUG_MODULE_COLLISION_DETECTION
 
 #include "cd_engine.h"
@@ -40,4 +40,6 @@
 {
   this->setClassID(CL_CD_ENGINE, "CDEngine");
+
+  this->bAbordOnFirstCollision = false;
 }
 
@@ -59,56 +61,11 @@
 
 /**
- *  this is the collision checking function
-
-    there are some speed improvements that can be done here. a rewrite of the list a would be appropriate to
-    be able to enhance iteration speed.
+ *
  */
-//void CDEngine::checkCollisions()
-//{
-//  this->checkCollisionObjects();
-  //this->checkCollisionGround();
-//}
-
-/**
- *  this checks the collisions with the objects
- */
-//void CDEngine::checkCollisionObjects()
-//{
-//   BVTree* tree;
-//   tIterator<WorldEntity>* iterator1 = entityList->getIterator();
-//   tIterator<WorldEntity>* iterator2 = entityList->getIterator();
-//   WorldEntity* entity1 = iterator1->firstElement();
-//   WorldEntity* entity2 = iterator2->iteratorElement(iterator1);
-//   PRINTF(3)("checking for collisions\n");
-//   while( entity1 != NULL)
-//   {
-//     if( likely(entity1 != this->terrain))
-//     {
-//       entity2 = iterator2->nextElement();
-//
-//       while( entity2 != NULL)
-//       {
-//         if( likely(entity2 != this->terrain))
-//         {
-//           PRINTF(4)("checking object %s against %s\n", entity1->getName(), entity2->getName());
-//           tree = entity1->getOBBTree();
-//           if( likely(tree != NULL) && entity2->getOBBTree() != NULL) tree->collideWith(entity1, entity2);
-//         }
-//         entity2 = iterator2->nextElement();
-//       }
-//     }
-//     entity1 = iterator1->nextElement();
-//     entity2 = iterator2->iteratorElement(iterator1);
-//     entity2 = iterator2->nextElement();
-//   }
-//   delete iterator1;
-//   delete iterator2;
-//}
-
 void CDEngine::checkCollisions(ObjectManager::EntityList& list1, ObjectManager::EntityList& list2)
 {
   BVTree* tree;
   ObjectManager::EntityList::iterator entity1, entity2, pre1, pre2;
-  PRINTF(3)("checking for collisions\n");
+  PRINTF(5)("checking for collisions\n");
 
   pre1 = list1.begin();
@@ -126,5 +83,6 @@
           PRINTF(4)("checking object %s against %s\n", (*entity1)->getName(), (*entity2)->getName());
           tree = (*entity1)->getOBBTree();
-          if( likely(tree != NULL) && (*entity2)->getOBBTree() != NULL) tree->collideWith(*entity1, *entity2);
+          if( likely(tree != NULL) && (*entity2)->getOBBTree() != NULL)
+            tree->collideWith(*entity1, *entity2);
         }
       }
@@ -150,25 +108,4 @@
 
 /**
- *  this draws the bounding volume tree
- * @param depth until which depth to draw the tree
- * @param drawMode mod which states how to draw it
- */
-void CDEngine::drawBV(int depth, int drawMode) const
-{
-  /* this would operate on  worldList bases, for testing purposes, we only use one OBBTree */
-  //this->rootTree->drawBV(depth, drawMode);
-  /// FIXME
-/*  tIterator<WorldEntity>* iterator = entityList->getIterator();
-  WorldEntity* entity = iterator->firstElement();
-  while( entity != NULL)
-  {
-    entity->drawBVTree(depth, drawMode);
-    entity = iterator->nextElement();
-  }
-  delete iterator;*/
-}
-
-
-/**
  * some debug output on the class
  */
@@ -189,7 +126,6 @@
 void CDEngine::debugSpawnTree(int depth, sVec3D* vertices, int numVertices)
 {
-  if ( this->rootTree == NULL)
-    this->rootTree = new OBBTree();
-  this->rootTree->spawnBVTree(depth, vertices, numVertices);
+//   if ( this->rootTree == NULL)
+//     this->rootTree = new OBBTree(depth, vertices, numVertices);
 }
 
@@ -199,6 +135,5 @@
   ObjectManager::EntityList::const_iterator entity;
   for (entity = drawList.begin(); entity != drawList.end(); entity++)
-    if ((*entity)->isVisible())
-      (*entity)->drawBVTree(3, 226);
+    (*entity)->drawBVTree(3, 226);
 }
 
@@ -208,5 +143,5 @@
 void CDEngine::debugDraw(int depth, int drawMode)
 {
-  if(this-> rootTree != NULL)
-    this->rootTree->drawBV(depth, drawMode);
+//   if(this-> rootTree != NULL)
+//     this->rootTree->drawBV(depth, drawMode);
 }
Index: trunk/src/lib/collision_detection/cd_engine.h
===================================================================
--- trunk/src/lib/collision_detection/cd_engine.h	(revision 7677)
+++ trunk/src/lib/collision_detection/cd_engine.h	(revision 7711)
@@ -50,9 +50,5 @@
 
   inline void setTerrain(Terrain* terrain) { this->terrain = terrain; }
-  //  inline void setPlayer(Player* player) { this->player = player; } /* only for debug purposes \todo: delete*/
 
-  void drawBV(int depth, int drawMode) const;
-
-//  void checkCollisions();
   void checkCollisions(std::list<WorldEntity*>& list1, std::list<WorldEntity*>& list2);
 
@@ -76,8 +72,8 @@
  private:
   int                     state;                            //!< the current state of the cd engine
+  bool                    bAbordOnFirstCollision;           //!< abords the collision detection on first collision event => more speed but less accurate infomations
   OBBTree*                rootTree;                         //!< for testing purposes a root tree
 
   Terrain*                terrain;                          //!< this it a ref to the terrain, serving as a ground for all WE
-//  Player*                 player;
 };
 
Index: trunk/src/lib/collision_detection/collision.cc
===================================================================
--- trunk/src/lib/collision_detection/collision.cc	(revision 7677)
+++ trunk/src/lib/collision_detection/collision.cc	(revision 7711)
@@ -1,3 +1,3 @@
-/* 
+/*
    orxonox - the future of 3D-vertical-scrollers
 
@@ -14,5 +14,5 @@
 */
 
-#define DEBUG_SPECIAL_MODULE DEBUG_MODULE_COLLISION
+#define DEBUG_SPECIAL_MODULE DEBUG_MODULE_COLLISION_DETECTION
 
 #include "collision.h"
@@ -24,7 +24,7 @@
  *  standard constructor
 */
-Collision::Collision () 
+Collision::Collision ()
 {
-   this->setClassID(CL_COLLISION, "Collision"); 
+   this->setClassID(CL_COLLISION, "Collision");
 
 }
@@ -35,5 +35,5 @@
 
 */
-Collision::~Collision () 
+Collision::~Collision ()
 {
   // delete what has to be deleted here
Index: trunk/src/lib/collision_detection/obb.cc
===================================================================
--- trunk/src/lib/collision_detection/obb.cc	(revision 7677)
+++ trunk/src/lib/collision_detection/obb.cc	(revision 7711)
@@ -14,5 +14,5 @@
 */
 
-#define DEBUG_SPECIAL_MODULE DEBUG_MODULE_COLLISION
+#define DEBUG_SPECIAL_MODULE DEBUG_MODULE_COLLISION_DETECTION
 
 #include "obb.h"
@@ -28,6 +28,5 @@
 {
    this->setClassID(CL_OBB, "OBB");
-   this->axis = new Vector[3];
-   this->halfLength = new float[3];
+   this->halfLength[0] = this->halfLength[1] = this->halfLength[2] = 0.0f;
    this->bCollided = false;
 }
@@ -40,6 +39,4 @@
 OBB::~OBB ()
 {
-  delete [] this->axis;
-  delete [] this->halfLength;
 }
 
Index: trunk/src/lib/collision_detection/obb.h
===================================================================
--- trunk/src/lib/collision_detection/obb.h	(revision 7677)
+++ trunk/src/lib/collision_detection/obb.h	(revision 7711)
@@ -21,5 +21,8 @@
 
 
-  inline Vector* getAxis () const { return this->axis; }
+  inline Vector getAxisX () const { return this->axis[0]; }
+  inline Vector getAxisY () const { return this->axis[1]; }
+  inline Vector getAxisZ () const { return this->axis[2]; }
+
   inline const float* getHalfLength() const { return this->halfLength; }
 
@@ -31,6 +34,6 @@
 
  public:
-  Vector*          axis;                       //!< Axes of oriented box [x,y,z]
-  float*           halfLength;                 //!< Half lengths of the box along the axis
+  Vector           axis[3];                    //!< Axes of oriented box [x,y,z]
+  float            halfLength[3];              //!< Half lengths of the box along the axis
   float            covarianceMatrix[3][3];     //!< the covariance matrix
 
Index: trunk/src/lib/collision_detection/obb_tree.cc
===================================================================
--- trunk/src/lib/collision_detection/obb_tree.cc	(revision 7677)
+++ trunk/src/lib/collision_detection/obb_tree.cc	(revision 7711)
@@ -14,5 +14,5 @@
 */
 
-#define DEBUG_SPECIAL_MODULE DEBUG_MODULE_COLLISION
+#define DEBUG_SPECIAL_MODULE DEBUG_MODULE_COLLISION_DETECTION
 
 #include "obb_tree.h"
@@ -31,19 +31,11 @@
  *  standard constructor
 */
-OBBTree::OBBTree ()
+OBBTree::OBBTree(int depth, const modelInfo* modelInf, WorldEntity* owner)
+  : BVTree()
 {
+  this->depth = depth;
   this->init();
-}
-
-OBBTree::OBBTree(int depth, sVec3D *verticesList, const int length)
-{
-  this->init();
-  this->spawnBVTree(depth, verticesList, length);
-}
-
-OBBTree::OBBTree(int depth, const modelInfo& modInfo)
-{
-  this->init();
-  this->spawnBVTree(depth, modInfo);
+  this->spawnBVTree(*modelInf);
+  this->owner = owner;
 }
 
@@ -53,7 +45,5 @@
 {
   this->setClassID(CL_OBB_TREE, "OBBTree");
-
   this->rootNode = NULL;
-
   this->id = 0;
 }
@@ -69,19 +59,9 @@
 
 
-void OBBTree::spawnBVTree(int depth, sVec3D *verticesList, const int length)
-{
-  if( unlikely(this->rootNode != NULL))
-    {
-      PRINTF(2)("The BVTree has already been spawned, flushing and respawning again...\n");
-      this->flushTree();
-    }
-  OBBTreeNode* node = new OBBTreeNode();
-  this->rootNode = node;
-  this->rootNode->setTreeRef(this);
-  this->rootNode->spawnBVTree(--depth, verticesList, length);
-}
-
-
-void OBBTree::spawnBVTree(int depth, const modelInfo& modInfo)
+/**
+ *  this function creates a bv tree out of a modelInf structure
+ * @param modelInf the model info of a model (modelInfo), containing vertices, triangle and normal infos
+ */
+void OBBTree::spawnBVTree(const modelInfo& modelInf)
 {
   if( unlikely(this->rootNode != NULL))
@@ -90,15 +70,27 @@
     this->flushTree();
   }
-  OBBTreeNode* node = new OBBTreeNode();
-  this->rootNode = node;
-  this->rootNode->setTreeRef(this);
-  this->rootNode->spawnBVTree(--depth, modInfo);
+  this->rootNode = new OBBTreeNode(*this, NULL, depth-1);
+
+  /* triangles indexes created */
+  int* triangleIndexes = new int[modelInf.numTriangles];
+  for( int i = 0; i < modelInf.numTriangles; ++i)
+    triangleIndexes[i] = i;
+
+  this->rootNode->spawnBVTree(modelInf, triangleIndexes, modelInf.numTriangles);
 }
 
 
+/**
+ *  release the current bv tree if any
+ */
 void OBBTree:: flushTree()
 {}
 
 
+/**
+ * this collides two bvtrees together. the trees are attached to pnodes Objects A and B
+ * @param nodeA: PNode of object A
+ * @param nodeB: Pnode of object B
+ */
 void OBBTree::collideWith(WorldEntity* entity1, WorldEntity* entity2)
 {
@@ -109,15 +101,6 @@
 
 /**
- * this collides two bvtrees together. the trees are attached to pnodes Objects A and B
- * @param tree: the other tree to collide with (Object B)
- * @param nodeA: PNode of object A
- * @param nodeB: Pnode of object B
+ *  draw bv tree
  */
-void OBBTree::collideWith(BVTree* tree, WorldEntity* nodeA, WorldEntity* nodeB)
-{
-  this->rootNode->collideWith(((OBBTree*)tree)->getRootNode(), nodeA, nodeB);
-}
-
-
 void OBBTree::drawBV(int depth, int drawMode) const
 {
@@ -129,5 +112,9 @@
 
 
-
+/**
+ *  some debug output and creation function
+ *
+ * doesn't work at the moment
+ */
 void OBBTree::debug()
 {
@@ -153,5 +140,5 @@
     }
 
-  this->spawnBVTree(3, vertList, length);
+//   this->spawnBVTree(vertList, length);
 
   PRINT(0)("=  Spawning Tree: Finished\n");
Index: trunk/src/lib/collision_detection/obb_tree.h
===================================================================
--- trunk/src/lib/collision_detection/obb_tree.h	(revision 7677)
+++ trunk/src/lib/collision_detection/obb_tree.h	(revision 7711)
@@ -15,26 +15,26 @@
 class OBBTreeNode;
 class PNode;
+class WorldEntity;
 
 //! A class for representing an obb tree
-class OBBTree : public BVTree {
+class OBBTree : public BVTree
+{
 
   public:
-    OBBTree();
-    OBBTree(int depth, sVec3D *verticesList, const int length);
-    OBBTree(int depth, const modelInfo& modInfo);
+    OBBTree(int depth, const modelInfo* modInfo, WorldEntity* entity);
     virtual ~OBBTree();
     void init();
 
-    virtual void spawnBVTree(int depth, sVec3D *verticesList, const int length);
-    virtual void spawnBVTree(int depth, const modelInfo& modInfo);
+    virtual void spawnBVTree(const modelInfo& modelInf);
     virtual void flushTree();
 
-    virtual void collideWith(BVTree* tree, WorldEntity* nodeA, WorldEntity* nodeB);
     virtual void collideWith(WorldEntity* entity1, WorldEntity* entity2);
-
     virtual void drawBV(int depth, int drawMode) const;
 
-    int getID() { return ++this->id;}
-    inline OBBTreeNode* getRootNode() { return this->rootNode; }
+    /** returns the next if for the obb tree node @return integer id number of the next node */
+    inline const int getID() { return ++this->id;}
+    /** returns the root node of the bounding volume tree @return reference to the root node */
+    inline OBBTreeNode* getRootNode() const { return this->rootNode; }
+    inline WorldEntity* getOwner() const { return this->owner; }
 
     void debug();
@@ -42,5 +42,7 @@
   private:
     OBBTreeNode*         rootNode;                        //!< reference to the root node of the tree
-    int                  id;
+    int                  id;                              //!< the next id of a obb tree node
+    int                  depth;                           //!< the depth of the tree to generate
+    WorldEntity*         owner;                           //!< owner
 };
 
Index: trunk/src/lib/collision_detection/obb_tree_node.cc
===================================================================
--- trunk/src/lib/collision_detection/obb_tree_node.cc	(revision 7677)
+++ trunk/src/lib/collision_detection/obb_tree_node.cc	(revision 7711)
@@ -11,13 +11,12 @@
 ### File Specific:
    main-programmer: Patrick Boenzli
-   co-programmer: ...
 */
 
-#define DEBUG_SPECIAL_MODULE DEBUG_MODULE_COLLISION
+#define DEBUG_SPECIAL_MODULE 3/* DEBUG_MODULE_COLLISION_DETECTION*/
 
 #include "obb_tree_node.h"
-#include "list.h"
+#include "obb_tree.h"
 #include "obb.h"
-#include "obb_tree.h"
+
 #include "matrix.h"
 #include "model.h"
@@ -26,7 +25,9 @@
 
 #include "color.h"
-
+#include "glincl.h"
+
+#include <list>
+#include <vector>
 #include "debug.h"
-#include "glincl.h"
 
 
@@ -34,43 +35,43 @@
 using namespace std;
 
-OBBTree*  OBBTreeNode::obbTree = NULL;
-
-float**  OBBTreeNode::coMat = NULL;
-float**  OBBTreeNode::eigvMat = NULL;
-float*   OBBTreeNode::eigvlMat = NULL;
-int*     OBBTreeNode::rotCount = NULL;
+
 GLUquadricObj* OBBTreeNode_sphereObj = NULL;
+
 
 /**
  *  standard constructor
+ * @param tree: reference to the obb tree
+ * @param depth: the depth of the obb tree to generate
  */
-OBBTreeNode::OBBTreeNode ()
+OBBTreeNode::OBBTreeNode (const OBBTree& tree, OBBTreeNode* prev, int depth)
+    : BVTreeNode()
 {
   this->setClassID(CL_OBB_TREE_NODE, "OBBTreeNode");
+
+  this->obbTree = &tree;
+  this->nodePrev = prev;
+  this->depth = depth;
+  this->nextID = 0;
+
   this->nodeLeft = NULL;
   this->nodeRight = NULL;
   this->bvElement = NULL;
 
-  if( OBBTreeNode::coMat == NULL)
-  {
-    OBBTreeNode::coMat = new float*[4];
-    for(int i = 0; i < 4; i++)
-      OBBTreeNode::coMat[i] = new float[4];
-  }
-  if( OBBTreeNode::eigvMat == NULL)
-  {
-    OBBTreeNode::eigvMat = new float*[4];
-    for( int i = 0; i < 4; i++)
-      OBBTreeNode::eigvMat[i] = new float[4];
-  }
-  if( OBBTreeNode::eigvlMat == NULL)
-  {
-    OBBTreeNode::eigvlMat = new float[4];
-  }
-  if( OBBTreeNode::rotCount == NULL)
-    OBBTreeNode::rotCount = new int;
+  this->triangleIndexList1 = NULL;
+  this->triangleIndexList2 = NULL;
+
+  this->modelInf = NULL;
+  this->triangleIndexes = NULL;
 
   if( OBBTreeNode_sphereObj == NULL)
     OBBTreeNode_sphereObj = gluNewQuadric();
+
+  this->owner = NULL;
+
+  /* debug ids */
+  if( this->nodePrev)
+    this->treeIndex = 100 * this->depth + this->nodePrev->getID();
+  else
+    this->treeIndex = 0;
 }
 
@@ -82,16 +83,15 @@
 {
   if( this->nodeLeft)
-  {
     delete this->nodeLeft;
-    this->nodeLeft = NULL;
-  }
   if( this->nodeRight)
-  {
     delete this->nodeRight;
-    this->nodeRight = NULL;
-  }
+
   if( this->bvElement)
     delete this->bvElement;
-  this->bvElement = NULL;
+
+//   if( this->triangleIndexList1 != NULL)
+//     delete [] this->triangleIndexList1;
+//   if( this->triangleIndexList2 != NULL)
+//     delete [] this->triangleIndexList2;
 }
 
@@ -105,126 +105,77 @@
  * on the triangle informations (triangle soup not polygon soup)
  */
-void OBBTreeNode::spawnBVTree(const int depth, const modelInfo& modInfo)
-{
-  int length = 0;
-  sVec3D* verticesList;
-
-  PRINT(3)("\n");
-  this->treeIndex = this->obbTree->getID();
-  PRINTF(3)("OBB Depth: %i, tree index: %i, numVertices: %i\n", depth, treeIndex, length);
+void OBBTreeNode::spawnBVTree(const modelInfo& modelInf, const int* triangleIndexes, int length)
+{
+  PRINTF(3)("\n==============================Creating OBB Tree Node==================\n");
+  PRINT(3)(" OBB Tree Infos: \n");
+  PRINT(3)("\tDepth: %i \n\tTree Index: %i \n\tNumber of Triangles: %i\n", depth, this->treeIndex, length);
   this->depth = depth;
 
-
   this->bvElement = new OBB();
-  this->bvElement->vertices = verticesList;
-  this->bvElement->numOfVertices = length;
-  PRINTF(3)("Created OBBox\n");
-  this->calculateBoxCovariance(this->bvElement, modInfo);
-  PRINTF(3)("Calculated attributes1\n");
-  this->calculateBoxEigenvectors(this->bvElement, modInfo);
-  PRINTF(3)("Calculated attributes2\n");
-  this->calculateBoxAxis(this->bvElement,modInfo);
-  PRINTF(3)("Calculated attributes3\n");
-
-  /* if this is the first node, the vertices data are the original ones of the model itself, so dont delete them in cleanup */
-  if( this->treeIndex == 1)
-    this->bvElement->bOrigVertices = true;
-
+  this->bvElement->modelInf = &modelInf;
+  this->bvElement->triangleIndexes = triangleIndexes;
+  this->bvElement->triangleIndexesLength = length;
+
+  // full debug output
+
+  //indexes debug
+//   for( int i = 0; i < length; ++i)
+//   {
+//     PRINTF(2)("triangle[%i], index: %i\n", i, triangleIndexes[i]);
+//   }
+//   for( int i = 0; i < length; ++i)
+//   {
+//     PRINTF(2)("triangle[%i]\n", i);
+//     for(int j = 0; j < 3; ++j)
+//     {
+//       PRINTF(2)("  vertex[%i]: %f, %f, %f\n", j,
+//       (&modelInf.pVertices[modelInf.pTriangles[triangleIndexes[i]].indexToVertices[j]])[0],
+//       (&modelInf.pVertices[modelInf.pTriangles[triangleIndexes[i]].indexToVertices[j]])[1],
+//       (&modelInf.pVertices[modelInf.pTriangles[triangleIndexes[i]].indexToVertices[j]])[2]);
+//     }
+//   }
+//   PRINT(0)("\n\n\n");
+//   for( int i = 0; i < this->bvElement->modelInf->numVertices*3; i+=3)
+//     PRINTF(0)(  "vertex[%i]: %f, %f, %f\n", i, this->bvElement->modelInf->pVertices[i],
+//                this->bvElement->modelInf->pVertices[i+1],
+//                this->bvElement->modelInf->pVertices[i+2]);
+
+
+
+  /* create the bounding boxes in three steps */
+  this->calculateBoxCovariance(*this->bvElement, modelInf, triangleIndexes, length);
+  this->calculateBoxEigenvectors(*this->bvElement, modelInf, triangleIndexes, length);
+//   this->calculateBoxAxis(*this->bvElement, modelInf, triangleIndexes, length);
+//   this->calculateBoxAxis(*this->bvElement, modelInf, triangleIndexes, length);
+  this->calculateBoxAxis(*this->bvElement, modelInf, triangleIndexes, length);
+
+  /* do we need to descent further in the obb tree?*/
   if( likely( this->depth > 0))
   {
-    this->forkBox(this->bvElement);
-
-
-//     if(this->tmpLen1 > 2)
-//     {
-//       OBBTreeNode* node1 = new OBBTreeNode();
-//       this->nodeLeft = node1;
-//       this->nodeLeft->spawnBVTree(depth - 1, this->tmpVert1, this->tmpLen1);
-//     }
-//     else
-//     {
-//       PRINTF(3)("Aboarding tree walk: less than 3 vertices left\n");
-//     }
-//
-//     if( this->tmpLen2 > 2)
-//     {
-//       OBBTreeNode* node2 = new OBBTreeNode();
-//       this->nodeRight = node2;
-//       this->nodeRight->spawnBVTree(depth - 1, this->tmpVert2, this->tmpLen2);
-//     }
-//     else
-//     {
-//       PRINTF(3)("Abording tree walk: less than 3 vertices left\n");
-//     }
-
-  }
-}
+    this->forkBox(*this->bvElement);
+
+    if( this->triangleIndexLength1 >= 3)
+    {
+      this->nodeLeft = new OBBTreeNode(*this->obbTree, this, depth - 1);
+      this->nodeLeft->spawnBVTree(modelInf, this->triangleIndexList1, this->triangleIndexLength1);
+    }
+    if( this->triangleIndexLength2 >= 3)
+    {
+      this->nodeRight = new OBBTreeNode(*this->obbTree, this, depth - 1);
+      this->nodeRight->spawnBVTree(modelInf, this->triangleIndexList2, this->triangleIndexLength2);
+    }
+  }
+}
+
 
 
 /**
- *  creates a new BVTree or BVTree partition
- * @param depth: how much more depth-steps to go: if == 1 don't go any deeper!
- * @param verticesList: the list of vertices of the object - each vertices triple is interpreted as a triangle
- *
- * this function creates an Bounding Volume tree from a vertices soup (no triangle data)
+ *  calculate the box covariance matrix
+ * @param box: reference to the box
+ * @param modelInf: the model info structure of the model
+ * @param tirangleIndexes: an array with the indexes of the triangles inside this
+ * @param length: the length of the indexes array
  */
-void OBBTreeNode::spawnBVTree(const int depth, const sVec3D *verticesList, unsigned int length)
-{
-  PRINT(3)("\n");
-  this->treeIndex = this->obbTree->getID();
-  PRINTF(3)("OBB Depth: %i, tree index: %i, numVertices: %i\n", depth, treeIndex, length);
-  this->depth = depth;
-
-
-  this->bvElement = new OBB();
-  this->bvElement->vertices = verticesList;
-  this->bvElement->numOfVertices = length;
-  PRINTF(3)("Created OBBox\n");
-  this->calculateBoxCovariance(this->bvElement, verticesList, length);
-  PRINTF(3)("Calculated attributes1\n");
-  this->calculateBoxEigenvectors(this->bvElement, verticesList, length);
-  PRINTF(3)("Calculated attributes2\n");
-  this->calculateBoxAxis(this->bvElement, verticesList, length);
-  PRINTF(3)("Calculated attributes3\n");
-
-  /* if this is the first node, the vertices data are the original ones of the model itself, so dont delete them in cleanup */
-  if( this->treeIndex == 1)
-    this->bvElement->bOrigVertices = true;
-
-  if( likely( this->depth > 0))
-  {
-    this->forkBox(this->bvElement);
-
-
-    if(this->tmpLen1 > 2)
-    {
-      OBBTreeNode* node1 = new OBBTreeNode();
-      this->nodeLeft = node1;
-      this->nodeLeft->spawnBVTree(depth - 1, this->tmpVert1, this->tmpLen1);
-    }
-    else
-    {
-      PRINTF(3)("Aboarding tree walk: less than 3 vertices left\n");
-    }
-
-    if( this->tmpLen2 > 2)
-    {
-      OBBTreeNode* node2 = new OBBTreeNode();
-      this->nodeRight = node2;
-      this->nodeRight->spawnBVTree(depth - 1, this->tmpVert2, this->tmpLen2);
-    }
-    else
-    {
-      PRINTF(3)("Abording tree walk: less than 3 vertices left\n");
-    }
-  }
-}
-
-
-void OBBTreeNode::calculateBoxCovariance(OBB* box, const modelInfo& modInfo)
-{}
-
-
-void OBBTreeNode::calculateBoxCovariance(OBB* box, const sVec3D* verticesList, unsigned int length)
+void OBBTreeNode::calculateBoxCovariance(OBB& box, const modelInfo& modelInf, const int* triangleIndexes, int length)
 {
   float     facelet[length];                         //!< surface area of the i'th triangle of the convex hull
@@ -235,191 +186,91 @@
   Vector    t1, t2;                                  //!< temporary values
   float     covariance[3][3] = {0,0,0, 0,0,0, 0,0,0};//!< the covariance matrix
-  int       mode = 0;                                //!< mode = 0: vertex soup, no connections, mode = 1: 3 following verteces build a triangle
-
-  this->numOfVertices = length;
-  this->vertices = verticesList;
-
-
-  if( likely(mode == 0))
-  {
-    /* fist compute all the convex hull face/facelets and centroids */
-    for( int i = 0; i+3 < length ; i+=3)          /* FIX-ME-QUICK: hops of 3, array indiscontinuity*/
-    {
-      p = verticesList[i];
-      q = verticesList[i + 1];
-      r = verticesList[i + 2];
-
-      t1 = p - q; t2 = p - r;
-
-      /* finding the facelet surface via cross-product */
-      facelet[i] = 0.5f * fabs( t1.cross(t2).len() );
-      /* update the entire convex hull surface */
-      face += facelet[i];
-
-      /* calculate the cetroid of the hull triangles */
-      centroid[i] = (p + q + r) * 1/3;
-      /* now calculate the centroid of the entire convex hull, weighted average of triangle centroids */
-      center += centroid[i] * facelet[i];
-    }
-    /* take the average of the centroid sum */
-    center /= face;
-    PRINTF(3)("-- Calculated Center\n");
-
-
-    /* now calculate the covariance matrix - if not written in three for-loops, it would compute faster: minor */
-    for( int j = 0; j < 3; ++j)
-    {
-      for( int k = 0; k < 3; ++k)
+  const float*   tmpVec = NULL;                           //!< a temp saving place for sVec3Ds
+
+  /* fist compute all the convex hull face/facelets and centroids */
+  for( int i = 0; i < length ; ++i)
+  {
+    p = &modelInf.pVertices[modelInf.pTriangles[triangleIndexes[i]].indexToVertices[0]];
+    q = &modelInf.pVertices[modelInf.pTriangles[triangleIndexes[i]].indexToVertices[1]];
+    r = &modelInf.pVertices[modelInf.pTriangles[triangleIndexes[i]].indexToVertices[2]];
+
+    /* finding the facelet surface via cross-product */
+    t1 = p - q;
+    t2 = p - r;
+    facelet[i] = 0.5f * /*fabs*/( t1.cross(t2).len() );
+    /* update the entire convex hull surface */
+    face += facelet[i];
+
+    /* calculate the cetroid of the hull triangles */
+    centroid[i] = (p + q + r) / 3.0f;
+    /* now calculate the centroid of the entire convex hull, weighted average of triangle centroids */
+    center += centroid[i] * facelet[i];
+    /* the arithmetical center */
+  }
+  /* take the average of the centroid sum */
+  center /= face;
+
+
+  /* now calculate the covariance matrix - if not written in three for-loops,
+     it would compute faster: minor */
+  for( int j = 0; j < 3; ++j)
+  {
+    for( int k = 0; k < 3; ++k)
+    {
+      for( int i = 0; i < length; ++i)
       {
-        for( int i = 0; i + 3 < length; i+=3)
-        {
-          p = verticesList[i];
-          q = verticesList[i + 1];
-          r = verticesList[i + 2];
-
-          covariance[j][k] = facelet[i] / (12.0f * face) * (9.0f * centroid[i][j] * centroid[i][k] + p[j] * p[k] +
-              q[j] * q[k] + r[j] * r[k]) - center[j] * center[k];
-        }
+        p = (&modelInf.pVertices[modelInf.pTriangles[triangleIndexes[i]].indexToVertices[0]]);
+        q = (&modelInf.pVertices[modelInf.pTriangles[triangleIndexes[i]].indexToVertices[1]]);
+        r = (&modelInf.pVertices[modelInf.pTriangles[triangleIndexes[i]].indexToVertices[2]]);
+
+        covariance[j][k] = facelet[i] * (9.0f * centroid[i][j] * centroid[i][k] + p[j] * p[k] +
+                           q[j] * q[k] + r[j] * r[k]);
       }
-    }
-    PRINTF(3)("-- Calculated Covariance\n");
-  }
-  else if( mode == 1)
-  {
-    for( int i = 0; i + 3 < length; i+=3)          /* FIX-ME-QUICK: hops of 3, array indiscontinuity*/
-    {
-      p = verticesList[i];
-      q = verticesList[i + 1];
-      r = verticesList[i + 2];
-
-      centroid[i] = (p + q + r) / 3.0f;
-      center += centroid[i];
-    }
-    center /= length;
-
-    for( int j = 0; j < 3; ++j)
-    {
-      for( int k = 0; k < 3; ++k)
-      {
-        for( int i = 0; i + 3 < length; i+=3)
-        {
-          p = verticesList[i];
-          q = verticesList[i +1];
-          r = verticesList[i + 2];
-
-          covariance[j][k] = p[j] * p[k] + q[j] * q[k] + r[j] + r[k];
-        }
-        covariance[j][k] /= (3.0f * length);
-      }
-    }
-    PRINTF(3)("-- Calculated Covariance\n");
-  }
-  else if( mode == 2)
-  {
-    /* fist compute all the convex hull face/facelets and centroids */
-    for(int i = 0; i + 3 < length; i+=3)          /* FIX-ME-QUICK: hops of 3, array indiscontinuity*/
-    {
-      p = verticesList[i];
-      q = verticesList[i + 1];
-      r = verticesList[i + 2];
-
-      t1 = p - q; t2 = p - r;
-
-      /* finding the facelet surface via cross-product */
-      facelet[i] = 0.5f * fabs( t1.cross(t2).len() );
-      /* update the entire convex hull surface */
-      face += facelet[i];
-
-      /* calculate the cetroid of the hull triangles */
-      centroid[i] = (p + q + r) * 1/3;
-      /* now calculate the centroid of the entire convex hull, weighted average of triangle centroids */
-      center += centroid[i] * facelet[i];
-    }
-    /* take the average of the centroid sum */
-    center /= face;
-    PRINTF(3)("-- Calculated Center\n");
-
-    for( int j = 0; j < 3; ++j)
-    {
-      for( int k = 0; k < 3; ++k)
-      {
-        for( int i = 0; i + 3 < length; i+=3)
-        {
-          p = verticesList[i];
-          q = verticesList[i +1];
-          r = verticesList[i + 2];
-
-          covariance[j][k] = p[j] * p[k] + q[j] * q[k] + r[j] + r[k];
-        }
-        covariance[j][k] /= (3.0f * length);
-      }
-    }
-    PRINTF(3)("-- Calculated Covariance\n");
-  }
-  else
-  {
-    for( int i = 0; i < length; ++i)          /* FIX-ME-QUICK: hops of 3, array indiscontinuity*/
-    {
-      center += verticesList[i];
-    }
-    center /= length;
-
-    for( int j = 0; j < 3; ++j)
-    {
-      for( int k = 0; k < 3; ++k)
-      {
-        for( int i = 0; i + 3 < length; i+=3)
-        {
-          p = verticesList[i];
-          q = verticesList[i +1];
-          r = verticesList[i + 2];
-
-          covariance[j][k] = p[j] * p[k] + q[j] * q[k] + r[j] + r[k];
-        }
-        covariance[j][k] /= (3.0f * length);
-      }
-    }
-    PRINTF(3)("-- Calculated Covariance\n");
-  }
-
-  PRINTF(3)("\nVertex Data:\n");
-  for(int i = 0; i < length; i++)
-  {
-    PRINTF(3)("vertex %i: %f, %f, %f\n", i, box->vertices[i][0], box->vertices[i][1], box->vertices[i][2]);
-  }
-
-
-  PRINTF(3)("\nCovariance Matrix:\n");
+      covariance[j][k] = covariance[j][k] / (12.0f * face) - center[j] * center[k];
+    }
+  }
+  for( int i = 0; i < 3; ++i)
+  {
+    box.covarianceMatrix[i][0] = covariance[i][0];
+    box.covarianceMatrix[i][1] = covariance[i][1];
+    box.covarianceMatrix[i][2] = covariance[i][2];
+  }
+  box.center = center;
+
+  /* debug output section*/
+  PRINTF(3)("\nOBB Covariance Matrix:\n");
   for(int j = 0; j < 3; ++j)
   {
-    PRINT(3)(" |");
+    PRINT(3)("\t\t");
     for(int k = 0; k < 3; ++k)
     {
-      PRINT(3)(" \b%f ", covariance[j][k]);
-    }
-    PRINT(3)(" |\n");
-  }
-
-  PRINTF(3)("center: %f, %f, %f\n", center.x, center.y, center.z);
-
-
-  for(int i = 0; i < 3; ++i)
-  {
-    box->covarianceMatrix[i][0] = covariance[i][0];
-    box->covarianceMatrix[i][1] = covariance[i][1];
-    box->covarianceMatrix[i][2] = covariance[i][2];
-  }
-  *box->center = center;
-  PRINTF(3)("-- Written Result to obb\n");
-}
-
-
-void OBBTreeNode::calculateBoxEigenvectors(OBB* box, const modelInfo& modInfo)
-{}
-
-void OBBTreeNode::calculateBoxEigenvectors(OBB* box, const sVec3D* verticesList, unsigned int length)
-{
-
-  /* now getting spanning vectors of the sub-space:
+      PRINT(3)("%11.4f\t", covariance[j][k]);
+    }
+    PRINT(3)("\n");
+  }
+  PRINTF(3)("\nWeighteed OBB Center:\n\t\t%11.4f\t %11.4f\t %11.4f\n", center.x, center.y, center.z);
+//   PRINTF(3)("\nArithmetical OBB Center:\n\t\t%11.4f\t %11.4f\t %11.4f\n", box.arithCenter.x, box.arithCenter.y, box.arithCenter.z);
+
+  /* write back the covariance matrix data to the object oriented bouning box */
+}
+
+
+
+/**
+ *  calculate the eigenvectors for the object oriented box
+ * @param box: reference to the box
+ * @param modelInf: the model info structure of the model
+ * @param tirangleIndexes: an array with the indexes of the triangles inside this
+ * @param length: the length of the indexes array
+ */
+void OBBTreeNode::calculateBoxEigenvectors(OBB& box, const modelInfo& modelInf,
+    const int* triangleIndexes, int length)
+{
+
+  Vector         axis[3];                            //!< the references to the obb axis
+  Matrix         covMat(  box.covarianceMatrix  );   //!< covariance matrix (in the matrix dataform)
+
+  /*
+  now getting spanning vectors of the sub-space:
   the eigenvectors of a symmertric matrix, such as the
   covarience matrix are mutually orthogonal.
@@ -427,142 +278,98 @@
   vectors
   */
-  Vector*              axis = new Vector[3];                //!< the references to the obb axis
-
-  Matrix covMat(  box->covarianceMatrix  );
+
+  /* calculate the axis */
   covMat.getEigenVectors(axis[0], axis[1], axis[2] );
-
-
-  /* new jacobi tests */
-//  JacobI(OBBTreeNode::coMat, OBBTreeNode::eigvlMat, OBBTreeNode::eigvMat, OBBTreeNode::rotCount);
-//  PRINTF(3)("-- Done Jacobi Decomposition\n");
-
-
-//   PRINTF(0)("Jacobi\n");
-//   for(int j = 0; j < 3; ++j)
-//   {
-//     printf(" |");
-//     for(int k = 0; k < 3; ++k)
-//     {
-//       printf(" \t%f ", OBBTreeNode::OBBTreeNode::eigvMat[j][k]);
-//     }
-//     printf(" |\n");
-//   }
-
-/*  axis[0].x = OBBTreeNode::eigvMat[0][0]; axis[0].y = OBBTreeNode::eigvMat[1][0]; axis[0].z = OBBTreeNode::eigvMat[2][0];
-  axis[1].x = OBBTreeNode::eigvMat[0][1]; axis[1].y = OBBTreeNode::eigvMat[1][1]; axis[1].z = OBBTreeNode::eigvMat[2][1];
-  axis[2].x = OBBTreeNode::eigvMat[0][2]; axis[2].y = OBBTreeNode::eigvMat[1][2]; axis[2].z = OBBTreeNode::eigvMat[2][2];
-  axis[0].normalize();
-  axis[1].normalize();
-  axis[2].normalize();*/
-  box->axis = axis;
-
-//   PRINTF(0)("-- Got Axis\n");
-//
-//   PRINTF(0)("eigenvector: %f, %f, %f\n", box->axis[0].x, box->axis[0].y, box->axis[0].z);
-//   PRINTF(0)("eigenvector: %f, %f, %f\n", box->axis[1].x, box->axis[1].y, box->axis[1].z);
-//   PRINTF(0)("eigenvector: %f, %f, %f\n", box->axis[2].x, box->axis[2].y, box->axis[2].z);
-}
-
-
-void OBBTreeNode::calculateBoxAxis(OBB* box, const modelInfo& modInfo)
-{
-  this->calculateBoxAxis(box, (const sVec3D*)modInfo.pVertices, modInfo.numVertices);
-}
-
-
-
-void OBBTreeNode::calculateBoxAxis(OBB* box, const sVec3D* verticesList, unsigned int length)
-{
-
+  box.axis[0] = axis[0];
+  box.axis[1] = axis[1];
+  box.axis[2] = axis[2];
+
+//   box.axis[0] = Vector(1,0,0);
+//   box.axis[1] = Vector(0,1,0);
+//   box.axis[2] = Vector(0,0,1);
+
+  PRINTF(3)("Eigenvectors:\n");
+  PRINT(3)("\t\t%11.2f \t%11.2f \t%11.2f\n", box.axis[0].x, box.axis[0].y, box.axis[0].z);
+  PRINT(3)("\t\t%11.2f \t%11.2f \t%11.2f\n", box.axis[1].x, box.axis[1].y, box.axis[1].z);
+  PRINT(3)("\t\t%11.2f \t%11.2f \t%11.2f\n", box.axis[2].x, box.axis[2].y, box.axis[2].z);
+}
+
+
+
+
+/**
+ *  calculate the eigenvectors for the object oriented box
+ * @param box: reference to the box
+ * @param modelInf: the model info structure of the model
+ * @param tirangleIndexes: an array with the indexes of the triangles inside this
+ * @param length: the length of the indexes array
+ */
+void OBBTreeNode::calculateBoxAxis(OBB& box, const modelInfo& modelInf, const int* triangleIndexes, int length)
+{
+
+  PRINTF(3)("Calculate Box Axis\n");
   /* now get the axis length */
-  Line                ax[3];                                 //!< the axis
-  float*              halfLength = new float[3];             //!< half length of the axis
+  float               halfLength[3];                         //!< half length of the axis
   float               tmpLength;                             //!< tmp save point for the length
-  Plane               p0(box->axis[0], *box->center);       //!< the axis planes
-  Plane               p1(box->axis[1], *box->center);
-  Plane               p2(box->axis[2], *box->center);
-  float               maxLength[3];
-  float               minLength[3];
-
-
-  /* get a bad bounding box */
-  halfLength[0] = -1.0f;
-  for(int j = 0; j < length; ++j)
-    {
-      tmpLength = fabs(p0.distancePoint(vertices[j]));
-      if( tmpLength > halfLength[0])
-        halfLength[0] = tmpLength;
-    }
-
-  halfLength[1] = -1.0f;
-  for(int j = 0; j < length; ++j)
-    {
-      tmpLength = fabs(p1.distancePoint(vertices[j]));
-      if( tmpLength > halfLength[1])
-        halfLength[1] = tmpLength;
-    }
-
-  halfLength[2] = -1.0f;
-  for(int j = 0; j < length; ++j)
-    {
-      tmpLength = fabs(p2.distancePoint(vertices[j]));
-      if( tmpLength > halfLength[2])
-        halfLength[2] = tmpLength;
-    }
-
-
+  Plane               p0(box.axis[0], box.center);           //!< the axis planes
+  Plane               p1(box.axis[1], box.center);           //!< the axis planes
+  Plane               p2(box.axis[2], box.center);           //!< the axis planes
+  float               maxLength[3];                          //!< maximal lenth of the axis
+  float               minLength[3];                          //!< minimal length of the axis
+  const float*        tmpVec;                                //!< variable taking tmp vectors
+  float               centerOffset[3];
 
   /* get the maximal dimensions of the body in all directions */
-    maxLength[0] = p0.distancePoint(vertices[0]);
-    minLength[0] = p0.distancePoint(vertices[0]);
-   for(int j = 0; j < length; ++j)
-   {
-     tmpLength = p0.distancePoint(vertices[j]);
-     if( tmpLength > maxLength[0])
-       maxLength[0] = tmpLength;
-     else if( tmpLength < minLength[0])
-       minLength[0] = tmpLength;
-   }
-
-   maxLength[1] = p1.distancePoint(vertices[0]);
-   minLength[1] = p1.distancePoint(vertices[0]);
-   for(int j = 0; j < length; ++j)
-   {
-     tmpLength = p1.distancePoint(vertices[j]);
-     if( tmpLength > maxLength[1])
-       maxLength[1] = tmpLength;
-     else if( tmpLength < minLength[1])
-       minLength[1] = tmpLength;
-   }
-
-   maxLength[2] = p2.distancePoint(vertices[0]);
-   minLength[2] = p2.distancePoint(vertices[0]);
-   for(int j = 0; j < length; ++j)
-   {
-     tmpLength = p2.distancePoint(vertices[j]);
-     if( tmpLength > maxLength[2])
-       maxLength[2] = tmpLength;
-     else if( tmpLength < minLength[2])
-       minLength[2] = tmpLength;
-   }
-
-
-   /* calculate the real centre of the body by using the axis length */
-   float centerOffset[3];
-   float newHalfLength[3];
-   for(int i = 0; i < 3; ++i)
-     {
-       PRINTF(3)("max: %f, min: %f \n", maxLength[i], minLength[i]);
-       centerOffset[i] = (maxLength[i] + minLength[i]) / 2.0f;       // min length is negatie
-       newHalfLength[i] = (maxLength[i] - minLength[i]) / 2.0f;      // min length is negative
-       *box->center +=  (box->axis[i] * centerOffset[i]);            // update the new center vector
-       halfLength[i] = newHalfLength[i];
-     }
-
-
-
-  box->halfLength = halfLength;
-  PRINTF(3)("-- Written Axis to obb\n");
-  PRINTF(3)("-- Finished Calculating Attributes\n");
+  /* for the initialisation the value just has to be inside of the polygon soup -> first vertices (rand) */
+  for( int k = 0; k  < 3; k++)
+  {
+    tmpVec = (&modelInf.pVertices[modelInf.pTriangles[triangleIndexes[0]].indexToVertices[0]]);
+    Plane* p;
+    if( k == 0)
+      p = &p0;
+    else if( k == 1)
+      p = &p1;
+    else
+      p = &p2;
+    maxLength[k] = p->distancePoint(tmpVec);
+    minLength[k] = p->distancePoint(tmpVec);
+
+    for( int j = 0; j < length; ++j) {
+      for( int i = 0; i < 3; ++i) {
+        tmpVec = &modelInf.pVertices[modelInf.pTriangles[triangleIndexes[j]].indexToVertices[i]];
+        tmpLength = p->distancePoint(tmpVec);
+
+        if( tmpLength > maxLength[k])
+          maxLength[k] = tmpLength;
+        else if( tmpLength < minLength[k])
+          minLength[k] = tmpLength;
+      }
+    }
+  }
+
+
+
+  /* calculate the real centre of the body by using the axis length */
+  for( int i = 0; i < 3; ++i)
+  {
+    if( maxLength[i] > 0.0f && minLength[i] > 0.0f)  // both axis positiv
+      centerOffset[i] = minLength[i] + (maxLength[i] - minLength[i]) / 2.0f;
+    else if( maxLength[i] > 0.0f && maxLength[i] < 0.0f) // positiv and negativ
+      centerOffset[i] = (maxLength[i] + minLength[i]) / 2.0f;
+    else //both negativ
+     centerOffset[i] = minLength[i] + (maxLength[i] - minLength[i]) / 2.0f;
+
+    box.halfLength[i] = (maxLength[i] - minLength[i]) / 2.0f;
+  }
+
+  box.center += (box.axis[0] * centerOffset[0]);
+  box.center += (box.axis[1] * centerOffset[1]);
+  box.center += (box.axis[2] * centerOffset[2]);
+
+
+  PRINTF(3)("\n");
+  PRINT(3)("\tAxis halflength x: %11.2f (max: %11.2f, \tmin: %11.2f), offset: %11.2f\n", box.halfLength[0], maxLength[0], minLength[0], centerOffset[0]);
+  PRINT(3)("\tAxis halflength y: %11.2f (max: %11.2f, \tmin: %11.2f), offset: %11.2f\n", box.halfLength[1], maxLength[1], minLength[1], centerOffset[1] );
+  PRINT(3)("\tAxis halflength z: %11.2f (max: %11.2f, \tmin: %11.2f), offset: %11.2f\n", box.halfLength[2], maxLength[2], minLength[2], centerOffset[2]);
 }
 
@@ -570,45 +377,38 @@
 
 /**
-  \brief this separates an ob-box in the middle
-* @param box: the box to separate
-
-  this will separate the box into to smaller boxes. the separation is done along the middle of the longest axis
+ *  this separates an ob-box in the middle
+ * @param box: the box to separate
+ *
+ * this will separate the box into to smaller boxes. the separation is done along the middle of the longest axis
  */
-void OBBTreeNode::forkBox(OBB* box)
-{
+void OBBTreeNode::forkBox(OBB& box)
+{
+
+  PRINTF(3)("Fork Box\n");
+  PRINTF(4)("Calculating the longest Axis\n");
   /* get the longest axis of the box */
-  float               aLength = -1.0f;                     //!< the length of the longest axis
-  int                 axisIndex = 0;                       //!< this is the nr of the longest axis
-
-  for(int i = 0; i < 3; ++i)
-  {
-    if( aLength < box->halfLength[i])
-    {
-      aLength = box->halfLength[i];
-      axisIndex = i;
-    }
-  }
-
-   PRINTF(3)("longest axis is: nr %i with a half-length of: %f\n", axisIndex, aLength);
-
-
+  float               longestAxis = -1.0f;                 //!< the length of the longest axis
+  int                 longestAxisIndex = 0;                //!< this is the nr of the longest axis
+
+
+  /* now get the longest axis of the three exiting */
+  for( int i = 0; i < 3; ++i)
+  {
+    if( longestAxis < box.halfLength[i])
+    {
+      longestAxis = box.halfLength[i];
+      longestAxisIndex = i;
+    }
+  }
+  PRINTF(3)("\nLongest Axis is: Nr %i with a half-length of:%11.2f\n", longestAxisIndex, longestAxis);
+
+
+  PRINTF(4)("Separating along the longest axis\n");
   /* get the closest vertex near the center */
   float               dist = 999999.0f;                    //!< the smallest distance to each vertex
-  float               tmpDist;                             //!< temporary distance
-  int                 vertexIndex;
-  Plane               middlePlane(box->axis[axisIndex], *box->center); //!< the middle plane
-
-  vertexIndex = 0;
-  for(int i = 0; i < box->numOfVertices; ++i)
-  {
-    tmpDist = fabs(middlePlane.distancePoint(box->vertices[i]));
-    if( tmpDist < dist)
-    {
-      dist = tmpDist;
-      vertexIndex = i;
-    }
-  }
-
-  PRINTF(3)("\nthe clostest vertex is nr: %i, with a dist of: %f\n", vertexIndex ,dist);
+  float               tmpDist;                             //!< variable to save diverse distances temporarily
+  int                 vertexIndex;                         //!< index of the vertex near the center
+  Plane               middlePlane(box.axis[longestAxisIndex], box.center); //!< the middle plane
+  const sVec3D*       tmpVec;                              //!< temp simple 3D vector
 
 
@@ -616,80 +416,67 @@
   the points depending on which side they are located
   */
-  tList<const sVec3D>      partition1;                           //!< the vertex partition 1
-  tList<const sVec3D>      partition2;                           //!< the vertex partition 2
-
-
-  PRINTF(3)("vertex index: %i, of %i\n", vertexIndex, box->numOfVertices);
-  this->separationPlane = new Plane(box->axis[axisIndex], box->vertices[vertexIndex]);  //!< separation plane
-  this->sepPlaneCenter = &box->vertices[vertexIndex];
-  this->longestAxisIndex = axisIndex;
-
-  for(int i = 0; i < box->numOfVertices; ++i)
-  {
-    if( i == vertexIndex) continue;
-    tmpDist = this->separationPlane->distancePoint(box->vertices[i]);
-    if( tmpDist > 0.0)
-      partition1.add(&box->vertices[i]); /* positive numbers plus zero */
-    else
-      partition2.add(&box->vertices[i]); /* negatice numbers */
-  }
-  partition1.add(&box->vertices[vertexIndex]);
-  partition2.add(&box->vertices[vertexIndex]);
-
-  PRINTF(3)("\npartition1: got %i vertices/ partition 2: got %i vertices\n", partition1.getSize(), partition2.getSize());
+  std::list<int>           partition1;                           //!< the vertex partition 1
+  std::list<int>           partition2;                           //!< the vertex partition 2
+  float*                   triangleCenter = new float[3];        //!< the center of the triangle
+  const float*             a;                                    //!< triangle  edge a
+  const float*             b;                                    //!< triangle  edge b
+  const float*             c;                                    //!< triangle  edge c
+
+
+  /* find the center of the box */
+  this->separationPlane = Plane(box.axis[longestAxisIndex], box.center);
+  this->sepPlaneCenter[0] = box.center.x;
+  this->sepPlaneCenter[1] = box.center.y;
+  this->sepPlaneCenter[2] = box.center.z;
+  this->longestAxisIndex = longestAxisIndex;
+
+  for( int i = 0; i < box.triangleIndexesLength; ++i)
+  {
+    /* first calculate the middle of the triangle */
+    a = &box.modelInf->pVertices[box.modelInf->pTriangles[box.triangleIndexes[i]].indexToVertices[0]];
+    b = &box.modelInf->pVertices[box.modelInf->pTriangles[box.triangleIndexes[i]].indexToVertices[1]];
+    c = &box.modelInf->pVertices[box.modelInf->pTriangles[box.triangleIndexes[i]].indexToVertices[2]];
+
+    triangleCenter[0] = (a[0] + b[0] + c[0]) / 3.0f;
+    triangleCenter[1] = (a[1] + b[1] + c[1]) / 3.0f;
+    triangleCenter[2] = (a[2] + b[2] + c[2]) / 3.0f;
+    tmpDist = this->separationPlane.distancePoint(*((sVec3D*)triangleCenter));
+
+    if( tmpDist > 0.0f)
+      partition1.push_back(box.triangleIndexes[i]); /* positive numbers plus zero */
+    else if( tmpDist < 0.0f)
+      partition2.push_back(box.triangleIndexes[i]); /* negatice numbers */
+    else {
+      partition1.push_back(box.triangleIndexes[i]); /* 0.0f? unprobable... */
+      partition2.push_back(box.triangleIndexes[i]);
+    }
+  }
+  PRINTF(3)("\nPartition1: got \t%i Vertices \nPartition2: got \t%i Vertices\n", partition1.size(), partition2.size());
 
 
   /* now comes the separation into two different sVec3D arrays */
-  tIterator<const sVec3D>* iterator;                       //!< the iterator to go through the lists
-  const sVec3D*      element;                              //!< the elements
   int                index;                                //!< index storage place
-  sVec3D*            vertList1;                            //!< the vertex list 1
-  sVec3D*            vertList2;                            //!< the vertex list 2
-
-  vertList1 = new sVec3D[partition1.getSize()];
-  vertList2 = new sVec3D[partition2.getSize()];
-
-  iterator = partition1.getIterator();
-  element = iterator->firstElement();
-  index = 0;
-  while( element != NULL)
-  {
-    vertList1[index][0] = element[0][0];
-    vertList1[index][1] = element[0][1];
-    vertList1[index][2] = element[0][2];
-    ++index;
-    element = iterator->nextElement();
-  }
-
-//   PRINTF(0)("\npartition 1:\n");
-//   for(int i = 0; i < partition1.getSize(); ++i)
-//   {
-//     PRINTF(0)("v[%i][0] = %f,\tv[%i][1] = %f,\tv[%i][1] = %f\n", i, vertList1[i][0], i, vertList1[i][1], i, vertList1[i][2]);
-//   }
-
-  iterator = partition2.getIterator();
-  element = iterator->firstElement();
-  index = 0;
-  while( element != NULL)
-  {
-    vertList2[index][0] = element[0][0];
-    vertList2[index][1] = element[0][1];
-    vertList2[index][2] = element[0][2];
-    ++index;
-    element = iterator->nextElement();
-  }
-
-  this->tmpVert1 = vertList1;
-  this->tmpVert2 = vertList2;
-  this->tmpLen1 = partition1.getSize();
-  this->tmpLen2 = partition2.getSize();
-
-  delete iterator;
-
-//   PRINTF(0)("\npartition 2:\n");
-//   for(int i = 0; i < partition2.getSize(); ++i)
-//   {
-//     PRINTF(0)("v[%i][0] = %f,\tv[%i][1] = %f,\tv[%i][1] = %f\n", i, vertList2[i][0], i,  vertList2[i][1], i, vertList2[i][2]);
-//   }
+  int*               triangleIndexList1;                   //!< the vertex list 1
+  int*               triangleIndexList2;                   //!< the vertex list 2
+  std::list<int>::iterator element;                        //!< the list iterator
+
+  triangleIndexList1 = new int[partition1.size()];
+  triangleIndexList2 = new int[partition2.size()];
+
+  for( element = partition1.begin(), index = 0; element != partition1.end(); element++, index++)
+    triangleIndexList1[index] = (*element);
+
+  for( element = partition2.begin(), index = 0; element != partition2.end(); element++, index++)
+    triangleIndexList2[index] = (*element);
+
+  if( this->triangleIndexList1!= NULL)
+    delete[] this->triangleIndexList1;
+  this->triangleIndexList1 = triangleIndexList1;
+  this->triangleIndexLength1 = partition1.size();
+
+  if( this->triangleIndexList2 != NULL)
+    delete[] this->triangleIndexList2;
+  this->triangleIndexList2 = triangleIndexList2;
+  this->triangleIndexLength2 = partition2.size();
 }
 
@@ -699,21 +486,25 @@
 void OBBTreeNode::collideWith(BVTreeNode* treeNode, WorldEntity* nodeA, WorldEntity* nodeB)
 {
-  PRINTF(3)("collideWith\n");
+  if( unlikely(treeNode == NULL))
+    return;
+
+  PRINTF(4)("collideWith\n");
   /* if the obb overlap, make subtests: check which node is realy overlaping  */
-  PRINT(3)("Checking OBB %i vs %i: ", this->getIndex(), treeNode->getIndex());
-  if( unlikely(treeNode == NULL)) return;
-
-  if( this->overlapTest(this->bvElement, ((OBBTreeNode*)treeNode)->bvElement, nodeA, nodeB))
-  {
-    PRINTF(3)("collision @ lvl %i, object %s vs. %s, (%p, %p)\n", this->depth, nodeA->getClassName(), nodeB->getClassName(), this->nodeLeft, this->nodeRight);
+  PRINTF(4)("Checking OBB %i vs %i: ", this->getIndex(), treeNode->getIndex());
+  //   if( unlikely(treeNode == NULL)) return;
+
+
+  if( this->overlapTest(*this->bvElement, *(((const OBBTreeNode*)&treeNode)->bvElement), nodeA, nodeB))
+  {
+    PRINTF(4)("collision @ lvl %i, object %s vs. %s, (%p, %p)\n", this->depth, nodeA->getClassName(), nodeB->getClassName(), this->nodeLeft, this->nodeRight);
 
     /* check if left node overlaps */
     if( likely( this->nodeLeft != NULL))
     {
-      PRINT(3)("Checking OBB %i vs %i: ", this->nodeLeft->getIndex(), treeNode->getIndex());
-      if( this->overlapTest(this->nodeLeft->bvElement, ((OBBTreeNode*)treeNode)->bvElement, nodeA, nodeB))
+      PRINTF(4)("Checking OBB %i vs %i: ", this->nodeLeft->getIndex(), treeNode->getIndex());
+      if( this->overlapTest(*this->nodeLeft->bvElement, *(((const OBBTreeNode*)&treeNode)->bvElement), nodeA, nodeB))
       {
-        this->nodeLeft->collideWith(((OBBTreeNode*)treeNode)->nodeLeft, nodeA, nodeB);
-        this->nodeLeft->collideWith(((OBBTreeNode*)treeNode)->nodeRight, nodeA, nodeB);
+        this->nodeLeft->collideWith((((const OBBTreeNode*)treeNode)->nodeLeft), nodeA, nodeB);
+        this->nodeLeft->collideWith((((const OBBTreeNode*)treeNode)->nodeRight), nodeA, nodeB);
       }
     }
@@ -721,27 +512,33 @@
     if( likely( this->nodeRight != NULL))
     {
-      PRINT(3)("Checking OBB %i vs %i: ", this->nodeRight->getIndex(), treeNode->getIndex());
-      if(this->overlapTest(this->nodeRight->bvElement, ((OBBTreeNode*)treeNode)->bvElement, nodeA, nodeB))
+      PRINTF(4)("Checking OBB %i vs %i: ", this->nodeRight->getIndex(), treeNode->getIndex());
+      if(this->overlapTest(*this->nodeRight->bvElement, *(((const OBBTreeNode*)&treeNode)->bvElement), nodeA, nodeB))
       {
-       this->nodeRight->collideWith(((OBBTreeNode*)treeNode)->nodeLeft, nodeA, nodeB);
-       this->nodeRight->collideWith(((OBBTreeNode*)treeNode)->nodeRight, nodeA, nodeB);
+        this->nodeRight->collideWith((((const OBBTreeNode*)treeNode)->nodeLeft), nodeA, nodeB);
+        this->nodeRight->collideWith((((const OBBTreeNode*)treeNode)->nodeRight), nodeA, nodeB);
       }
     }
 
     /* so there is a collision and this is the last box in the tree (i.e. leaf) */
+    /* FIXME: If we would choose || insead of && there would also be asymmetrical cases supported */
     if( unlikely(this->nodeRight == NULL && this->nodeLeft == NULL))
     {
-      nodeA->collidesWith(nodeB, *((OBBTreeNode*)treeNode)->bvElement->center);
-
-      nodeB->collidesWith(nodeA, *this->bvElement->center);
-    }
-
-  }
-}
-
-
-
-bool OBBTreeNode::overlapTest(OBB* boxA, OBB* boxB, WorldEntity* nodeA, WorldEntity* nodeB)
-{
+      nodeA->collidesWith(nodeB, (((const OBBTreeNode*)&treeNode)->bvElement->center));
+
+      nodeB->collidesWith(nodeA, this->bvElement->center);
+    }
+
+  }
+}
+
+
+
+bool OBBTreeNode::overlapTest(OBB& boxA, OBB& boxB, WorldEntity* nodeA, WorldEntity* nodeB)
+{
+  //HACK remove this again
+  this->owner = nodeA;
+  //   if( boxB == NULL || boxA == NULL)
+  //     return false;
+
   /* first check all axis */
   Vector t;
@@ -752,22 +549,23 @@
   Vector rotAxisB[3];
 
-  rotAxisA[0] =  nodeA->getAbsDir().apply(boxA->axis[0]);
-  rotAxisA[1] =  nodeA->getAbsDir().apply(boxA->axis[1]);
-  rotAxisA[2] =  nodeA->getAbsDir().apply(boxA->axis[2]);
-
-  rotAxisB[0] =  nodeB->getAbsDir().apply(boxB->axis[0]);
-  rotAxisB[1] =  nodeB->getAbsDir().apply(boxB->axis[1]);
-  rotAxisB[2] =  nodeB->getAbsDir().apply(boxB->axis[2]);
-
-  t = nodeA->getAbsCoor() + nodeA->getAbsDir().apply(*boxA->center) - ( nodeB->getAbsCoor() + nodeB->getAbsDir().apply(*boxB->center));
-
-//   printf("\n");
-//   printf("(%f, %f, %f) -> (%f, %f, %f)\n", boxA->axis[0].x, boxA->axis[0].y, boxA->axis[0].z, rotAxisA[0].x, rotAxisA[0].y, rotAxisA[0].z);
-//   printf("(%f, %f, %f) -> (%f, %f, %f)\n", boxA->axis[1].x, boxA->axis[1].y, boxA->axis[1].z, rotAxisA[1].x, rotAxisA[1].y, rotAxisA[1].z);
-//   printf("(%f, %f, %f) -> (%f, %f, %f)\n", boxA->axis[2].x, boxA->axis[2].y, boxA->axis[2].z, rotAxisA[2].x, rotAxisA[2].y, rotAxisA[2].z);
-//
-//   printf("(%f, %f, %f) -> (%f, %f, %f)\n", boxB->axis[0].x, boxB->axis[0].y, boxB->axis[0].z, rotAxisB[0].x, rotAxisB[0].y, rotAxisB[0].z);
-//   printf("(%f, %f, %f) -> (%f, %f, %f)\n", boxB->axis[1].x, boxB->axis[1].y, boxB->axis[1].z, rotAxisB[1].x, rotAxisB[1].y, rotAxisB[1].z);
-//   printf("(%f, %f, %f) -> (%f, %f, %f)\n", boxB->axis[2].x, boxB->axis[2].y, boxB->axis[2].z, rotAxisB[2].x, rotAxisB[2].y, rotAxisB[2].z);
+  rotAxisA[0] =  nodeA->getAbsDir().apply(boxA.axis[0]);
+  rotAxisA[1] =  nodeA->getAbsDir().apply(boxA.axis[1]);
+  rotAxisA[2] =  nodeA->getAbsDir().apply(boxA.axis[2]);
+
+  rotAxisB[0] =  nodeB->getAbsDir().apply(boxB.axis[0]);
+  rotAxisB[1] =  nodeB->getAbsDir().apply(boxB.axis[1]);
+  rotAxisB[2] =  nodeB->getAbsDir().apply(boxB.axis[2]);
+
+
+  t = nodeA->getAbsCoor() + nodeA->getAbsDir().apply(boxA.center) - ( nodeB->getAbsCoor() + nodeB->getAbsDir().apply(boxB.center));
+
+  //   printf("\n");
+  //   printf("(%f, %f, %f) -> (%f, %f, %f)\n", boxA->axis[0].x, boxA->axis[0].y, boxA->axis[0].z, rotAxisA[0].x, rotAxisA[0].y, rotAxisA[0].z);
+  //   printf("(%f, %f, %f) -> (%f, %f, %f)\n", boxA->axis[1].x, boxA->axis[1].y, boxA->axis[1].z, rotAxisA[1].x, rotAxisA[1].y, rotAxisA[1].z);
+  //   printf("(%f, %f, %f) -> (%f, %f, %f)\n", boxA->axis[2].x, boxA->axis[2].y, boxA->axis[2].z, rotAxisA[2].x, rotAxisA[2].y, rotAxisA[2].z);
+  //
+  //   printf("(%f, %f, %f) -> (%f, %f, %f)\n", boxB->axis[0].x, boxB->axis[0].y, boxB->axis[0].z, rotAxisB[0].x, rotAxisB[0].y, rotAxisB[0].z);
+  //   printf("(%f, %f, %f) -> (%f, %f, %f)\n", boxB->axis[1].x, boxB->axis[1].y, boxB->axis[1].z, rotAxisB[1].x, rotAxisB[1].y, rotAxisB[1].z);
+  //   printf("(%f, %f, %f) -> (%f, %f, %f)\n", boxB->axis[2].x, boxB->axis[2].y, boxB->axis[2].z, rotAxisB[2].x, rotAxisB[2].y, rotAxisB[2].z);
 
 
@@ -779,17 +577,17 @@
     l = rotAxisA[j];
 
-    rA += fabs(boxA->halfLength[0] * rotAxisA[0].dot(l));
-    rA += fabs(boxA->halfLength[1] * rotAxisA[1].dot(l));
-    rA += fabs(boxA->halfLength[2] * rotAxisA[2].dot(l));
-
-    rB += fabs(boxB->halfLength[0] * rotAxisB[0].dot(l));
-    rB += fabs(boxB->halfLength[1] * rotAxisB[1].dot(l));
-    rB += fabs(boxB->halfLength[2] * rotAxisB[2].dot(l));
-
-    PRINTF(3)("s = %f, rA+rB = %f\n", fabs(t.dot(l)), rA+rB);
+    rA += fabs(boxA.halfLength[0] * rotAxisA[0].dot(l));
+    rA += fabs(boxA.halfLength[1] * rotAxisA[1].dot(l));
+    rA += fabs(boxA.halfLength[2] * rotAxisA[2].dot(l));
+
+    rB += fabs(boxB.halfLength[0] * rotAxisB[0].dot(l));
+    rB += fabs(boxB.halfLength[1] * rotAxisB[1].dot(l));
+    rB += fabs(boxB.halfLength[2] * rotAxisB[2].dot(l));
+
+    PRINTF(5)("s = %f, rA+rB = %f\n", fabs(t.dot(l)), rA+rB);
 
     if( (rA + rB) < fabs(t.dot(l)))
     {
-      PRINT(3)("no Collision\n");
+      PRINTF(4)("no Collision\n");
       return false;
     }
@@ -803,17 +601,17 @@
     l = rotAxisB[j];
 
-    rA += fabs(boxA->halfLength[0] * rotAxisA[0].dot(l));
-    rA += fabs(boxA->halfLength[1] * rotAxisA[1].dot(l));
-    rA += fabs(boxA->halfLength[2] * rotAxisA[2].dot(l));
-
-    rB += fabs(boxB->halfLength[0] * rotAxisB[0].dot(l));
-    rB += fabs(boxB->halfLength[1] * rotAxisB[1].dot(l));
-    rB += fabs(boxB->halfLength[2] * rotAxisB[2].dot(l));
-
-    PRINTF(3)("s = %f, rA+rB = %f\n", fabs(t.dot(l)), rA+rB);
+    rA += fabs(boxA.halfLength[0] * rotAxisA[0].dot(l));
+    rA += fabs(boxA.halfLength[1] * rotAxisA[1].dot(l));
+    rA += fabs(boxA.halfLength[2] * rotAxisA[2].dot(l));
+
+    rB += fabs(boxB.halfLength[0] * rotAxisB[0].dot(l));
+    rB += fabs(boxB.halfLength[1] * rotAxisB[1].dot(l));
+    rB += fabs(boxB.halfLength[2] * rotAxisB[2].dot(l));
+
+    PRINTF(5)("s = %f, rA+rB = %f\n", fabs(t.dot(l)), rA+rB);
 
     if( (rA + rB) < fabs(t.dot(l)))
     {
-      PRINT(3)("no Collision\n");
+      PRINTF(4)("no Collision\n");
       return false;
     }
@@ -831,17 +629,17 @@
       l = rotAxisA[j].cross(rotAxisB[k]);
 
-      rA += fabs(boxA->halfLength[0] * rotAxisA[0].dot(l));
-      rA += fabs(boxA->halfLength[1] * rotAxisA[1].dot(l));
-      rA += fabs(boxA->halfLength[2] * rotAxisA[2].dot(l));
-
-      rB += fabs(boxB->halfLength[0] * rotAxisB[0].dot(l));
-      rB += fabs(boxB->halfLength[1] * rotAxisB[1].dot(l));
-      rB += fabs(boxB->halfLength[2] * rotAxisB[2].dot(l));
-
-      PRINTF(3)("s = %f, rA+rB = %f\n", fabs(t.dot(l)), rA+rB);
+      rA += fabs(boxA.halfLength[0] * rotAxisA[0].dot(l));
+      rA += fabs(boxA.halfLength[1] * rotAxisA[1].dot(l));
+      rA += fabs(boxA.halfLength[2] * rotAxisA[2].dot(l));
+
+      rB += fabs(boxB.halfLength[0] * rotAxisB[0].dot(l));
+      rB += fabs(boxB.halfLength[1] * rotAxisB[1].dot(l));
+      rB += fabs(boxB.halfLength[2] * rotAxisB[2].dot(l));
+
+      PRINTF(5)("s = %f, rA+rB = %f\n", fabs(t.dot(l)), rA+rB);
 
       if( (rA + rB) < fabs(t.dot(l)))
       {
-        PRINT(3)("keine Kollision\n");
+        PRINTF(4)("keine Kollision\n");
         return false;
       }
@@ -849,8 +647,10 @@
   }
 
-
-  boxA->bCollided = true; /* use this ONLY(!!!!) for drawing operations */
-  boxB->bCollided = true;
-  PRINT(3)("Kollision!\n");
+  /* FIXME: there is no collision mark set now */
+     boxA.bCollided = true; /* use this ONLY(!!!!) for drawing operations */
+     boxB.bCollided = true;
+
+
+  PRINTF(4)("Kollision!\n");
   return true;
 }
@@ -860,28 +660,30 @@
 
 
+
+
+
+
+
+/**
+ *
+ * draw the BV tree - debug mode
+ */
 void OBBTreeNode::drawBV(int depth, int drawMode, const Vector& color,  bool top) const
 {
-
-  /* draw the model itself, there is some problem concerning this: the vertices are drawn multiple times */
-  if( drawMode & DRAW_MODEL || drawMode & DRAW_ALL)
-  {
-    if( !(drawMode & DRAW_SINGLE && depth != 0))
-    {
-      if( drawMode & DRAW_POINTS)
+  /* this function can be used to draw the triangles and/or the points only  */
+  if( 1 /*drawMode & DRAW_MODEL || drawMode & DRAW_ALL*/)
+  {
+    if( depth == 0/*!(drawMode & DRAW_SINGLE && depth != 0)*/)
+    {
+      if( 1 /*drawMode & DRAW_POINTS*/)
+      {
         glBegin(GL_POINTS);
-      for(int i = 0; i < this->bvElement->numOfVertices; ++i)
-      {
-        if( drawMode & DRAW_POINTS)
-          glVertex3f(this->bvElement->vertices[i][0], this->bvElement->vertices[i][1], this->bvElement->vertices[i][2]);
-        else
-        {
-          glPushMatrix();
-          glTranslatef(this->bvElement->vertices[i][0], this->bvElement->vertices[i][1], this->bvElement->vertices[i][2]);
-          gluSphere(OBBTreeNode_sphereObj, 0.1, 10, 10);
-          glPopMatrix();
-        }
+        glColor3f(0.3, 0.8, 0.54);
+        for( int i = 0; i < this->bvElement->modelInf->numVertices*3; i+=3)
+          glVertex3f(this->bvElement->modelInf->pVertices[i],
+                     this->bvElement->modelInf->pVertices[i+1],
+                     this->bvElement->modelInf->pVertices[i+2]);
+        glEnd();
       }
-      if( drawMode & DRAW_POINTS)
-        glEnd();
     }
   }
@@ -897,5 +699,5 @@
 
   /* draw world axes */
-  if( drawMode & DRAW_BV_AXIS)
+  if( 1 /*drawMode & DRAW_BV_AXIS*/)
   {
     glBegin(GL_LINES);
@@ -915,25 +717,27 @@
 
 
-  if( drawMode & DRAW_BV_AXIS || drawMode & DRAW_ALL)
-  {
-    if( !(drawMode & DRAW_SINGLE && depth != 0))
+  if( 1/*drawMode & DRAW_BV_AXIS || drawMode & DRAW_ALL*/)
+  {
+    if( 1/*drawMode & DRAW_SINGLE && depth != 0*/)
     {
       /* draw the obb axes */
       glBegin(GL_LINES);
-      glColor3f(0.0, 0.4, 0.3);
-      glVertex3f(this->bvElement->center->x, this->bvElement->center->y, this->bvElement->center->z);
-      glVertex3f(this->bvElement->center->x + this->bvElement->axis[0].x * this->bvElement->halfLength[0],
-                 this->bvElement->center->y + this->bvElement->axis[0].y * this->bvElement->halfLength[0],
-                 this->bvElement->center->z + this->bvElement->axis[0].z * this->bvElement->halfLength[0]);
-
-      glVertex3f(this->bvElement->center->x, this->bvElement->center->y, this->bvElement->center->z);
-      glVertex3f(this->bvElement->center->x + this->bvElement->axis[1].x * this->bvElement->halfLength[1],
-                 this->bvElement->center->y + this->bvElement->axis[1].y * this->bvElement->halfLength[1],
-                 this->bvElement->center->z + this->bvElement->axis[1].z * this->bvElement->halfLength[1]);
-
-      glVertex3f(this->bvElement->center->x, this->bvElement->center->y, this->bvElement->center->z);
-      glVertex3f(this->bvElement->center->x + this->bvElement->axis[2].x * this->bvElement->halfLength[2],
-                 this->bvElement->center->y + this->bvElement->axis[2].y * this->bvElement->halfLength[2],
-                 this->bvElement->center->z + this->bvElement->axis[2].z * this->bvElement->halfLength[2]);
+      glColor3f(1.0, 0.0, 0.0);
+      glVertex3f(this->bvElement->center.x, this->bvElement->center.y, this->bvElement->center.z);
+      glVertex3f(this->bvElement->center.x + this->bvElement->axis[0].x * this->bvElement->halfLength[0],
+                 this->bvElement->center.y + this->bvElement->axis[0].y * this->bvElement->halfLength[0],
+                 this->bvElement->center.z + this->bvElement->axis[0].z * this->bvElement->halfLength[0]);
+
+      glColor3f(0.0, 1.0, 0.0);
+      glVertex3f(this->bvElement->center.x, this->bvElement->center.y, this->bvElement->center.z);
+      glVertex3f(this->bvElement->center.x + this->bvElement->axis[1].x * this->bvElement->halfLength[1],
+                 this->bvElement->center.y + this->bvElement->axis[1].y * this->bvElement->halfLength[1],
+                 this->bvElement->center.z + this->bvElement->axis[1].z * this->bvElement->halfLength[1]);
+
+      glColor3f(0.0, 0.0, 1.0);
+      glVertex3f(this->bvElement->center.x, this->bvElement->center.y, this->bvElement->center.z);
+      glVertex3f(this->bvElement->center.x + this->bvElement->axis[2].x * this->bvElement->halfLength[2],
+                 this->bvElement->center.y + this->bvElement->axis[2].y * this->bvElement->halfLength[2],
+                 this->bvElement->center.z + this->bvElement->axis[2].z * this->bvElement->halfLength[2]);
       glEnd();
     }
@@ -950,101 +754,41 @@
     }
 
-    if(this->nodeLeft == NULL || this->nodeRight == NULL)
+    if( this->nodeLeft == NULL && this->nodeRight == NULL)
       depth = 0;
-    if( !(drawMode & DRAW_SINGLE && depth != 0))
-    {
-    Vector cen = *this->bvElement->center;
-    Vector* axis = this->bvElement->axis;
-    float* len = this->bvElement->halfLength;
-
-    if( this->bvElement->bCollided)
-    {
-      glColor4f(1.0, 1.0, 1.0, .5); // COLLISION COLOR
-    }
-    else if( drawMode & DRAW_BV_BLENDED)
-    {
-      glColor4f(color.x, color.y, color.z, .5);
-    }
-
-    /* draw bounding box */
-    if( drawMode & DRAW_BV_BLENDED)
-      glBegin(GL_QUADS);
-    else
-      glBegin(GL_LINE_LOOP);
-    glVertex3f(cen.x + axis[0].x * len[0] + axis[1].x * len[1] + axis[2].x * len[2],
-               cen.y + axis[0].y * len[0] + axis[1].y * len[1] + axis[2].y * len[2],
-               cen.z + axis[0].z * len[0] + axis[1].z * len[1] + axis[2].z * len[2]);
-    glVertex3f(cen.x + axis[0].x * len[0] + axis[1].x * len[1] - axis[2].x * len[2],
-               cen.y + axis[0].y * len[0] + axis[1].y * len[1] - axis[2].y * len[2],
-               cen.z + axis[0].z * len[0] + axis[1].z * len[1] - axis[2].z * len[2]);
-    glVertex3f(cen.x + axis[0].x * len[0] - axis[1].x * len[1] - axis[2].x * len[2],
-               cen.y + axis[0].y * len[0] - axis[1].y * len[1] - axis[2].y * len[2],
-               cen.z + axis[0].z * len[0] - axis[1].z * len[1] - axis[2].z * len[2]);
-    glVertex3f(cen.x + axis[0].x * len[0] - axis[1].x * len[1] + axis[2].x * len[2],
-               cen.y + axis[0].y * len[0] - axis[1].y * len[1] + axis[2].y * len[2],
-               cen.z + axis[0].z * len[0] - axis[1].z * len[1] + axis[2].z * len[2]);
-    glEnd();
-
-    if( drawMode & DRAW_BV_BLENDED)
-      glBegin(GL_QUADS);
-    else
-      glBegin(GL_LINE_LOOP);
-    glVertex3f(cen.x + axis[0].x * len[0] - axis[1].x * len[1] + axis[2].x * len[2],
-               cen.y + axis[0].y * len[0] - axis[1].y * len[1] + axis[2].y * len[2],
-               cen.z + axis[0].z * len[0] - axis[1].z * len[1] + axis[2].z * len[2]);
-    glVertex3f(cen.x + axis[0].x * len[0] - axis[1].x * len[1] - axis[2].x * len[2],
-               cen.y + axis[0].y * len[0] - axis[1].y * len[1] - axis[2].y * len[2],
-               cen.z + axis[0].z * len[0] - axis[1].z * len[1] - axis[2].z * len[2]);
-    glVertex3f(cen.x - axis[0].x * len[0] - axis[1].x * len[1] - axis[2].x * len[2],
-               cen.y - axis[0].y * len[0] - axis[1].y * len[1] - axis[2].y * len[2],
-               cen.z - axis[0].z * len[0] - axis[1].z * len[1] - axis[2].z * len[2]);
-    glVertex3f(cen.x - axis[0].x * len[0] - axis[1].x * len[1] + axis[2].x * len[2],
-               cen.y - axis[0].y * len[0] - axis[1].y * len[1] + axis[2].y * len[2],
-               cen.z - axis[0].z * len[0] - axis[1].z * len[1] + axis[2].z * len[2]);
-    glEnd();
-
-    if( drawMode & DRAW_BV_BLENDED)
-      glBegin(GL_QUADS);
-    else
-      glBegin(GL_LINE_LOOP);
-    glVertex3f(cen.x - axis[0].x * len[0] - axis[1].x * len[1] + axis[2].x * len[2],
-               cen.y - axis[0].y * len[0] - axis[1].y * len[1] + axis[2].y * len[2],
-               cen.z - axis[0].z * len[0] - axis[1].z * len[1] + axis[2].z * len[2]);
-    glVertex3f(cen.x - axis[0].x * len[0] - axis[1].x * len[1] - axis[2].x * len[2],
-               cen.y - axis[0].y * len[0] - axis[1].y * len[1] - axis[2].y * len[2],
-               cen.z - axis[0].z * len[0] - axis[1].z * len[1] - axis[2].z * len[2]);
-    glVertex3f(cen.x - axis[0].x * len[0] + axis[1].x * len[1] - axis[2].x * len[2],
-               cen.y - axis[0].y * len[0] + axis[1].y * len[1] - axis[2].y * len[2],
-               cen.z - axis[0].z * len[0] + axis[1].z * len[1] - axis[2].z * len[2]);
-    glVertex3f(cen.x - axis[0].x * len[0] + axis[1].x * len[1] + axis[2].x * len[2],
-               cen.y - axis[0].y * len[0] + axis[1].y * len[1] + axis[2].y * len[2],
-               cen.z - axis[0].z * len[0] + axis[1].z * len[1] + axis[2].z * len[2]);
-    glEnd();
-
-    if( drawMode & DRAW_BV_BLENDED)
-      glBegin(GL_QUADS);
-    else
-      glBegin(GL_LINE_LOOP);
-    glVertex3f(cen.x - axis[0].x * len[0] + axis[1].x * len[1] - axis[2].x * len[2],
-               cen.y - axis[0].y * len[0] + axis[1].y * len[1] - axis[2].y * len[2],
-               cen.z - axis[0].z * len[0] + axis[1].z * len[1] - axis[2].z * len[2]);
-    glVertex3f(cen.x - axis[0].x * len[0] + axis[1].x * len[1] + axis[2].x * len[2],
-               cen.y - axis[0].y * len[0] + axis[1].y * len[1] + axis[2].y * len[2],
-               cen.z - axis[0].z * len[0] + axis[1].z * len[1] + axis[2].z * len[2]);
-    glVertex3f(cen.x + axis[0].x * len[0] + axis[1].x * len[1] + axis[2].x * len[2],
-               cen.y + axis[0].y * len[0] + axis[1].y * len[1] + axis[2].y * len[2],
-               cen.z + axis[0].z * len[0] + axis[1].z * len[1] + axis[2].z * len[2]);
-    glVertex3f(cen.x + axis[0].x * len[0] + axis[1].x * len[1] - axis[2].x * len[2],
-               cen.y + axis[0].y * len[0] + axis[1].y * len[1] - axis[2].y * len[2],
-               cen.z + axis[0].z * len[0] + axis[1].z * len[1] - axis[2].z * len[2]);
-    glEnd();
-
-
-    if( drawMode & DRAW_BV_BLENDED)
-    {
-      glBegin(GL_QUADS);
-      glVertex3f(cen.x - axis[0].x * len[0] + axis[1].x * len[1] - axis[2].x * len[2],
-                 cen.y - axis[0].y * len[0] + axis[1].y * len[1] - axis[2].y * len[2],
-                 cen.z - axis[0].z * len[0] + axis[1].z * len[1] - axis[2].z * len[2]);
+
+    if( depth == 0 /*!(drawMode & DRAW_SINGLE && depth != 0)*/)
+    {
+
+
+      Vector cen = this->bvElement->center;
+      Vector* axis = this->bvElement->axis;
+      float* len = this->bvElement->halfLength;
+
+      if( this->bvElement->bCollided)
+      {
+        glColor4f(1.0, 1.0, 1.0, .5); // COLLISION COLOR
+      }
+      else if( drawMode & DRAW_BV_BLENDED)
+      {
+        glColor4f(color.x, color.y, color.z, .5);
+      }
+
+      // debug out
+      if( this->obbTree->getOwner() != NULL)
+      {
+        PRINTF(4)("debug poly draw: depth: %i, mode: %i, entity-name: %s, class: %s\n", depth, drawMode, this->obbTree->getOwner()->getName(), this->obbTree->getOwner()->getClassName());
+      }
+      else
+        PRINTF(4)("debug poly draw: depth: %i, mode: %i\n", depth, drawMode);
+
+
+      /* draw bounding box */
+      if( drawMode & DRAW_BV_BLENDED)
+        glBegin(GL_QUADS);
+      else
+        glBegin(GL_LINE_LOOP);
+      glVertex3f(cen.x + axis[0].x * len[0] + axis[1].x * len[1] + axis[2].x * len[2],
+                 cen.y + axis[0].y * len[0] + axis[1].y * len[1] + axis[2].y * len[2],
+                 cen.z + axis[0].z * len[0] + axis[1].z * len[1] + axis[2].z * len[2]);
       glVertex3f(cen.x + axis[0].x * len[0] + axis[1].x * len[1] - axis[2].x * len[2],
                  cen.y + axis[0].y * len[0] + axis[1].y * len[1] - axis[2].y * len[2],
@@ -1053,10 +797,52 @@
                  cen.y + axis[0].y * len[0] - axis[1].y * len[1] - axis[2].y * len[2],
                  cen.z + axis[0].z * len[0] - axis[1].z * len[1] - axis[2].z * len[2]);
+      glVertex3f(cen.x + axis[0].x * len[0] - axis[1].x * len[1] + axis[2].x * len[2],
+                 cen.y + axis[0].y * len[0] - axis[1].y * len[1] + axis[2].y * len[2],
+                 cen.z + axis[0].z * len[0] - axis[1].z * len[1] + axis[2].z * len[2]);
+      glEnd();
+
+      if( drawMode & DRAW_BV_BLENDED)
+        glBegin(GL_QUADS);
+      else
+        glBegin(GL_LINE_LOOP);
+      glVertex3f(cen.x + axis[0].x * len[0] - axis[1].x * len[1] + axis[2].x * len[2],
+                 cen.y + axis[0].y * len[0] - axis[1].y * len[1] + axis[2].y * len[2],
+                 cen.z + axis[0].z * len[0] - axis[1].z * len[1] + axis[2].z * len[2]);
+      glVertex3f(cen.x + axis[0].x * len[0] - axis[1].x * len[1] - axis[2].x * len[2],
+                 cen.y + axis[0].y * len[0] - axis[1].y * len[1] - axis[2].y * len[2],
+                 cen.z + axis[0].z * len[0] - axis[1].z * len[1] - axis[2].z * len[2]);
       glVertex3f(cen.x - axis[0].x * len[0] - axis[1].x * len[1] - axis[2].x * len[2],
                  cen.y - axis[0].y * len[0] - axis[1].y * len[1] - axis[2].y * len[2],
                  cen.z - axis[0].z * len[0] - axis[1].z * len[1] - axis[2].z * len[2]);
+      glVertex3f(cen.x - axis[0].x * len[0] - axis[1].x * len[1] + axis[2].x * len[2],
+                 cen.y - axis[0].y * len[0] - axis[1].y * len[1] + axis[2].y * len[2],
+                 cen.z - axis[0].z * len[0] - axis[1].z * len[1] + axis[2].z * len[2]);
       glEnd();
 
-      glBegin(GL_QUADS);
+      if( drawMode & DRAW_BV_BLENDED)
+        glBegin(GL_QUADS);
+      else
+        glBegin(GL_LINE_LOOP);
+      glVertex3f(cen.x - axis[0].x * len[0] - axis[1].x * len[1] + axis[2].x * len[2],
+                 cen.y - axis[0].y * len[0] - axis[1].y * len[1] + axis[2].y * len[2],
+                 cen.z - axis[0].z * len[0] - axis[1].z * len[1] + axis[2].z * len[2]);
+      glVertex3f(cen.x - axis[0].x * len[0] - axis[1].x * len[1] - axis[2].x * len[2],
+                 cen.y - axis[0].y * len[0] - axis[1].y * len[1] - axis[2].y * len[2],
+                 cen.z - axis[0].z * len[0] - axis[1].z * len[1] - axis[2].z * len[2]);
+      glVertex3f(cen.x - axis[0].x * len[0] + axis[1].x * len[1] - axis[2].x * len[2],
+                 cen.y - axis[0].y * len[0] + axis[1].y * len[1] - axis[2].y * len[2],
+                 cen.z - axis[0].z * len[0] + axis[1].z * len[1] - axis[2].z * len[2]);
+      glVertex3f(cen.x - axis[0].x * len[0] + axis[1].x * len[1] + axis[2].x * len[2],
+                 cen.y - axis[0].y * len[0] + axis[1].y * len[1] + axis[2].y * len[2],
+                 cen.z - axis[0].z * len[0] + axis[1].z * len[1] + axis[2].z * len[2]);
+      glEnd();
+
+      if( drawMode & DRAW_BV_BLENDED)
+        glBegin(GL_QUADS);
+      else
+        glBegin(GL_LINE_LOOP);
+      glVertex3f(cen.x - axis[0].x * len[0] + axis[1].x * len[1] - axis[2].x * len[2],
+                 cen.y - axis[0].y * len[0] + axis[1].y * len[1] - axis[2].y * len[2],
+                 cen.z - axis[0].z * len[0] + axis[1].z * len[1] - axis[2].z * len[2]);
       glVertex3f(cen.x - axis[0].x * len[0] + axis[1].x * len[1] + axis[2].x * len[2],
                  cen.y - axis[0].y * len[0] + axis[1].y * len[1] + axis[2].y * len[2],
@@ -1065,18 +851,46 @@
                  cen.y + axis[0].y * len[0] + axis[1].y * len[1] + axis[2].y * len[2],
                  cen.z + axis[0].z * len[0] + axis[1].z * len[1] + axis[2].z * len[2]);
-      glVertex3f(cen.x + axis[0].x * len[0] - axis[1].x * len[1] + axis[2].x * len[2],
-                 cen.y + axis[0].y * len[0] - axis[1].y * len[1] + axis[2].y * len[2],
-                 cen.z + axis[0].z * len[0] - axis[1].z * len[1] + axis[2].z * len[2]);
-      glVertex3f(cen.x - axis[0].x * len[0] - axis[1].x * len[1] + axis[2].x * len[2],
-                 cen.y - axis[0].y * len[0] - axis[1].y * len[1] + axis[2].y * len[2],
-                 cen.z - axis[0].z * len[0] - axis[1].z * len[1] + axis[2].z * len[2]);
+      glVertex3f(cen.x + axis[0].x * len[0] + axis[1].x * len[1] - axis[2].x * len[2],
+                 cen.y + axis[0].y * len[0] + axis[1].y * len[1] - axis[2].y * len[2],
+                 cen.z + axis[0].z * len[0] + axis[1].z * len[1] - axis[2].z * len[2]);
       glEnd();
-    }
-
-
-    if( drawMode & DRAW_BV_BLENDED)
-      glColor3f(color.x, color.y, color.z);
-    }
-
+
+
+      if( drawMode & DRAW_BV_BLENDED)
+      {
+        glBegin(GL_QUADS);
+        glVertex3f(cen.x - axis[0].x * len[0] + axis[1].x * len[1] - axis[2].x * len[2],
+                   cen.y - axis[0].y * len[0] + axis[1].y * len[1] - axis[2].y * len[2],
+                   cen.z - axis[0].z * len[0] + axis[1].z * len[1] - axis[2].z * len[2]);
+        glVertex3f(cen.x + axis[0].x * len[0] + axis[1].x * len[1] - axis[2].x * len[2],
+                   cen.y + axis[0].y * len[0] + axis[1].y * len[1] - axis[2].y * len[2],
+                   cen.z + axis[0].z * len[0] + axis[1].z * len[1] - axis[2].z * len[2]);
+        glVertex3f(cen.x + axis[0].x * len[0] - axis[1].x * len[1] - axis[2].x * len[2],
+                   cen.y + axis[0].y * len[0] - axis[1].y * len[1] - axis[2].y * len[2],
+                   cen.z + axis[0].z * len[0] - axis[1].z * len[1] - axis[2].z * len[2]);
+        glVertex3f(cen.x - axis[0].x * len[0] - axis[1].x * len[1] - axis[2].x * len[2],
+                   cen.y - axis[0].y * len[0] - axis[1].y * len[1] - axis[2].y * len[2],
+                   cen.z - axis[0].z * len[0] - axis[1].z * len[1] - axis[2].z * len[2]);
+        glEnd();
+
+        glBegin(GL_QUADS);
+        glVertex3f(cen.x - axis[0].x * len[0] + axis[1].x * len[1] + axis[2].x * len[2],
+                   cen.y - axis[0].y * len[0] + axis[1].y * len[1] + axis[2].y * len[2],
+                   cen.z - axis[0].z * len[0] + axis[1].z * len[1] + axis[2].z * len[2]);
+        glVertex3f(cen.x + axis[0].x * len[0] + axis[1].x * len[1] + axis[2].x * len[2],
+                   cen.y + axis[0].y * len[0] + axis[1].y * len[1] + axis[2].y * len[2],
+                   cen.z + axis[0].z * len[0] + axis[1].z * len[1] + axis[2].z * len[2]);
+        glVertex3f(cen.x + axis[0].x * len[0] - axis[1].x * len[1] + axis[2].x * len[2],
+                   cen.y + axis[0].y * len[0] - axis[1].y * len[1] + axis[2].y * len[2],
+                   cen.z + axis[0].z * len[0] - axis[1].z * len[1] + axis[2].z * len[2]);
+        glVertex3f(cen.x - axis[0].x * len[0] - axis[1].x * len[1] + axis[2].x * len[2],
+                   cen.y - axis[0].y * len[0] - axis[1].y * len[1] + axis[2].y * len[2],
+                   cen.z - axis[0].z * len[0] - axis[1].z * len[1] + axis[2].z * len[2]);
+        glEnd();
+      }
+
+      if( drawMode & DRAW_BV_BLENDED)
+        glColor3f(color.x, color.y, color.z);
+    }
   }
 
@@ -1089,19 +903,19 @@
         glColor4f(color.x, color.y, color.z, .6);
 
-    /* now draw the separation plane */
-    Vector a1 = this->bvElement->axis[(this->longestAxisIndex + 1)%3];
-    Vector a2 = this->bvElement->axis[(this->longestAxisIndex + 2)%3];
-    Vector c = *this->bvElement->center;
-    float l1 = this->bvElement->halfLength[(this->longestAxisIndex + 1)%3];
-    float l2 = this->bvElement->halfLength[(this->longestAxisIndex + 2)%3];
-    glBegin(GL_QUADS);
-    glVertex3f(c.x + a1.x * l1 + a2.x * l2, c.y + a1.y * l1+ a2.y * l2, c.z + a1.z * l1 + a2.z * l2);
-    glVertex3f(c.x - a1.x * l1 + a2.x * l2, c.y - a1.y * l1+ a2.y * l2, c.z - a1.z * l1 + a2.z * l2);
-    glVertex3f(c.x - a1.x * l1 - a2.x * l2, c.y - a1.y * l1- a2.y * l2, c.z - a1.z * l1 - a2.z * l2);
-    glVertex3f(c.x + a1.x * l1 - a2.x * l2, c.y + a1.y * l1- a2.y * l2, c.z + a1.z * l1 - a2.z * l2);
-    glEnd();
-
-    if( drawMode & DRAW_BV_BLENDED)
-      glColor4f(color.x, color.y, color.z, 1.0);
+      /* now draw the separation plane */
+      Vector a1 = this->bvElement->axis[(this->longestAxisIndex + 1)%3];
+      Vector a2 = this->bvElement->axis[(this->longestAxisIndex + 2)%3];
+      Vector c = this->bvElement->center;
+      float l1 = this->bvElement->halfLength[(this->longestAxisIndex + 1)%3];
+      float l2 = this->bvElement->halfLength[(this->longestAxisIndex + 2)%3];
+      glBegin(GL_QUADS);
+      glVertex3f(c.x + a1.x * l1 + a2.x * l2, c.y + a1.y * l1+ a2.y * l2, c.z + a1.z * l1 + a2.z * l2);
+      glVertex3f(c.x - a1.x * l1 + a2.x * l2, c.y - a1.y * l1+ a2.y * l2, c.z - a1.z * l1 + a2.z * l2);
+      glVertex3f(c.x - a1.x * l1 - a2.x * l2, c.y - a1.y * l1- a2.y * l2, c.z - a1.z * l1 - a2.z * l2);
+      glVertex3f(c.x + a1.x * l1 - a2.x * l2, c.y + a1.y * l1- a2.y * l2, c.z + a1.z * l1 - a2.z * l2);
+      glEnd();
+
+      if( drawMode & DRAW_BV_BLENDED)
+        glColor4f(color.x, color.y, color.z, 1.0);
 
     }
@@ -1127,10 +941,7 @@
 void OBBTreeNode::debug() const
 {
-
-  /*
-  for(int i = 0; i < length; i++)
-  {
-  PRINTF(3)("vertex %i: %f, %f, %f\n", i, verticesList[i][0], verticesList[i][1], verticesList[i][2]);
-}
-  */
-}
+  PRINT(0)("========OBBTreeNode::debug()=====\n");
+  PRINT(0)(" Current depth: %i", this->depth);
+  PRINT(0)(" ");
+  PRINT(0)("=================================\n");
+}
Index: trunk/src/lib/collision_detection/obb_tree_node.h
===================================================================
--- trunk/src/lib/collision_detection/obb_tree_node.h	(revision 7677)
+++ trunk/src/lib/collision_detection/obb_tree_node.h	(revision 7711)
@@ -1,6 +1,5 @@
 /*!
  * @file bv_tree.h
-  *  Definition of a bounding volume tree
-
+ *  Definition of a bounding volume tree
  */
 
@@ -8,9 +7,8 @@
 #define _OBB_TREE_NODE_H
 
+
 #include "bv_tree_node.h"
+#include "plane.h"
 
-
-
-// FORWARD DECLARATION
 class BoundingVolume;
 class OBB;
@@ -18,66 +16,66 @@
 class Plane;
 class PNode;
-//struct sVec3D;
+
 
 //! A class that represents a bounding volume tree
-class OBBTreeNode : public BVTreeNode {
+class OBBTreeNode : public BVTreeNode
+{
 
 
   public:
-    OBBTreeNode();
+    OBBTreeNode(const OBBTree& tree, OBBTreeNode* prev, int depth);
     virtual ~OBBTreeNode();
 
-    virtual void spawnBVTree(const int depth, const sVec3D *verticesList, unsigned int length);
-    virtual void spawnBVTree(const int depth, const modelInfo& modInfo);
+    /**  this function returns the bounding volume of this tree node @return: returns the BV */
+    virtual inline const BoundingVolume* getBV() const { return (BoundingVolume*)this->bvElement; }
 
-    BoundingVolume* getBV(int index) const { return (BoundingVolume*)this->bvElement; }
-    inline const int getIndex() { return this->treeIndex; }
-    inline void setTreeRef(OBBTree* tree) { this->obbTree = tree;}
+    virtual void spawnBVTree(const modelInfo& modelInf, const int* triangleIndexes, int length);
 
     virtual void collideWith(BVTreeNode* treeNode, WorldEntity* nodeA, WorldEntity* nodeB);
-
     virtual void drawBV(int depth, int drawMode, const Vector& color = Vector(1,0,0), bool top = true) const;
-
     void debug() const;
 
-  private:
-    void calculateBoxCovariance(OBB* box, const sVec3D* verticesList, unsigned int length);
-    void calculateBoxEigenvectors(OBB* box, const sVec3D* verticesList, unsigned int length);
-    void calculateBoxAxis(OBB* box, const sVec3D* verticesList, unsigned int length);
-
-    void calculateBoxCovariance(OBB* box, const modelInfo& modInfo);
-    void calculateBoxEigenvectors(OBB* box, const modelInfo& modInfo);
-    void calculateBoxAxis(OBB* box, const modelInfo& modInfo);
+    /**  gets the id of the current child @return id of the child */
+    inline const int getID() { return this->nextID++; }
 
 
-    void forkBox(OBB* box);
+  private:
+    void calculateBoxAxis(OBB& box, const sVec3D* verticesList, unsigned int length);
 
-    bool overlapTest(OBB* boxA, OBB* boxB, WorldEntity* nodeA, WorldEntity* nodeB);
+    void calculateBoxCovariance(OBB& box, const modelInfo& modelInf, const int* triangleIndexes, int length);
+    void calculateBoxEigenvectors(OBB& box, const modelInfo& modelInf, const int* triangleIndexes, int length);
+    void calculateBoxAxis(OBB& box, const modelInfo& modelInf, const int* triangleIndexes, int length);
+    void forkBox(OBB& box);
+
+    bool overlapTest(OBB& boxA, OBB& boxB, WorldEntity* nodeA, WorldEntity* nodeB);
+
 
   protected:
     OBB*                bvElement;                  //!< the obb element
+    OBBTreeNode*        nodePrev;                   //!< ref to the previous (parent) tree node = NULL if first
     OBBTreeNode*        nodeLeft;                   //!< ref to the left tree node
     OBBTreeNode*        nodeRight;                  //!< ref to the right tree node
 
+
   private:
-    unsigned int        treeIndex;                  //!< Index number of the BV in the tree
-    const sVec3D*       vertices;                   //!< pointer to the vertices data
-    int                 numOfVertices;              //!< number of vertices in vertices data
+    int                 treeIndex;                  //!< Index number of the BV in the tree
+    int                 nextID;                     //!< the id of the next child
     int                 depth;                      //!< the depth of the node in the tree
-    static OBBTree*     obbTree;                    //!< reference to the obb tree
-    Plane*              separationPlane;            //!< the separation plane of the obb
-    const sVec3D*       sepPlaneCenter;             //!< only needed to draw plane
+    const OBBTree*      obbTree;                    //!< reference to the obb tree
+
+    const modelInfo*    modelInf;                   //!< pointer to the models modelInfo object
+    const int*          triangleIndexes;            //!< indexes to the used model triangles
+
+    Plane               separationPlane;            //!< the separation plane of the obb
+    sVec3D              sepPlaneCenter;             //!< only needed to draw plane
     int                 longestAxisIndex;           //!< only needed to draw plane
 
     /* tmp saving place for obb variables */
-    sVec3D*             tmpVert1;                   //!< pointer to the vert data of obbox1
-    sVec3D*             tmpVert2;                   //!< pointer to the vert data of obbox1
-    int                 tmpLen1;                    //!< len vert data obbox1
-    int                 tmpLen2;                    //!< len vert data obbox2
+    int*                triangleIndexList1;         //!< pointer to the vert data of obbox1
+    int*                triangleIndexList2;         //!< pointer to the vert data of obbox1
+    int                 triangleIndexLength1;       //!< len vert data obbox1
+    int                 triangleIndexLength2;       //!< len vert data obbox2
 
-    static float**      coMat;
-    static float**      eigvMat;
-    static float*       eigvlMat;
-    static int*         rotCount;
+    WorldEntity*        owner;
 };
 
Index: trunk/src/lib/graphics/importer/static_model.cc
===================================================================
--- trunk/src/lib/graphics/importer/static_model.cc	(revision 7677)
+++ trunk/src/lib/graphics/importer/static_model.cc	(revision 7711)
@@ -196,4 +196,5 @@
   this->buildTriangleList();
 
+  // write out the modelInfo data used for the collision detection!
   this->pModelInfo.pVertices = &this->vertices[0];
   this->pModelInfo.pNormals = &this->normals[0];
@@ -805,9 +806,11 @@
     this->buildVertexNormals();
 
-  int index = 0;                   //!< the counter for the triangle array
-  ModelFaceElement* tmpElem;       //!< the temporary faceelement reference
-  ModelFace* tmpFace;              //!< the temporary face referece
+  int                index = 0;                   //!< the counter for the triangle array
+  ModelFaceElement*  tmpElem;       //!< the temporary faceelement reference
+  ModelFace*         tmpFace;              //!< the temporary face referece
 
   bool warned = false;
+
+  this->pModelInfo.numTriangles = 0;
 
   /* count the number of triangles */
@@ -819,22 +822,16 @@
     while( tmpFace != NULL)
     {
-
       /* if its a triangle just add it to the list */
-      if( tmpFace->vertexCount == 3)
-      {
+      if( tmpFace->vertexCount == 3){
         ++this->pModelInfo.numTriangles;
       } /* if the polygon is a quad */
-      else if( tmpFace->vertexCount == 4)
-      {
+      else if( tmpFace->vertexCount == 4) {
         this->pModelInfo.numTriangles += 2;
       }
-      else if( tmpFace->vertexCount > 4)
-      {
-        if (!warned)
-        {
+      else if( tmpFace->vertexCount > 4) {
+        if (!warned)        {
           PRINTF(2)("This model (%s) got over 4 vertices per face <=> conflicts in the CD engine!\n", this->getName());
           warned = true;
         }
-        //exit(0);
       }
       tmpFace = tmpFace->next;
@@ -859,5 +856,4 @@
   while( this->currentGroup != NULL)
   {
-    // Putting Faces to GL
     tmpFace = this->currentGroup->firstFace;
     while( tmpFace != NULL)
@@ -870,8 +866,9 @@
         for( int j = 0; j < 3; ++j)
         {
-          this->pModelInfo.pTriangles[index].indexToVertices[j] = (unsigned int)tmpElem->vertexNumber /* * 3 */;
-          this->pModelInfo.pTriangles[index].indexToNormals[j] = (unsigned int)tmpElem->normalNumber /* * 3 */;
-          this->pModelInfo.pTriangles[index].indexToTexCoor[j] = (unsigned int)tmpElem->texCoordNumber /* * 3 */;
+          this->pModelInfo.pTriangles[index].indexToVertices[j] = (unsigned int)tmpElem->vertexNumber * 3 ;
+          this->pModelInfo.pTriangles[index].indexToNormals[j] = (unsigned int)tmpElem->normalNumber * 3 ;
+          this->pModelInfo.pTriangles[index].indexToTexCoor[j] = (unsigned int)tmpElem->texCoordNumber * 3 ;
           tmpElem = tmpElem->next;
+
         }
         ++index;
@@ -880,30 +877,30 @@
       {
 
-        this->pModelInfo.pTriangles[index].indexToVertices[0] = (unsigned int)tmpElem->vertexNumber;
-        this->pModelInfo.pTriangles[index].indexToNormals[0] = (unsigned int)tmpElem->normalNumber;
-        this->pModelInfo.pTriangles[index].indexToTexCoor[0] = (unsigned int)tmpElem->texCoordNumber;
-
-        this->pModelInfo.pTriangles[index + 1].indexToVertices[0] = (unsigned int)tmpElem->vertexNumber;
-        this->pModelInfo.pTriangles[index + 1].indexToNormals[0] = (unsigned int)tmpElem->normalNumber;
-        this->pModelInfo.pTriangles[index + 1].indexToTexCoor[0] = (unsigned int)tmpElem->texCoordNumber;
+        this->pModelInfo.pTriangles[index].indexToVertices[0] = (unsigned int)tmpElem->vertexNumber * 3;
+        this->pModelInfo.pTriangles[index].indexToNormals[0] = (unsigned int)tmpElem->normalNumber * 3;
+        this->pModelInfo.pTriangles[index].indexToTexCoor[0] = (unsigned int)tmpElem->texCoordNumber * 3;
+
+        this->pModelInfo.pTriangles[index + 1].indexToVertices[0] = (unsigned int)tmpElem->vertexNumber * 3;
+        this->pModelInfo.pTriangles[index + 1].indexToNormals[0] = (unsigned int)tmpElem->normalNumber * 3;
+        this->pModelInfo.pTriangles[index + 1].indexToTexCoor[0] = (unsigned int)tmpElem->texCoordNumber * 3;
         tmpElem = tmpElem->next;
 
-        this->pModelInfo.pTriangles[index].indexToVertices[1] = (unsigned int)tmpElem->vertexNumber;
-        this->pModelInfo.pTriangles[index].indexToNormals[1] = (unsigned int)tmpElem->normalNumber;
-        this->pModelInfo.pTriangles[index].indexToTexCoor[1] = (unsigned int)tmpElem->texCoordNumber;
+        this->pModelInfo.pTriangles[index].indexToVertices[1] = (unsigned int)tmpElem->vertexNumber * 3;
+        this->pModelInfo.pTriangles[index].indexToNormals[1] = (unsigned int)tmpElem->normalNumber * 3;
+        this->pModelInfo.pTriangles[index].indexToTexCoor[1] = (unsigned int)tmpElem->texCoordNumber * 3;
         tmpElem = tmpElem->next;
 
-        this->pModelInfo.pTriangles[index].indexToVertices[2] = (unsigned int)tmpElem->vertexNumber;
-        this->pModelInfo.pTriangles[index].indexToNormals[2] = (unsigned int)tmpElem->normalNumber;
-        this->pModelInfo.pTriangles[index].indexToTexCoor[2] = (unsigned int)tmpElem->texCoordNumber;
-
-        this->pModelInfo.pTriangles[index + 1].indexToVertices[2] = (unsigned int)tmpElem->vertexNumber;
-        this->pModelInfo.pTriangles[index + 1].indexToNormals[2] = (unsigned int)tmpElem->normalNumber;
-        this->pModelInfo.pTriangles[index + 1].indexToTexCoor[2] = (unsigned int)tmpElem->texCoordNumber;
+        this->pModelInfo.pTriangles[index].indexToVertices[2] = (unsigned int)tmpElem->vertexNumber * 3;
+        this->pModelInfo.pTriangles[index].indexToNormals[2] = (unsigned int)tmpElem->normalNumber * 3;
+        this->pModelInfo.pTriangles[index].indexToTexCoor[2] = (unsigned int)tmpElem->texCoordNumber * 3;
+
+        this->pModelInfo.pTriangles[index + 1].indexToVertices[2] = (unsigned int)tmpElem->vertexNumber * 3;
+        this->pModelInfo.pTriangles[index + 1].indexToNormals[2] = (unsigned int)tmpElem->normalNumber * 3;
+        this->pModelInfo.pTriangles[index + 1].indexToTexCoor[2] = (unsigned int)tmpElem->texCoordNumber * 3;
         tmpElem = tmpElem->next;
 
-        this->pModelInfo.pTriangles[index + 1].indexToVertices[1] = (unsigned int)tmpElem->vertexNumber;
-        this->pModelInfo.pTriangles[index + 1].indexToNormals[1] = (unsigned int)tmpElem->normalNumber;
-        this->pModelInfo.pTriangles[index + 1].indexToTexCoor[1] = (unsigned int)tmpElem->texCoordNumber;
+        this->pModelInfo.pTriangles[index + 1].indexToVertices[1] = (unsigned int)tmpElem->vertexNumber * 3;
+        this->pModelInfo.pTriangles[index + 1].indexToNormals[1] = (unsigned int)tmpElem->normalNumber * 3;
+        this->pModelInfo.pTriangles[index + 1].indexToTexCoor[1] = (unsigned int)tmpElem->texCoordNumber * 3;
 
         index += 2;
Index: trunk/src/lib/math/matrix.cc
===================================================================
--- trunk/src/lib/math/matrix.cc	(revision 7677)
+++ trunk/src/lib/math/matrix.cc	(revision 7711)
@@ -244,4 +244,6 @@
 
     eigVc3 = eigVc1.cross(eigVc2);
+
+    eigVc2 = eigVc3.cross(eigVc1);
   }
   else if (eigenValuesCount == 1)
@@ -254,4 +256,11 @@
   eigVc2.normalize();
   eigVc3.normalize();
+
+  if (!(eigVc1.cross(eigVc3) == eigVc2))
+  {
+    eigVc3.cross(eigVc1);
+//     eigVc2.debug();
+  }
+/*  printf("ok\n")*/;
 }
 
Index: trunk/src/lib/math/plane.cc
===================================================================
--- trunk/src/lib/math/plane.cc	(revision 7677)
+++ trunk/src/lib/math/plane.cc	(revision 7711)
@@ -222,4 +222,5 @@
 }
 
+
 /**
  *  calculate the two points of minimal distance of two lines
@@ -267,5 +268,5 @@
 {
   n = (a-b).cross(c-b);
-  k = -(n.x*b.x+n.y*b.y+n.z*b.z);
+  k = n.dot(a) / n.len();
 }
 
@@ -278,5 +279,5 @@
 {
   n = norm;
-  k = -(n.x*p.x+n.y*p.y+n.z*p.z);
+  k = n.dot(p) / n.len();
 }
 
@@ -291,5 +292,5 @@
   Vector p(g[0], g[1], g[2]);
   n = norm;
-  k = -(n.x*p.x+n.y*p.y+n.z*p.z);
+  k = n.dot(p) / n.len();
 }
 
@@ -315,5 +316,5 @@
   float l = n.len();
   if( l == 0.0) return 0.0;
-  return (n.dot(p) + k) / n.len();
+  return (n.dot(p) / n.len() - k);
 }
 
@@ -324,10 +325,25 @@
  * @return the distance between the plane and the point (can be negative)
  */
-float Plane::distancePoint (const sVec3D& p) const
+// float Plane::distancePoint (const sVec3D& p) const
+// {
+//   Vector s(p[0], p[1], p[2]);
+//   float l = n.len();
+//   if( l == 0.0) return 0.0;
+//   return (n.dot(s) + k) / n.len();
+// }
+
+
+/**
+ *  returns the distance between the plane and a point
+ * @param p: a Point
+ * @return the distance between the plane and the point (can be negative)
+ */
+float Plane::distancePoint (const float* p) const
 {
   Vector s(p[0], p[1], p[2]);
+
   float l = n.len();
   if( l == 0.0) return 0.0;
-  return (n.dot(s) + k) / n.len();
+  return (n.dot(s) / n.len() - k);
 }
 
Index: trunk/src/lib/math/plane.h
===================================================================
--- trunk/src/lib/math/plane.h	(revision 7677)
+++ trunk/src/lib/math/plane.h	(revision 7711)
@@ -51,5 +51,6 @@
   Vector intersectLine (const Line& l) const;
   float distancePoint (const Vector& p) const;
-  float distancePoint (const sVec3D& p) const;
+//   float distancePoint (const sVec3D& p) const;
+  float distancePoint (const float* p) const;
   float locatePoint (const Vector& p) const;
 };
Index: trunk/src/lib/math/vector.h
===================================================================
--- trunk/src/lib/math/vector.h	(revision 7677)
+++ trunk/src/lib/math/vector.h	(revision 7711)
@@ -85,4 +85,5 @@
   /** copy constructor* @param v the sVec3D to assign to this vector. @returns the vector v */
   inline const Vector& operator= (const sVec3D& v) { this->x = v[0]; this->y = v[1]; this->z = v[2]; }
+  inline const Vector& operator= (const float* v) { this->x = v[0]; this->y = v[1]; this->z = v[2]; }
   /** @param v: the other vector \return the dot product of the vectors */
   float dot (const Vector& v) const { return x*v.x+y*v.y+z*v.z; };
Index: trunk/src/lib/util/executor/executor_specials.h
===================================================================
--- trunk/src/lib/util/executor/executor_specials.h	(revision 7677)
+++ trunk/src/lib/util/executor/executor_specials.h	(revision 7711)
@@ -8,4 +8,5 @@
 
 #include "executor.h"
+#include "compiler.h"
 
 #include "compiler.h"
Index: trunk/src/lib/util/executor/functor_list.h
===================================================================
--- trunk/src/lib/util/executor/functor_list.h	(revision 7677)
+++ trunk/src/lib/util/executor/functor_list.h	(revision 7711)
@@ -140,5 +140,5 @@
 
   FUNCTOR_LIST(3)(l_STRING, l_FLOAT, l_UINT);
-
+  FUNCTOR_LIST(4)(l_STRING, l_FLOAT, l_UINT, l_UINT);
 
 #endif /* FUNCTOR_LIST */
