Index: orxonox/trunk/src/lib/graphics/spatial_separation/quadtree.cc
===================================================================
--- orxonox/trunk/src/lib/graphics/spatial_separation/quadtree.cc	(revision 4957)
+++ orxonox/trunk/src/lib/graphics/spatial_separation/quadtree.cc	(revision 4968)
@@ -168,8 +168,10 @@
 
   /* check if object is still inside the terrain - this check is not complete @todo check all 4 bounds */
-  if( i < this->maxIndex && j < this->maxIndex)
-    this->nodes[i + j * this->maxIndex]->includesPoint(position);
-  else
-    PRINTF(5)("Object has left terrain\n");
+  if( i < this->maxIndex && j < this->maxIndex && i >= 0 && j >= 0)
+    return this->nodes[i + j * this->maxIndex];
+
+
+  PRINTF(4)("Object has left terrain\n");
+  return NULL;
 }
 
@@ -185,5 +187,8 @@
 {
   QuadtreeNode* q = this->getQuadtreeFromPosition(position);
-  return q->getTriangle(position);
+
+  if( likely(q != NULL))
+    return q->getTriangle(position - *this->offset);
+  return NULL;
 }
 
Index: orxonox/trunk/src/lib/graphics/spatial_separation/quadtree_node.cc
===================================================================
--- orxonox/trunk/src/lib/graphics/spatial_separation/quadtree_node.cc	(revision 4957)
+++ orxonox/trunk/src/lib/graphics/spatial_separation/quadtree_node.cc	(revision 4968)
@@ -134,4 +134,10 @@
   if( this->nodeD != NULL)
     delete this->nodeD;
+
+  if( this->pTriangles)
+    delete [] this->pTriangles;
+
+  if( this->pDimension)
+    delete this->pDimension;
 }
 
@@ -374,5 +380,5 @@
  * @returns true if the vector is included
   */
-bool QuadtreeNode::includesPoint(const Vector& v)
+bool QuadtreeNode::includesPoint(const Vector& v) const
 {
   Vector center = *this->pDimension->getCenter();
@@ -381,8 +387,5 @@
   if( v.x > center.x - ax && v.x < center.x + ax &&
       v.z > center.z - ax && v.z < center.z + ax )
-  {
-    this->bDraw = true;
     return true;
-  }
   return false;
 }
@@ -396,5 +399,38 @@
 
   for( int i = 0; i < this->numTriangles; ++i)
