source: orxonox.OLD/orxonox/trunk/src/track_manager.cc @ 3661

/* 
   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: ...
*/

#define DEBUG_SPECIAL_MODULE DEBUG_MODULE_TRACK_MANAGER

#include "track_manager.h"

#include "base_object.h"
#include "p_node.h"
#include "track_node.h"
#include "stdincl.h"
#include "list.h"



#include <stdarg.h>

using namespace std;

/**
   \brief initializes a TrackElement (sets the default values)
*/
TrackElement::TrackElement(void)
{
  this->isFresh = true;
  this->isHotPoint = false;
  this->isSavePoint = false;
  this->isFork = false;
  this->isJoined = false;
  this->mainJoin = false;
  this->ID = -1;
  this->startingTime = 0;
  this->duration = TMAN_DEFAULT_DURATION;
  this->endTime = 1;
  this->jumpTime = 0;
  this->width = TMAN_DEFAULT_WIDTH;
  this->nodeCount = 0;
  this->childCount = 0;
  this->name = NULL;
  this->curve = NULL;
  this->children = NULL;

  this->history = NULL;

  this->condFunc = &TrackElement::random;
}

/** 
    \brief destroys all alocated memory)
    \todo eventually when deleting a TrackElement you would not like to delete all its preceding TrackElements
*/
TrackElement::~TrackElement(void)
{
  if (this->name)
    delete []name;
  if (this->curve)
    delete this->curve;
  if ((!this->isJoined &&this->childCount > 0) || (this->isJoined && this->mainJoin))
    {
      tIterator<TrackElement>* iterator = this->children->getIterator();
      TrackElement* enumElem = iterator->nextElement();
      while (enumElem)
        {
          delete enumElem;
          enumElem = iterator->nextElement();
        }
      delete iterator;
      delete this->children;
    }
}

/**
   \brief Searches through all the TrackElements for trackID.
   \param trackID The ID to search for.
   \returns The TrackElement if Found, NULL otherwise.
   
   \todo make this more modular, to search for different things
*/
TrackElement* TrackElement::findByID(unsigned int trackID)
{
  // return if Found.
  if (this->ID == trackID)
    return this;
  // search on.
  if (this->childCount > 0)
    {
      tIterator<TrackElement>* iterator = this->children->getIterator();
      TrackElement* enumElem = iterator->nextElement();
      TrackElement* tmpElem;
      while (enumElem)
        {
          if ((tmpElem = enumElem->findByID(trackID)))
            return tmpElem;
          enumElem = iterator->nextElement();
        }
      delete iterator;
    }
  else 
    return NULL;
}


/**
   \brief checks if there are any BackLoops in the Track
   \param trackElem the trackElement to check about
   it simply does this by looking if the current trackElem is found again somewhere else in the Track
*/
bool TrackElement::backLoopCheck(TrackElement* trackElem)
{
  if (this->childCount == 0)
    return true;
  else
    {
      tIterator<TrackElement>* iterator = this->children->getIterator();
      TrackElement* enumElem = iterator->nextElement();
      while (enumElem)
        {
          if(!enumElem->backLoopCheck(trackElem))
            return false;
          enumElem = iterator->nextElement();
        }
      delete iterator;
      
      return true;
    }
}

/**
   \param childNumber which child to return
   \returns the n-the children (starting at 0)
*/
TrackElement* TrackElement::getChild(int childCount)
{
  if (this->childCount == 0)
    return NULL;
  if (childCount > this->childCount)
    childCount = this->childCount;
  
  TrackElement* enumElem = this->children->enumerate();
  for (int i = 0; i < childCount; i++)
    enumElem = this->children->nextElement();
  return enumElem;
}

/**
   \param name the Name to set.
*/
void TrackElement::setName(const char* name)
{
  //  delete the old name
  if (this->name)
    delete []this->name;
  // if a name was given already.
  if (name)
    {
      this->name = new char[strlen(name)+1];
      strcpy(this->name, name);
    }
  else 
    this->name = NULL;
}

