Index: orxonox/trunk/src/lib/graphics/spatial_separation/quadtree.cc
===================================================================
--- orxonox/trunk/src/lib/graphics/spatial_separation/quadtree.cc	(revision 4955)
+++ orxonox/trunk/src/lib/graphics/spatial_separation/quadtree.cc	(revision 4956)
@@ -159,5 +159,5 @@
   this function will return the quadtree that contains the position
  */
-QuadtreeNode* Quadtree::getQuadtreeFromPosition(const Vector& position)
+QuadtreeNode* Quadtree::getQuadtreeFromPosition(const Vector& position) const
 {
   /* shift into model space */
@@ -182,5 +182,5 @@
   this function will return the quadtree that contains the position
  */
-sTriangleExt* Quadtree::getTriangleFromPosition(const Vector& position)
+sTriangleExt* Quadtree::getTriangleFromPosition(const Vector& position) const
 {
   QuadtreeNode* q = this->getQuadtreeFromPosition(position);
Index: orxonox/trunk/src/lib/graphics/spatial_separation/quadtree.h
===================================================================
--- orxonox/trunk/src/lib/graphics/spatial_separation/quadtree.h	(revision 4955)
+++ orxonox/trunk/src/lib/graphics/spatial_separation/quadtree.h	(revision 4956)
@@ -27,6 +27,6 @@
     virtual ~Quadtree();
 
-    QuadtreeNode* getQuadtreeFromPosition(const Vector& position);
-    sTriangleExt* getTriangleFromPosition(const Vector& position);
+    QuadtreeNode* getQuadtreeFromPosition(const Vector& position) const;
+    sTriangleExt* getTriangleFromPosition(const Vector& position) const;
 
     void drawTree() const;
Index: orxonox/trunk/src/lib/graphics/spatial_separation/quadtree_node.cc
===================================================================
--- orxonox/trunk/src/lib/graphics/spatial_separation/quadtree_node.cc	(revision 4955)
+++ orxonox/trunk/src/lib/graphics/spatial_separation/quadtree_node.cc	(revision 4956)
@@ -389,24 +389,78 @@
 
 
-sTriangleExt* QuadtreeNode::getTriangle(const Vector& position)
-{
-
-}
-
-bool QuadtreeNode::pointInTriangle(const Vector&p, const Vector& a, const Vector& b, const Vector& c)
+
+float QuadtreeNode::getHeight(const Vector& position) const
+{
+  Vector a, b, c;
+  sTriangleExt* tri;
+
+  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;
+
+  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;
+}
+
+
+/**
+ *  checks if the point is in the triangle
+ * @param point to be checked
+ * @param rectangle edge a
+ * @param rectangle edge b
+ * @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
 {
   if( this->sameSide(p, a, b, c) && this->sameSide(p, b, a, c) && sameSide(p, c, a, b))
     return true;
   return false;
-  /*
-  function PointInTriangle(p, a,b,c)
-  if SameSide(p,a, b,c) and SameSide(p,b, a,c)
-  and SameSide(p,c, a,b) then return true
-  else return false
-  */
-}
-
-
-bool QuadtreeNode::sameSide(const Vector& p1, const Vector&p2, const Vector& a, const Vector& b)
+}
+
+
+/**
+ *  checks if two points are on the same side 
+ * @param point one to be checked
+ * @param point two to be checked
+ * @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
 {
   Vector cp1 = (b - a).cross(p1 - a);
Index: orxonox/trunk/src/lib/graphics/spatial_separation/quadtree_node.h
===================================================================
--- orxonox/trunk/src/lib/graphics/spatial_separation/quadtree_node.h	(revision 4955)
+++ orxonox/trunk/src/lib/graphics/spatial_separation/quadtree_node.h	(revision 4956)
@@ -36,8 +36,10 @@
 
     void buildHashTable(QuadtreeNode** nodeList, int* index);
+    inline Rectangle* getDimension() { return this->pDimension; }
+
     bool includesPoint(const Vector& v);
-    sTriangleExt* getTriangle(const Vector& position);
-    float getHeight(const Vector& position);
-    inline Rectangle* getDimension() { return this->pDimension; }
+    sTriangleExt* getTriangle(const Vector& position) const;
+    float getHeight(const Vector& position) const;
+
 
     void drawTree() const;
@@ -52,6 +54,6 @@
     Rectangle* getDimFromModel();
 
-    bool sameSide(const Vector& p1, const Vector&p2, const Vector& a, const Vector& b);
-    bool pointInTriangle(const Vector&p, const Vector& a, const Vector& b, const Vector& c);
+    bool sameSide(const Vector& p1, const Vector&p2, const Vector& a, const Vector& b) const;
+    bool pointInTriangle(const Vector&p, const Vector& a, const Vector& b, const Vector& c) const;
 
   protected:
