Index: orxonox/trunk/src/track_manager.cc
===================================================================
--- orxonox/trunk/src/track_manager.cc	(revision 3365)
+++ orxonox/trunk/src/track_manager.cc	(revision 3433)
@@ -1,4 +1,2 @@
-
-
 /* 
    orxonox - the future of 3D-vertical-scrollers
@@ -12,5 +10,5 @@
 
    ### File Specific:
-   main-programmer: Patrick Boenzli
+   main-programmer: Benjamin Grauer
    co-programmer: ...
 */
@@ -19,5 +17,5 @@
 #include "track_manager.h"
 #include <stdarg.h>
-
+#include "p_node.h"
 
 using namespace std;
@@ -38,10 +36,10 @@
   this->startingTime = 0; //!< \todo eventually set this to the max time of TrackManager.
   this->duration = 1;
+  this->endTime = 1;
+  this->jumpTime = 0;
   this->curveType = BEZIERCURVE;
   this->nodeCount = 0;
   this->childCount = 0;
   this->name = NULL;
-  this->startPoint = Vector(0,0,0);
-  this->startTangentPoint = Vector(0,0,0);
   this->curve = NULL;
   this->children = NULL;
@@ -111,4 +109,5 @@
   this->maxTime = 0;
   this->trackElemCount = 1;
+  this->bindSlave = NULL;
 }
 
@@ -151,4 +150,5 @@
 {
   this->currentTrackElem->childCount = childCount;
+  this->currentTrackElem->mainJoin = true;
   this->currentTrackElem->children = new TrackElement*[childCount];
   for (int i=0; i<childCount; i++)
@@ -156,7 +156,6 @@
       this->currentTrackElem->children[i] = new TrackElement();
       this->currentTrackElem->children[i]->ID = ++trackElemCount;
-      this->currentTrackElem->children[i]->startingTime = this->currentTrackElem->startingTime+this->currentTrackElem->duration;
-      this->currentTrackElem->children[i]->startPoint = this->currentTrackElem->curve->getNode(this->currentTrackElem->curve->getNodeCount());
-      this->currentTrackElem->children[i]->startTangentPoint = (this->currentTrackElem->children[i]->startPoint *2) - this->currentTrackElem->curve->getNode(this->currentTrackElem->curve->getNodeCount()-1);
+      this->currentTrackElem->children[i]->startingTime = this->currentTrackElem->endTime + this->currentTrackElem->jumpTime;
+      this->addPoint(this->currentTrackElem->curve->getNode(this->currentTrackElem->curve->getNodeCount()), this->currentTrackElem->children[i]);
     }
 }
@@ -193,19 +192,19 @@
    \brief curveType The Type to set
 */
-void TrackManager::setCurveType(CurveType curveType)
-{
-  if (!this->currentTrackElem->isFresh)
+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->currentTrackElem->curveType = curveType;
+  trackElem->curveType = curveType;
   switch (curveType)
     {
     case BEZIERCURVE:
-      this->currentTrackElem->curve = new BezierCurve();
+      trackElem->curve = new BezierCurve();
       break;
     case UPOINTCURVE:
-      this->currentTrackElem->curve = new UPointCurve();
+      trackElem->curve = new UPointCurve();
       break;
     }
@@ -220,4 +219,5 @@
 {
   this->currentTrackElem->duration = time;
+  this->currentTrackElem->endTime = this->currentTrackElem->startingTime + time;
 }
 
@@ -240,11 +240,6 @@
   if (trackElem->isFresh)
     {
-      this->setCurveType(BEZIERCURVE);
+      this->setCurveType(BEZIERCURVE, trackElem);
       trackElem->isFresh = false;
-      if(trackElem != this->firstTrackElem)
-	{
-	  this->addPoint(trackElem->startPoint);
-	  this->addPoint(trackElem->startTangentPoint);
-	}
     }
   trackElem->curve->addNode(newPoint);
@@ -374,18 +369,31 @@
 void TrackManager::joinV(unsigned int count, int* trackIDs)
 {
-  printf("Joining %d tracks to Track %d\n", count, trackIDs[0]);
+  printf("Joining %d tracks and merging to Track %d\n", count, trackIDs[0]);
 
   // 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 tmpTangentPoint = firstJoint->curve->getNode(firstJoint->curve->getNodeCount()-1);
+  Vector tmpc2Point = firstJoint->curve->getNode(firstJoint->curve->getNodeCount()-2);
   firstJoint->isJoined = true;
-  firstJoint->mainJoin = 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++)
     {
@@ -395,6 +403,9 @@
       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
@@ -403,12 +414,57 @@
 	  tmpJoinElem->isSavePoint = firstJoint->isSavePoint;
 	  tmpJoinElem->isFork = firstJoint->isFork;
-	  
+
 	  tmpJoinElem->isJoined = true;
 	}
     }