/**
   \returns The name of this TrackElement
*/
char* TrackElement::getName(void) const
{
  return this->name;
}

/**
   \brief prints out debug information about this TrackElement
*/
void TrackElement::debug(void)
{
  PRINT(0)("--== TrackElement:%i ==--", this->ID);
  if(this->getName())
    PRINT(0)("Name: %s::", this->getName());
  if(this->isFresh)
    PRINT(0)("  -- has not jet eddited in any way --\n");
  PRINT(0)("\n   TimeTable: startingTime=%f; endTime=%f; duration=%f; jumpTime=%f\n", this->startingTime, this->endTime, this->duration, this->jumpTime);
  PRINT(0)("   consists of %d Points\n", this->nodeCount);
  if (this->childCount == 0)
    PRINT(0)("   has no child\n");
  else if (this->childCount == 1)
    PRINT(0)("   has 1 child: =%d=\n", this->getChild(0)->ID);
  else if (this->childCount > 1)
    {
      PRINT(0)("   has %d children: ", this->childCount);
      TrackElement* enumElem = this->children->enumerate();
      while (enumElem)
        {
          PRINT(0)("=%d= ", enumElem->ID);
          enumElem = this->children->nextElement();
        }
      PRINT(0)("\n");
    }
  
  if(this->isHotPoint)
    PRINT(0)("   is a special Point:\n");
  if(this->isSavePoint)
    PRINT(0)("    is a SavePoint\n");
  if(this->isFork)
    {
      PRINT(0)("    is A Fork with with %d children.\n", this->childCount);
    }
  if(this->isJoined)
    PRINT(0)("   is Joined at the End\n");
  
  if(!this->backLoopCheck(this)) /* this should not happen */
    PRINT(2)(" THERE IS A BACKLOOP TO THIS ELEMENT\n");
}

/**
   \brief CONDITION that chooses the first child for the decision (static)
   \param nothing Nothing in this function
   \returns the chosen child
*/
int TrackElement::lowest(void* nothing)
{
  return 0;
}

/**
   \brief CONDITION that chooses the last child for the decision (static)
   \param nothing Nothing in this function
   \returns the chosen child
*/
int TrackElement::highest(void* nothing)
{ 
  return this->childCount-1;
}

/**
   \brief CONDITION that chooses a random child for the decision (static)
   \param nothing Nothing in this function
   \returns the chosen child
*/
int TrackElement::random(void* nothing)
{
  int i = (int)floor ((float)rand()/(float)RAND_MAX * (float)this->childCount);
  if (i >= this->childCount)
    return this->childCount-1;
  else 
    return i;
}

/**
   \brief CONDITION that chooses child 0, if the node(probably Player)
   is left of its parent (z<0)) and 1/right otherwise.
   \param node The node to act upon.
   \returns the chosen child
*/
int TrackElement::leftRight(void* node)
{
  PNode* tmpNode = (PNode*)node;

  if (tmpNode->getRelCoor().z < 0)
    return 0;
  else 
    return 1;
}


/**
   \brief CONDITION that chooses the child, that has the nearest distance to the node (probably player).
   \param node The node to act upon.
   \returns the chosen child

   This is rather dangerous, because one must carefully set the points on the curve.
   The best Way is to set the nodes as wide away of each other as possible,
   but take into consideration, that if the nodes are to far from a center node, the center will be chosen.
   (play with this!!).
*/
int TrackElement::nearest(void* node)
{
  PNode* tmpNode = (PNode*)node;

  Vector nodeRelCoord = tmpNode->getRelCoor();
  float minDist = 100000000;
  int childNumber = 0;
  int i = 0;

  TrackElement* enumElem = this->children->enumerate();
  while (enumElem)
    {
      float dist = (nodeRelCoord - enumElem->curve->getNode(4)).len();
      if (dist < minDist)
        {
          minDist = dist;
          childNumber = i;
        }
      i++;
      enumElem = this->children->nextElement();
    }

  PRINTF(4)("PathDecision with nearest algorithm: %d\n", childNumber);
  return childNumber;
}


