source: orxonox.OLD/trunk/src/lib/math/curve.cc @ 10556

/*
   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
     !!tList implementation!!

*/

#define DEBUG_SPECIAL_MODULE DEBUG_MODULE_MATH

#include "curve.h"

#include "debug.h"

#include <cmath>
#include <stdio.h>


/**
  *  default constructor for a Curve
*/
Curve::Curve()
{
  this->nodeCount = 0;
  this->localTime = 0;
  this->derivation = 0;
  this->dirCurve = NULL;
  this->firstNode = new PathNode();
  this->currentNode = firstNode;

  this->firstNode->position = Vector (.0, .0, .0);
  this->firstNode->number = 0;
  this->firstNode->next = 0; // not sure if this really points to NULL!!
}


Curve::~Curve()
{
  PathNode* pn = this->firstNode;
  PathNode* unusedPN;
  while( pn != NULL)
  {
    unusedPN = pn;
    pn = pn->next;
    delete unusedPN;
  }

  if (dirCurve)
    delete dirCurve;
}


/**
 *  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;
}

/**
 *  adds a new Node to the bezier Curve
 * @param newNode a Vector to the position of the new node
 * @param insertPosition after the n-th node the new node will be inserted
*/
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;
}

/**
 *  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;
}

/**
 * This function returns an approximation of the length of the curve
 *
*/
float Curve::getLength()
{
      float length = 0;
      PathNode* tmpNode = this->firstNode;
      for(int i = 1; i < this->nodeCount; i++)
      {
              length += Vector(tmpNode->next->position - tmpNode->position).len();
      }
      return length;
}

/**
 *  Outputs information about the state of this Curve
*/
void Curve::debug()
{
  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 //////////////////
///////////////////////////////////

/**
 *  Creates a new BezierCurve
*/
BezierCurve::BezierCurve ()
{
  this->derivation = 0;
  dirCurve = new BezierCurve(1);
}

/**
 *  Creates a new BezierCurve-Derivation-Curve
*/
BezierCurve::BezierCurve (int derivation)
{
  this->derivation = derivation;
  dirCurve=NULL;
}

/**
 *  Deletes a BezierCurve.

   It does this by freeing all the space taken over from the nodes
*/
BezierCurve::~BezierCurve()
{

}

/**
 *  Rebuilds a Curve
*/
void BezierCurve::rebuild()
{
  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;
        }
    }
}

/**
 *  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;
}

/**
 *  Calulates the direction of the Curve at time t.
 * @param t The time at which to evaluate the curve.
 * @returns The valuated Vector.
*/
Vector BezierCurve::calcDir (float t)
{
  return this->calcPos(t + 0.01) - this->calcPos(t);
  //return this->dirCurve->calcPos(t);
}

/**
 *  Calulates the acceleration (second derivate) of the Curve at time t.
 * @param t The time at which to evaluate the curve.
 * @returns The valuated Vector.
*/
Vector BezierCurve::calcAcc (float t)
{
  return this->dirCurve->getDirCurve()->calcPos(t);
}

/**
 *  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() const
{
  return curvePoint;
}