+  if(firstJoint->childCount > 0)
+    for(int i = 0; i < firstJoint->childCount; i++)
+      {
+	printf("Setting startingTime of %d to %f.\n", firstJoint->children[i]->ID, tmpLatestTime);
+	firstJoint->children[i]->startingTime = tmpLatestTime;
+	firstJoint->children[i]->endTime = tmpLatestTime+firstJoint->children[i]->duration;
+      }	
+
   // 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)
+	{
+	  for (int j = 0; j < tmpElem->childCount; j++)
+	    {
+	      
+	      // c1-continuity
+	      tmpElem->children[j]->curve->addNode(tmpElem->children[j]->curve->getNode(0) +
+						   ((tmpElem->children[j]->curve->getNode(0) - 
+						    tmpElem->curve->getNode(tmpElem->curve->getNodeCount()-1))
+						    ),2);
+	      tmpElem->children[j]->nodeCount++;
+	      // c2-continuity
+	      tmpElem->children[j]->curve->addNode((tmpElem->curve->getNode(tmpElem->curve->getNodeCount())-
+						    tmpElem->curve->getNode(tmpElem->curve->getNodeCount()-1)) * 4 +
+						   tmpElem->curve->getNode(tmpElem->curve->getNodeCount()-2), 3);
+	      tmpElem->children[j]->nodeCount++;						    
+	      printf("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,
+		     tmpElem->children[j]->ID, tmpElem->children[j]->nodeCount,
+		     tmpElem->children[j]->curve->calcAcc(0).x, tmpElem->children[j]->curve->calcAcc(0).y, tmpElem->children[j]->curve->calcAcc(0).z);
+	    }
+	}
+    }
+}
+
+
 // RUNTIME //
 
@@ -429,5 +485,5 @@
 Vector TrackManager::calcDir() const
 {
-  return this->currentTrackElem->curve->calcDir((this->localTime-this->currentTrackElem->startingTime)/this->currentTrackElem->duration);
+  return this->currentTrackElem->curve->calcDir((this->localTime - this->currentTrackElem->startingTime)/this->currentTrackElem->duration);
 }
 
@@ -440,9 +496,23 @@
 void TrackManager::tick(float dt)
 {
+  dt /= 1000;
+  printf("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);
   this->localTime += dt;
-  if (this->localTime > this->currentTrackElem->startingTime + this->currentTrackElem->duration && this->currentTrackElem->childCount > 0)
-    this->currentTrackElem = this->currentTrackElem->children[0];
+  if (this->localTime > this->currentTrackElem->endTime
+      && this->currentTrackElem->children)
+    {
+      if (this->currentTrackElem->jumpTime > 0) 
+	this->jumpTo(this->localTime + this->currentTrackElem->jumpTime);
+      this->currentTrackElem = this->currentTrackElem->children[0];
+    }
+  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)); 
+      this->bindSlave->setAbsCoor(&tmp);
+      this->bindSlave->setAbsDir(&quat);
+    }
 }
 
@@ -471,4 +541,13 @@
 }
 
+/**
+   \brief Sets the PNode, that should be moved along the Tack
+   \param bindSlave the PNode to set
+*/
+void TrackManager::setBindSlave(PNode* bindSlave)
+{
+  if (!this->bindSlave)
+    this->bindSlave = bindSlave;
+}
 
 
@@ -500,4 +579,8 @@
 }
 
+/**
+   \brief outputs debug information about the trackManager
+   \param level how much debug
+*/
 void TrackManager::debug(unsigned int level) const
 {
@@ -516,5 +599,5 @@
 	  if(tmpElem->isFresh)
 	    printf("   has not jet eddited in any way\n");
-	  printf("\n   TimeTable: starting at = %f; ends at = %f; duration = %f\n", tmpElem->startingTime, tmpElem->startingTime+tmpElem->duration, tmpElem->duration);
+	  printf("\n   TimeTable: startingTime=%f; endTime=%f; duration=%f; jumpTime=%f\n", tmpElem->startingTime, tmpElem->endTime, tmpElem->duration, tmpElem->jumpTime);
 	  printf("   consists of %d Points\n", tmpElem->nodeCount);
 	  if (tmpElem->childCount == 0)
@@ -536,5 +619,5 @@
 	  if(tmpElem->isFork)
 	    {
-	      printf("    is A Fork with with %d children.", tmpElem->childCount);
+	      printf("    is A Fork with with %d children.\n", tmpElem->childCount);
 	    }
 	  if(tmpElem->isJoined)