////////////////////////
///// TRACKMANAGER /////
////////////////////////
/**
   \brief standard constructor

*/
TrackManager::TrackManager(void)
{
  this->setClassName("TrackManager");
  
  TrackManager::singletonRef = this;

  PRINTF(3)("Initializing the TrackManager\n");
  this->firstTrackElem = new TrackElement();
  this->firstTrackElem->ID = 1;
  this->currentTrackElem = firstTrackElem;
  this->localTime = 0;
  this->maxTime = 0;
  this->trackElemCount = 1;
  this->setBindSlave(this->trackNode = new TrackNode());
}


/**
   \brief standard destructor
*/
TrackManager::~TrackManager(void)
{
  PRINTF(3)("Destruct TrackManager\n");

  PRINTF(4)("Deleting all the TrackElements\n");
  delete this->firstTrackElem;

  // we do not have a TrackManager anymore
  TrackManager::singletonRef = NULL;
}

//! Singleton Reference to TrackManager
TrackManager* TrackManager::singletonRef = NULL;

/**
   \returns The reference on the TrackManager.

   If the TrackManager does not exist, it will be created.
*/
TrackManager* TrackManager::getInstance(void) 
{
  if (!TrackManager::singletonRef)
    TrackManager::singletonRef = new TrackManager();
  return TrackManager::singletonRef;
}

/**
   \brief reserves Space for childCount children
   \param childCount The Count of children to make space for.
*/
void TrackManager::initChildren(unsigned int childCount)
{
  this->currentTrackElem->childCount = childCount;
  this->currentTrackElem->mainJoin = true;
  this->currentTrackElem->children =  new tList<TrackElement>();
  for (int i = 0; i < childCount; i++)
    {
      TrackElement* newElem = new TrackElement();
      this->currentTrackElem->children->add(newElem);
      newElem->ID = ++trackElemCount;
      newElem->startingTime = this->currentTrackElem->endTime + this->currentTrackElem->jumpTime;
      this->addPoint(this->currentTrackElem->curve->getNode(this->currentTrackElem->curve->getNodeCount()), this->currentTrackElem->getChild(i));
    }
  if (childCount == 1)
    this->currentTrackElem->getChild(0)->setName(this->currentTrackElem->getName());
}

/**
   \brief Searches for a given trackID.
   \param trackID the trackID to search for.
   \returns The TrackElement #trackID if found, NULL otherwise.
*/
TrackElement* TrackManager::findTrackElementByID(unsigned int trackID) const
{
  return firstTrackElem->findByID(trackID);
}

// INITIALIZE //

/**
   \brief Sets the trackID we are working on.
   \param trackID the trackID we are working on
*/
void TrackManager::workOn(unsigned int trackID)
{
  TrackElement* tmpElem = findTrackElementByID(trackID);
  if (tmpElem)
    this->currentTrackElem = tmpElem;
  else
    PRINTF(2)("TrackElement not Found, leaving unchanged\n");
  PRINTF(4)("now Working on %d\n", this->currentTrackElem->ID);

}

/**
   \brief Sets the Type of the Curve
   \param curveType The Type to set
   \param trackElem the TrackElement that should get a new Curve.
*/
void TrackManager::setCurveType(CurveType curveType, TrackElement* trackElem)
{
  if (!trackElem->isFresh)
    {
      PRINTF(2)("It is not possible to change the type of a Curve after you have have appended some points to it\n");
      return;
    }
  this->curveType = curveType;
  switch (curveType)
    {
    case BEZIERCURVE:
      trackElem->curve = new BezierCurve();
      break;

    }
}

/**
   \brief Sets the duration of the current path in seconds.
   \param time The duration in seconds.
*/

void TrackManager::setDuration(float time)
{
  this->currentTrackElem->duration = time;
  this->currentTrackElem->endTime = this->currentTrackElem->startingTime + time;
}

/**
   \brief adds a point to the current TrackElement
   \param newPoint The point to add.
*/
bool TrackManager::addPoint(Vector newPoint)
{
  return this->addPoint(newPoint, this->currentTrackElem);
}