+  {
+    a = *(sVec3D*)&this->pVertices[this->pTriangles[i]->indexToVertices[0]];
+    b = *(sVec3D*)&this->pVertices[this->pTriangles[i]->indexToVertices[1]];
+    c = *(sVec3D*)&this->pVertices[this->pTriangles[i]->indexToVertices[2]];
+
+    if( unlikely(this->pointInTriangle(position, a, b, c)))
     {
+      tri = this->pTriangles[i];
+      break;
+    }
+  }
+
+  /* calculate height out of the data collected above */
+
+}
+
+
+/**
+ *  get triangles that includes the position
+ * @param position the position that is going to be checked
+ * @returns the triangle in which the position is included, NULL if there is no such triangle
+
+ There is some random behaviour if there are more than one triangle at the same y
+ coordinate. At the moment the function just takes the first triangle, that included the
+ vector
+ */
+sTriangleExt* QuadtreeNode::getTriangle(const Vector& position) const
+{
+  Vector a, b, c;
+  PRINTF(0)("Get Triangle, %i\n", this->numTriangles);
+
+  for( int i = 0; i < numTriangles; ++i)
+    {
+
       a = *(sVec3D*)&this->pVertices[this->pTriangles[i]->indexToVertices[0]];
       b = *(sVec3D*)&this->pVertices[this->pTriangles[i]->indexToVertices[1]];
@@ -402,36 +438,6 @@
 
       if( unlikely(this->pointInTriangle(position, a, b, c)))
-        {
-          tri = this->pTriangles[i];
-          break;
-        }
-    }
-
-  /* calculate height out of the data collected above */
-
-}
-
-
-/**
- *  get triangles that includes the position
- * @param position the position that is going to be checked
- * @returns the triangle in which the position is included, NULL if there is no such triangle
-
- There is some random behaviour if there are more than one triangle at the same y
- coordinate. At the moment the function just takes the first triangle, that included the
- vector
-*/
-sTriangleExt* QuadtreeNode::getTriangle(const Vector& position) const
-{
-  Vector a, b, c;
-
-  for( int i = 0; i < this->numTriangles; ++i)
-    {
-      a = *(sVec3D*)&this->pVertices[this->pTriangles[i]->indexToVertices[0]];
-      b = *(sVec3D*)&this->pVertices[this->pTriangles[i]->indexToVertices[1]];
-      c = *(sVec3D*)&this->pVertices[this->pTriangles[i]->indexToVertices[2]];
-
-      if( unlikely(this->pointInTriangle(position, a, b, c)))
         return this->pTriangles[i];
+
     }
   return NULL;
@@ -446,7 +452,13 @@
  * @param rectangle edge c
  * @returns true if the point is inside
-*/
+ */
 bool QuadtreeNode::pointInTriangle(const Vector&p, const Vector& a, const Vector& b, const Vector& c) const
 {
+
+  PRINTF(0)("p: (%f, %f, %f)\n", p.x, p.y, p.z);
+  PRINTF(0)("a: (%f, %f, %f)\n", a.x, a.y, a.z);
+  PRINTF(0)("b: (%f, %f, %f)\n", b.x, b.y, b.z);
+  PRINTF(0)("c: (%f, %f, %f)\n", c.x, c.y, c.z);
+
   if( this->sameSide(p, a, b, c) && this->sameSide(p, b, a, c) && sameSide(p, c, a, b))
     return true;
@@ -461,5 +473,5 @@
  * @param begining point of the line
  * @param end of the line
-*/
+ */
 bool QuadtreeNode::sameSide(const Vector& p1, const Vector&p2, const Vector& a, const Vector& b) const
 {
@@ -509,6 +521,7 @@
 void QuadtreeNode::draw() const
 {
-  if( likely(!bDraw))
+  if( likely(!this->bDraw))
     return;
+
   Vector t1 = *this->pDimension->getCenter();
   float ax = this->pDimension->getAxis();
@@ -524,4 +537,3 @@
 
   glEnd();
-
-}
+}
Index: orxonox/trunk/src/lib/graphics/spatial_separation/quadtree_node.h
===================================================================
--- orxonox/trunk/src/lib/graphics/spatial_separation/quadtree_node.h	(revision 4957)
+++ orxonox/trunk/src/lib/graphics/spatial_separation/quadtree_node.h	(revision 4968)
@@ -38,5 +38,5 @@
     inline Rectangle* getDimension() { return this->pDimension; }
 
-    bool includesPoint(const Vector& v);
+    bool includesPoint(const Vector& v) const;
     sTriangleExt* getTriangle(const Vector& position) const;
     float getHeight(const Vector& position) const;
@@ -63,5 +63,5 @@
     QuadtreeNode*                   nodeC;              //!< reference to the node C
     QuadtreeNode*                   nodeD;              //!< reference to the node D
-    QuadtreeNode**                  nodes;              //!< reference to the quadtree nodes
+    QuadtreeNode**                  nodes;              //!< reference to the quadtree nodes (nodeA, nodeB, nodeC, nodeD=[0..3])
 
 
Index: orxonox/trunk/src/lib/graphics/spatial_separation/spatial_separation.cc
===================================================================
--- orxonox/trunk/src/lib/graphics/spatial_separation/spatial_separation.cc	(revision 4957)
+++ orxonox/trunk/src/lib/graphics/spatial_separation/spatial_separation.cc	(revision 4968)
@@ -61,4 +61,6 @@
 SpatialSeparation::~SpatialSeparation ()
 {
+  if( this->quadtree)
+    delete this->quadtree;
 }
 
