Index: orxonox/branches/parenting/src/curve.cc
===================================================================
--- orxonox/branches/parenting/src/curve.cc	(revision 3322)
+++ orxonox/branches/parenting/src/curve.cc	(revision 3327)
@@ -23,7 +23,9 @@
 
 #include "curve.h"
+#include "matrix.h"
 
 #include <math.h>
 #include <stdio.h>
+
 
 /**
@@ -45,4 +47,8 @@
 
 
+///////////////////////////////////
+/// Bezier Curve //////////////////
+///////////////////////////////////
+
 /**
    \brief Creates a new BezierCurve
@@ -70,5 +76,5 @@
    It does this by freeing all the space taken over from the nodes
 */
-BezierCurve::~BezierCurve (void)
+BezierCurve::~BezierCurve(void)
 {
   PathNode* tmpNode;
@@ -197,2 +203,179 @@
   return curvePoint;
 }
+
+
+
+///////////////////////////////////
+//// Uniform Point curve  /////////
+///////////////////////////////////
+/**
+   \brief Creates a new UPointCurve
+*/
+UPointCurve::UPointCurve (void)
+{
+  this->derivation = 0;
+  dirCurve = new UPointCurve(1);
+  this->init();
+}
+
+/**
+   \brief Creates a new UPointCurve-Derivation-Curve of deriavation'th degree
+*/
+UPointCurve::UPointCurve (int derivation)
+{
+  this->derivation = derivation;
+  dirCurve=NULL;
+  this->init();
+}
+
+/**
+   \brief Deletes a UPointCurve.
+
+   It does this by freeing all the space taken over from the nodes
+*/
+UPointCurve::~UPointCurve(void)
+{
+  PathNode* tmpNode;
+  currentNode = firstNode;
+  while (tmpNode != 0)
+    {
+      tmpNode = currentNode;
+      currentNode = currentNode->next;
+      delete tmpNode;
+    }
+  if (dirCurve)
+    delete dirCurve;
+}
+
+/**
+   \brief Initializes a UPointCurve
+*/
+void UPointCurve::init(void)
+{
+  nodeCount = 0;
+  firstNode = new PathNode;
+  currentNode = firstNode;
+
+  firstNode->position = Vector (.0, .0, .0);
+  firstNode->number = 0;
+  firstNode->next = 0; // not sure if this really points to NULL!!
+
+  return;
+}
+
+/**
+   \brief Rebuilds a UPointCurve
+   
+   \todo very bad algorithm
+*/
+void UPointCurve::rebuild(void)
+{
+  // rebuilding the Curve itself
+  PathNode* tmpNode = this->firstNode;
+  int i=0;
+  Matrix xTmpMat = Matrix(this->nodeCount, this->nodeCount);
+  Matrix yTmpMat = Matrix(this->nodeCount, this->nodeCount);
+  Matrix zTmpMat = Matrix(this->nodeCount, this->nodeCount);
+  Matrix xValMat = Matrix(this->nodeCount, 3);
+  Matrix yValMat = Matrix(this->nodeCount, 3);
+  Matrix zValMat = Matrix(this->nodeCount, 3);
+  while(tmpNode)
+    {
+      Vector fac = Vector(1,1,1);
+      for (int j = 0; j < this->nodeCount; j++)
+	{
+	  xTmpMat(i,j) = fac.x; fac.x *= (float)i/(float)this->nodeCount;//tmpNode->position.x;
+	  yTmpMat(i,j) = fac.y; fac.y *= (float)i/(float)this->nodeCount;//tmpNode->position.y;
+	  zTmpMat(i,j) = fac.z; fac.z *= (float)i/(float)this->nodeCount;//tmpNode->position.z;
+	}
+      xValMat(i,0) = tmpNode->position.x;
+      yValMat(i,0) = tmpNode->position.y;
+      zValMat(i,0) = tmpNode->position.z;
+      ++i;
+      tmpNode = tmpNode->next;
+    }
+  tmpNode = this->firstNode;
+  xValMat = xTmpMat.Inv() *= xValMat;
+  yValMat = yTmpMat.Inv() *= yValMat;
+  zValMat = zTmpMat.Inv() *= zValMat;
+  i = 0;
+  while(tmpNode)
+    {
+      tmpNode->vFactor.x = xValMat(i,0);
+      tmpNode->vFactor.y = yValMat(i,0);
+      tmpNode->vFactor.z = zValMat(i,0);
+
+      i++;
+      tmpNode = tmpNode->next;
+    }
+
+  // rebuilding the Derivation curve
+  if(this->derivation == 0)
+    {
+      tmpNode = firstNode;
+      delete dirCurve;
+      dirCurve = new UPointCurve(1);
+      while(tmpNode->next)
+	{
+	  Vector tmpVector = (tmpNode->next->position)- (tmpNode->position);
+	  tmpVector.x*=(float)nodeCount;
+	  tmpVector.y*=(float)nodeCount;
+	  tmpVector.z*=(float)nodeCount;
+	  tmpVector.normalize();
+	  this->dirCurve->addNode(tmpVector);
+	  tmpNode = tmpNode->next;
+	}
+    }
+}
+
+/**
+   \brief calculates the Position on the curve
+   \param t The position on the Curve (0<=t<=1)
+   \return the Position on the Path
+*/
+Vector UPointCurve::calcPos(float t) 
+{
+  PathNode* tmpNode = firstNode;
+  Vector ret = Vector(0.0,0.0,0.0);
+  float factor = 1.0;
+  while(tmpNode)
+    {
+      ret.x += tmpNode->vFactor.x * factor;
+      ret.y += tmpNode->vFactor.y * factor;
+      ret.z += tmpNode->vFactor.z * factor;
+      factor *= t;
+
+      tmpNode = tmpNode->next;
+    }
+  return ret;
+}
+
+/**
+   \brief Calulates the direction of the Curve at time t.
+   \param The time at which to evaluate the curve.
+   \returns The vvaluated Vector.
+*/
+Vector UPointCurve::calcDir (float t)
+{
+  return dirCurve->calcPos(t);
+}
+
+/**
+   \brief Calculates the Quaternion needed for our rotations
+   \param t The time at which to evaluate the cuve.
+   \returns The evaluated Quaternion.
+*/
+Quaternion UPointCurve::calcQuat (float t)
+{
+  return Quaternion (calcDir(t), Vector(0,0,1));
+}
+
+
+/**
+  \brief returns the Position of the point calculated on the Curve
+  \return a Vector to the calculated position
+*/
+Vector UPointCurve::getPos(void) const
+{
+  return curvePoint;
+}