/**
   \brief adds a point to trackElem
   \param newPoint The point to add.
   \param trackElem The TrackElement to add the Point to
*/
bool TrackManager::addPoint(Vector newPoint, TrackElement* trackElem)
{
  if (trackElem->isFresh)
    {
      this->setCurveType(TMAN_DEFAULT_CURVETYPE, trackElem);
      trackElem->isFresh = false;
    }
  trackElem->curve->addNode(newPoint);
  trackElem->nodeCount++;
}

/**
   \brief adds save/splitpoint.
   \param newPoint The point to add.
   \returns A Pointer to a newly appended Curve
*/
int TrackManager::addHotPoint(Vector newPoint)
{
  PRINTF(4)("setting up a HotPoint\n");
  if (this->currentTrackElem->isFresh)
    {
      this->setCurveType(BEZIERCURVE);
      this->currentTrackElem->isFresh = false;
    }

  // \todo HotPoint Handling.
  this->currentTrackElem->curve->addNode(newPoint);
  this->currentTrackElem->nodeCount++;
  this->initChildren(1);
  this->currentTrackElem = this->currentTrackElem->getChild(0);
}

/**
   \brief Sets the last HotPoint into a savePoint.
   \returns A Pointer to a newly appended Curve
   
   If no HotPoint was defined the last added Point will be rendered into a savePoint. \n
   If the HotPoint was defined as a fork the Point will \b not be set into a savePoint.
*/
int TrackManager::setSavePoint(void)
{
  PRINTF(4)("setting up a SavePoint.\n");
  if (this->currentTrackElem->isFork || this->currentTrackElem->isSavePoint)
    {
      PRINTF(2)("%d is already finished \n", currentTrackElem->ID);
      return this->currentTrackElem->getChild(0)->ID;
    }
  this->currentTrackElem->isSavePoint = true;
  this->currentTrackElem->isHotPoint = true;

  this->initChildren(1);
  this->currentTrackElem = this->currentTrackElem->getChild(0);
}

/**
   \brief adds some interessting non-linear movments through the level.
   \param count The Count of childrens the current HotPoint will have.

   If no HotPoint was defined the last added Point will be rendered into a fork. \n
   If the HotPoint was defined as a savePoint the Point will \b not be set into a fork.
*/
void TrackManager::fork(unsigned int count, ...)
{
  int* trackIDs = new int[count];
  this->forkV(count, trackIDs);
  va_list ID;
  va_start (ID, count);
  for(int i = 0; i < count; i++)
    {
      *va_arg (ID, int*) = trackIDs[i];
    }
  va_end(ID);  
  delete []trackIDs;
}

/**
   \brief adds some interessting non-linear movments through the level.
   \param count The Count of childrens the current HotPoint will have.
   \param trackIDs A Pointer to an Array of ints which will hold the trackID's (the user will have to reserve space for this).

   \see void TrackManager::fork(int count, ...)

   \todo initialisation is wrong!! also in setSavePoint.
*/
void TrackManager::forkV(unsigned int count, int* trackIDs)
{
  PRINTF(4)("Forking with %d children\n", count);
  if (this->currentTrackElem->isSavePoint)
    return;
  this->currentTrackElem->isFork = true;
  this->currentTrackElem->isHotPoint = true;
  for(int i = 0; i < count; i++)
    trackIDs[i]=this->trackElemCount+1+i;
  this->initChildren(count);
}

