source: orxonox.OLD/orxonox/branches/trackManager/src/lib/math/curve.cc @ 3542

/* 
   orxonox - the future of 3D-vertical-scrollers

   Copyright (C) 2004 orx

   This program is free software; you can redistribute it and/or modify
   it under the terms of the GNU General Public License as published by
   the Free Software Foundation; either version 2, or (at your option)
   any later version.

   ### File Specific:
   main-programmer: Benjamin Grauer
   co-programmer: Patrick Boenzli

   ADD: Patrick Boenzli           B-Spline 


   TODO: 
     local-Time implementation
     NURBS
     
*/

#include "curve.h"
#include "matrix.h"
#include "debug.h"

#include <math.h>
#include <stdio.h>

/**
   \brief adds a new Node to the bezier Curve
   \param newNode a Vector to the position of the new node
*/
void Curve::addNode(const Vector& newNode)
{
  if (nodeCount != 0 )
    {
      currentNode = currentNode->next = new PathNode;
    }
  currentNode->position = newNode;
  currentNode->next = 0;
  currentNode->number = (++nodeCount);
  this->rebuild();
  return;
}

/**
   \brief adds a new Node to the bezier Curve ath Position insertPosition
   \param insertPosition The Position on the Path to insert the node.
   \param newNode a Vector to the position of the new node
*/
void Curve::addNode(const Vector& newNode, unsigned int insertPosition)
{

  if (this->nodeCount == 0 || insertPosition > this->nodeCount)
    return addNode(newNode);

  if (insertPosition == 0)
    insertPosition = 1;

  PathNode* insNode = new PathNode;

  // relinking
  PathNode* tmpNode = this->firstNode;
  if (insertPosition > 1)
    {
      while (tmpNode->next->number != insertPosition)
        tmpNode= tmpNode->next;
      insNode->next = tmpNode->next;
      tmpNode->next = insNode;
    }
  else
    {
      insNode->next = this->firstNode;
      this->firstNode = insNode;
    }
  // renumbering
  insNode->number = insertPosition;
  tmpNode = insNode->next;
  while (tmpNode)
    {
      tmpNode->number++;
      tmpNode = tmpNode->next;
    }

    // finished
  insNode->position = newNode;
  ++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 || nodeToFind < 0)
    return Vector(0,0,0);
  PathNode* tmpNode = this->firstNode;
  for (int i = 1; i < nodeToFind; i++)
    tmpNode = tmpNode->next;
  return tmpNode->position;
}

/**
   \brief Outputs information about the state of this Curve
*/
void Curve::debug(void)
{
  printf("<<-------------------------------\n");
  printf("Curve Information:\n");
  printf("NodeCount: %d\n", this->nodeCount);
  PathNode* tmpNode = this->firstNode;
  while (tmpNode)
    {
      printf("node #%d: %f, %f, %f\n", tmpNode->number, tmpNode->position.x, tmpNode->position.y, tmpNode->position.z);
      tmpNode = tmpNode->next;
    }
  printf("------------------------------->>\n");
}


///////////////////////////////////
/// Bezier Curve //////////////////
///////////////////////////////////

/**
   \brief Creates a new BezierCurve
*/
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;
  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 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 <= 1)
    {
      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 t The time at which to evaluate the curve.
   \returns The Directional Vector.
*/
Vector BezierCurve::calcDir (float t)
{
  return dirCurve->calcPos(t);
}

/**
   \brief Calulates the acceleration of the Curve at time t.
   \param t The time at which to evaluate the curve.
   \returns The acceleration-Vector.
*/
Vector BezierCurve::calcAcc (float t)
{
  return dirCurve->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
*/
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;
    }
}

/**
   \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 t The time at which to evaluate the curve.
   \returns The vvaluated Vector.
*/
Vector UPointCurve::calcDir (float t)
{
  PathNode* tmpNode = firstNode;
  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.normalize();
  return ret;
}

Vector UPointCurve::calcAcc (float 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;
}