Changeset 3365 in orxonox.OLD for orxonox/trunk/src/curve.cc

Timestamp:

Jan 7, 2005, 1:14:33 AM (19 years ago)

Author:

bensch

Message:

orxonox/trunk: merged branches/parenting back to the.
merged with command:
svn merge branches/parenting trunk -r 3247:HEAD
resolved all conflicts in favor of parenting.

File:

• orxonox/trunk/src/curve.cc (modified) (10 diffs)

orxonox/trunk/src/curve.cc

@@ -11 +11 @@
    ### File Specific:
    main-programmer: Benjamin Grauer
-   co-programmer: ...
+   co-programmer: Patrick Boenzli
+
+   ADD: Patrick Boenzli           B-Spline 
+
 
    TODO: 
      local-Time implementation
-     closed Bezier Curve
      NURBS
      
@@ -21 +23 @@
 
 #include "curve.h"
-
-
+#include "matrix.h"
+#include "debug.h"
+
+#include <math.h>
+#include <stdio.h>
 
 /**
@@ -37 +42 @@
   currentNode->next = 0; // not sure if this really points to NULL!!
   currentNode->number = (++nodeCount);
+  this->rebuild();
   return;
 }
 
+/**
+   \brief Finds a Node by its Number, and returns its Position
+   \param nodeToFind the n'th node in the List of nodes
+   \returns A Vector to the Position of the Node.
+*/
+Vector Curve::getNode(unsigned int nodeToFind)
+{
+  if (nodeToFind > this->nodeCount)
+    return Vector(0,0,0);
+  PathNode* tmpNode = this->firstNode;
+  for (int i = 1; i < nodeToFind; i++)
+    tmpNode = tmpNode->next;
+  return tmpNode->position;
+}
+
+///////////////////////////////////
+/// Bezier Curve //////////////////
+///////////////////////////////////
 
 /**
@@ -45 +69 @@
 */
 BezierCurve::BezierCurve (void)