/**
   \brief decides under what condition a certain Path will be chosen.
   \param cond the CONDITION of the decision
   \param subject the Subject that will be decided upon with CONDITION cond.
*/
void TrackManager::condition(CONDITION cond, void* subject)
{
  this->condition(this->currentTrackElem->ID, cond, subject);
}
/**
   \brief decides under what condition a certain Path will be chosen.
   \param groupID the ID on which to choose the preceding move
   \param cond the CONDITION of the decision
   \param subject the Subject that will be decided upon with CONDITION cond.
*/
void TrackManager::condition(unsigned int groupID, CONDITION cond, void* subject)
{
  TrackElement* tmpElem = this->findTrackElementByID(groupID);
  if (!tmpElem->isFork)
    {
      PRINTF(2)("%d is not a Fork, and no condition can be set in this case\n", tmpElem->ID);
      return;
    }
  else
    {
      switch (cond)
        {
        case LOWEST:
          tmpElem->condFunc = &TrackElement::lowest;
          break;
        case HIGHEST:
          tmpElem->condFunc = &TrackElement::highest;
          break;
        case RANDOM: 
          tmpElem->condFunc = &TrackElement::random;
          break;
        case LEFTRIGHT:
          tmpElem->condFunc = &TrackElement::leftRight;
          break;
        case NEAREST:
          tmpElem->condFunc = &TrackElement::nearest;
          break;
        case ENEMYKILLED:
          break;
        }
      tmpElem->subject=subject;
    }
}


/**
   \brief joins some tracks together again. 
   \param count The count of Paths to join.

   Join will set the localTime to the longest time a Path has to get to this Point. \n
   Join will join all curves to the first curve, meaning that all the tangents will be matched.
*/
void TrackManager::join(unsigned int count, ...)
{
  int* trackIDs = new int [count];
  va_list ID;
  va_start (ID, count);
  for(int i = 0; i < count; i++)
    {
      trackIDs[i] = va_arg (ID, int);
    }
  va_end(ID);
  this->joinV(count, trackIDs);
  delete []trackIDs;
}

/**
   \brief joins some tracks together again. 
   \param count The count of Paths to join.
   \param trackIDs an Array with the trackID's to join

   \see void TrackManager::join(int count, ...)
*/
void TrackManager::joinV(unsigned int count, int* trackIDs)
{
  PRINTF(3)("Joining %d tracks and merging to Track %d\n", count, trackIDs[0]);

  // checking if there is a back-loop-connection and ERROR if it is.
  TrackElement* tmpTrackElem = this->findTrackElementByID(trackIDs[0]);
  if (!tmpTrackElem->backLoopCheck(tmpTrackElem))
    PRINTF(2)("Backloop connection detected at joining trackElements\n");

  // chanching work-on to temporary value. going back at the end.
  int tmpCurrentWorkingID = this->currentTrackElem->ID;
  this->workOn(trackIDs[0]);
  TrackElement* firstJoint = this->currentTrackElem;
  float tmpLatestTime = firstJoint->endTime;

  Vector tmpEndPoint = firstJoint->curve->getNode(firstJoint->curve->getNodeCount());
  Vector tmpTangentPoint = firstJoint->curve->getNode(firstJoint->curve->getNodeCount()-1);
  Vector tmpc2Point = firstJoint->curve->getNode(firstJoint->curve->getNodeCount()-2);
  firstJoint->isJoined = true;
  //  firstJoint->mainJoin = true;
  if(!firstJoint->isHotPoint)
    this->setSavePoint();
  // Timing:
  for (int i = 0; i < count; i++)
    {
      TrackElement* tmpJoinElem = this->findTrackElementByID(trackIDs[i]);
      if (tmpJoinElem->childCount == 0
          && tmpJoinElem->endTime > tmpLatestTime)
        tmpLatestTime = tmpJoinElem->endTime;
    }
  // time the main Join.
  firstJoint->jumpTime = tmpLatestTime - firstJoint->endTime;
  
  // Joining:
  for (int i = 1; i < count; i++)
    {
      TrackElement* tmpJoinElem = this->findTrackElementByID(trackIDs[i]);
      if (tmpJoinElem->childCount > 0)
        printf("!!This Curve has children, and as such will not be joined!!\n You can try joining other childless TrackElements to this one!");
      else
        {
          this->addPoint(tmpc2Point, tmpJoinElem);
          this->addPoint(tmpTangentPoint, tmpJoinElem);
          this->addPoint(tmpEndPoint, tmpJoinElem);
          // time all other Joins
          tmpJoinElem->jumpTime = tmpLatestTime - tmpJoinElem->endTime;
          
          //Copying Joint-Info
          tmpJoinElem->children = firstJoint->children;
          tmpJoinElem->childCount = firstJoint->childCount;
          tmpJoinElem->isSavePoint = firstJoint->isSavePoint;
          tmpJoinElem->isFork = firstJoint->isFork;

          tmpJoinElem->isJoined = true;
        }
    }
  if(firstJoint->childCount > 0)
    {
      TrackElement* enumElem = firstJoint->children->enumerate();
      while (enumElem)
        {
          PRINTF(4)("Setting startingTime of %d to %f.\n", enumElem->ID, tmpLatestTime);
          enumElem->startingTime = tmpLatestTime;
          enumElem->endTime = tmpLatestTime + enumElem->duration;
          
          enumElem = firstJoint->children->nextElement();
        }
    }
  // returning to the TrackElement we were working on.
  this->workOn(tmpCurrentWorkingID);
}