+{
+  this->derivation = 0;
+  dirCurve = new BezierCurve(1);
+  this->init();
+}
+
+/**
+   \brief Creates a new BezierCurve-Derivation-Curve
+*/
+BezierCurve::BezierCurve (int derivation)
+{
+  this->derivation = derivation;
+  dirCurve=NULL;
+  this->init();
+}
+
+/**
+   \brief Deletes a BezierCurve.
+
+   It does this by freeing all the space taken over from the nodes
+*/
+BezierCurve::~BezierCurve(void)
+{
+  PathNode* tmpNode;
+  currentNode = firstNode;
+  while (tmpNode != 0)
+    {
+      tmpNode = currentNode;
+      currentNode = currentNode->next;
+      delete tmpNode;
+    }
+  if (dirCurve)
+    delete dirCurve;
+}
+
+/**
+   \brief Initializes a BezierCurve
+*/
+void BezierCurve::init(void)
 {
   nodeCount = 0;
@@ -58 +121 @@
 
 /**
-   \brief Deletes a BezierCurve.
+   \brief Rebuilds a Curve
+*/
+void BezierCurve::rebuild(void)
+{
+  PathNode* tmpNode = firstNode;
+
+  // rebuilding the Curve itself
+  float k=0;
+  float n = nodeCount -1;
+  float binCoef = 1;
+  while(tmpNode)
+    {
+      tmpNode->factor = binCoef;
+      if (tmpNode =tmpNode->next)
+        {
+          binCoef *=(n-k)/(k+1);
+          ++k;
+        }
+    }
+
+  // rebuilding the Derivation curve
+  if(this->derivation == 0)
+    {
+      tmpNode = firstNode;
+      delete dirCurve;
+      dirCurve = new BezierCurve(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 BezierCurve::calcPos(float t) 
+{
+  Vector ret = Vector(0.0,0.0,0.0);
+  if (this->nodeCount >= 3)
+    {
+      PathNode* tmpNode = this->firstNode;
+      double factor = pow(1.0-t,nodeCount-1);
+      while(tmpNode)
+        {
+          ret.x += tmpNode->factor * factor * tmpNode->position.x;
+          ret.y += tmpNode->factor * factor * tmpNode->position.y;
+          ret.z += tmpNode->factor * factor * tmpNode->position.z;
+          factor *= t/(1.0-t); // same as pow but much faster.
+          
+          tmpNode = tmpNode->next;
+        }
+    }
+  else if (nodeCount == 2)
+    {
+      ret = this->firstNode->position *(1.0-t);
+      ret = ret + this->firstNode->next->position * t;
+    }
+  else if (nodeCount == 1)
+    ret = this->firstNode->position;
+  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 BezierCurve::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 BezierCurve::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 BezierCurve::getPos(void) const
+{
+  return curvePoint;
+}
+
+
+
+///////////////////////////////////
+//// Uniform Point curve  /////////
+///////////////////////////////////
+/**
+   \brief Creates a new UPointCurve
+*/
+UPointCurve::UPointCurve (void)
+{
+  this->derivation = 0;
+  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
 */
-BezierCurve::~BezierCurve (void)
+UPointCurve::~UPointCurve(void)
 {
   PathNode* tmpNode;
@@ -72 +261 @@
       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;
+    }
 }
 
@@ -79 +333 @@
    \return the Position on the Path
 */
-Vector BezierCurve::calcPos(float t) 
-{
-  if (nodeCount <=4)
-    {
-      //      if (verbose >= 1)
-      //      printf ("Please define at least 4 nodes, until now you have only defined %i.\n", nodeCount);
-      return Vector(0,0,0);
-    }
+Vector UPointCurve::calcPos(float t) 
+{
   PathNode* tmpNode = firstNode;
-    
   Vector ret = Vector(0.0,0.0,0.0);
-  double factor;
-  int k=0;
-  while(tmpNode!=0)
-    {
-      k++;
-      factor = ncr (nodeCount, k);
-      
-      for (int j=0; j<nodeCount-k; j++)
-        factor*=(1-t);
-      for (int j=0; j<k; j++)
-        factor*=t;
-      ret.x += factor * tmpNode->position.x;
-      ret.y += factor * tmpNode->position.y;
-      ret.z += factor * tmpNode->position.z;
-      
+  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;
@@ -116 +355 @@
    \returns The vvaluated Vector.
 */
-Vector BezierCurve::calcDir (float t)
+Vector UPointCurve::calcDir (float t)
 {
   PathNode* tmpNode = firstNode;
-  BezierCurve* tmpCurve = new BezierCurve();
-  Vector ret;
-  Vector tmpVector;
-
-  while (tmpNode->next != 0)
-    {
-      tmpVector = (tmpNode->next->position)- (tmpNode->position);
-      tmpVector.x*=(float)nodeCount;
-      tmpVector.y*=(float)nodeCount;
-      tmpVector.z*=(float)nodeCount;
-
-      tmpCurve->addNode(tmpVector);
+  Vector ret = Vector(0.0,0.0,0.0);
+  float factor = 1.0/t;
+  int k=0;
+  while(tmpNode)
+    {
+      ret.x += tmpNode->vFactor.x * factor *k;
+      ret.y += tmpNode->vFactor.y * factor *k;
+      ret.z += tmpNode->vFactor.z * factor *k;
+      factor *= t;
+      k++;
       tmpNode = tmpNode->next;
     }
-  ret = tmpCurve->calcPos(t);
   ret.normalize();
-
   return ret;
 }
@@ -144 +379 @@
    \returns The evaluated Quaternion.
 */
-Quaternion BezierCurve::calcQuat (float t)
+Quaternion UPointCurve::calcQuat (float t)
 {
   return Quaternion (calcDir(t), Vector(0,0,1));
@@ -154 +389 @@
   \return a Vector to the calculated position
 */
-Vector BezierCurve::getPos() const
+Vector UPointCurve::getPos(void) const
 {
   return curvePoint;
 }
-
-/**
-   \brief ncr-calculator, did not find an other method
-   \todo a c++ variante to do this
-*/
-int ncr(int n, int i)
-{
-  int ret = 1;
-  for (int k=1; k<=n; k++)
-    ret*=k;
-  for (int k=1; k<=i; k++)
-    ret/=k;
-  for (int k=1; k<=n-i; k++)
-    ret/=k;
-
-  return ret;
-}
-