/**
   \brief finalizes the TrackSystem. after this it will not be editable anymore

   \todo check for any inconsistencies, output errors
*/
void TrackManager::finalize(void)
{
  for (int i = 1; i<= trackElemCount ;i++)
    {
      TrackElement* tmpElem = findTrackElementByID(i);
      if (tmpElem->childCount > 0 && tmpElem->mainJoin)
        {

          TrackElement* enumElem = tmpElem->children->enumerate();
          while (enumElem)
            {
              
              // c1-continuity
              enumElem->curve->addNode(enumElem->curve->getNode(0) +
                                                   ((enumElem->curve->getNode(0) - 
                                                    tmpElem->curve->getNode(tmpElem->curve->getNodeCount()-1))
                                                    ),2);
              enumElem->nodeCount++;
              // c2-continuity
              enumElem->curve->addNode((tmpElem->curve->getNode(tmpElem->curve->getNodeCount())-
                                                    tmpElem->curve->getNode(tmpElem->curve->getNodeCount()-1)) * 4 +
                                                   tmpElem->curve->getNode(tmpElem->curve->getNodeCount()-2), 3);
              enumElem->nodeCount++;                                                
              PRINTF(5)("accelerations: %d-in: count: %d, %f, %f, %f\n                  %d-out: count: %d %f, %f, %f\n",
                     tmpElem->ID, tmpElem->nodeCount,
                     tmpElem->curve->calcAcc(0.999).x, tmpElem->curve->calcAcc(0.999).y, tmpElem->curve->calcAcc(0.999).z,
                     enumElem->ID, enumElem->nodeCount,
                     enumElem->curve->calcAcc(0).x, enumElem->curve->calcAcc(0).y, enumElem->curve->calcAcc(0).z);
              
              enumElem = tmpElem->children->nextElement();
            }
        }
    }
  for (int i = 1; i <=trackElemCount;i++)
    if (this->findTrackElementByID(i)->endTime > this->maxTime)
      this->maxTime = findTrackElementByID(i)->endTime; // very bad implemented :/
}


// RUNTIME //

/**
   \brief calculates the Position for the localTime of the Track.
   \returns the calculated Position
*/
Vector TrackManager::calcPos() const
{
  return this->currentTrackElem->curve->calcPos((this->localTime-this->currentTrackElem->startingTime)/this->currentTrackElem->duration);
}

/**
   \brief calculates the Rotation for the localTime of the Track.
   \returns the calculated Rotation
*/
Vector TrackManager::calcDir() const
{
  return this->currentTrackElem->curve->calcDir((this->localTime - this->currentTrackElem->startingTime)/this->currentTrackElem->duration);
}

/**
   \returns the current Width of the track
*/
float TrackManager::getWidth(void) const
{
  return this->currentTrackElem->width;
}

/**
   \brief Advances the local-time of the Track around dt
   \param dt The time about which to advance.

   This function also checks, if the TrackElement has to be changed.
*/
void TrackManager::tick(float dt)
{
  dt /= 1000;
  PRINTF(4)("CurrentTrackID: %d, LocalTime is: %f, timestep is: %f\n", this->currentTrackElem->ID, this->localTime, dt);
  if (this->localTime <= this->firstTrackElem->duration)
    this->jumpTo(this->localTime);
  if (this->localTime <= this->maxTime)
    this->localTime += dt;
  if (this->localTime > this->currentTrackElem->endTime
      && this->currentTrackElem->children)
    {
      if (this->currentTrackElem->jumpTime != 0.0)
        this->jumpTo(this->localTime + this->currentTrackElem->jumpTime);
      // jump to the next TrackElement and also set the history of the new Element to the old one.
      TrackElement* tmpHistoryElem = this->currentTrackElem;
      this->currentTrackElem = this->currentTrackElem->getChild(this->choosePath(this->currentTrackElem));
      this->currentTrackElem->history = tmpHistoryElem;
    }
  if (this->bindSlave)
    {
      Vector tmp = this->calcPos();
      Quaternion quat = Quaternion(this->calcDir(), Vector(this->currentTrackElem->curve->calcAcc((localTime-this->currentTrackElem->startingTime)/this->currentTrackElem->duration).x,1,this->currentTrackElem->curve->calcAcc((localTime-this->currentTrackElem->startingTime)/this->currentTrackElem->duration).z)); 

      Vector v(0.0, 1.0, 0.0);
      Quaternion q(-PI/2, v);
      quat = quat * q;

      this->bindSlave->setAbsCoor(&tmp);
      this->bindSlave->setAbsDir(&quat);
    }
}

/**
   \brief Jumps to a certain point on the Track.
   \param time The time on the Track to jump to.

   This should be used to Jump backwards on a Track, because moving forward means to change between the Path. (it then tries to choose the default.)
   Max is trackLengthMax.
*/
void TrackManager::jumpTo(float time)
{
  if (time == 0)
    this->currentTrackElem = this->firstTrackElem;
  this->localTime = time;
}

/**
   \brief a Function that decides which Path we should follow.
   \param trackElem The Path to choose.
   
*/
int TrackManager::choosePath(TrackElement* trackElem)
{
  return (trackElem->*(trackElem->condFunc))(trackElem->subject);
}

/**
   \brief Sets the PNode, that should be moved along the Tack
   \param bindSlave the PNode to set
*/
void TrackManager::setBindSlave(PNode* bindSlave)
{
  this->bindSlave = bindSlave;
}

/**
   \returns the main TrackNode
*/
PNode* TrackManager::getTrackNode(void)
{
  return this->trackNode;
}

// DEBUG //

/**
   \brief Imports a model of the Graph into the OpenGL-environment.
   \param dt The Iterator used in seconds for Painting the Graph.

   This is for testing facility only. Do this if you want to see the Path inside the Level.
   eventually this will all be packed into a gl-list.
*/
void TrackManager::drawGraph(float dt) const
{

  for (int i = 1; i <= trackElemCount; i++)
    {
      glBegin(GL_LINE_STRIP);
      TrackElement* tmpElem = this->findTrackElementByID(i);
      if (tmpElem->curve)
        for(float f = 0.0; f < 1.0; f+=dt)
          {
            //      printf("%f, %f, %f\n",trackManager->calcPos().x, trackManager->calcPos().y, trackManager->calcPos().z);
            Vector tmpVector = tmpElem->curve->calcPos(f);
            glVertex3f(tmpVector.x, tmpVector.y, tmpVector.z);
          }
  glEnd();
    }
}

/**
   \brief outputs debug information about the trackManager
   \param level how much debug
*/
void TrackManager::debug(unsigned int level) const
{
  PRINT(0)("=========================================\n");
  PRINT(0)("= CLASS TRACKMANAGER::debug information =\n");
  PRINT(0)("=========================================\n");
  //  PRINT(0)("Status is: %
  PRINT(0)(" Consists of %d elements\n", this->trackElemCount);
  PRINT(0)(" localTime is: %f\n", this->localTime);
  if (level >= 2)
    {
      for (int i = 1; i <= trackElemCount; i++)
        {
          TrackElement* tmpElem = this->findTrackElementByID(i);
          tmpElem->debug();
        }
    }
  PRINT(0)("-----------------------------------------\n");
}