source: orxonox.OLD/trunk/src/lib/coord/p_node.cc @ 6428

/*
   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: Patrick Boenzli
   co-programmer: Benjamin Grauer
*/

#define DEBUG_SPECIAL_MODULE DEBUG_MODULE_PNODE

#include "p_node.h"

#include "load_param.h"
#include "class_list.h"

#include <algorithm>
#include "compiler.h"
#include "debug.h"

#include "glincl.h"
#include "color.h"

#include "synchronizeable.h"

#include "shell_command.h"
SHELL_COMMAND(debugNode, PNode, debugNodeSC);

using namespace std;


/**
 * @brief standard constructor
 * @param parent the Parent of this Node. __NULL__ if __No Parent__ requested, PNode::getNullParent(), if connected to NullParent directly (default)
 * @param nodeFlags all flags to set. THIS_WILL_OVERWRITE Default_Values.
 */
PNode::PNode (PNode* parent, long nodeFlags)
{
  this->setClassID(CL_PARENT_NODE, "PNode");

  this->bRelCoorChanged = true;
  this->bRelDirChanged = true;
  this->parent = NULL;
  this->parentMode = nodeFlags;
  this->bActive = true;

  // smooth-movers
  this->toCoordinate = NULL;
  this->toDirection = NULL;
  this->bias = 1.0;

  if (parent != NULL)
    parent->addChild(this);
}

// NullParent Reference
PNode* PNode::nullParent = NULL;

/**
 * @brief standard deconstructor
 *
 * There are two general ways to delete a PNode
 * 1. delete instance;
 *   -> result
 *    delete this Node and all its children and children's children...
 *    (danger if you still need the children's instance somewhere else!!)
 *
 * 2. instance->removeNode(); delete instance;
 *   -> result:
 *    moves its children to the NullParent
 *    then deletes the Element.
 */
PNode::~PNode ()
{
  // remove the Node, delete it's children (if required).
  std::list<PNode*>::iterator tmp = this->children.begin();
  std::list<PNode*>::iterator deleteNode;
  while(!this->children.empty())
    while (tmp != this->children.end())
    {
      deleteNode = tmp;
      tmp++;
//      printf("TEST::%s(%s) %s\n", (*deleteNode)->getName(), (*deleteNode)->getClassName(), this->getName());
      if ((this->parentMode & PNODE_PROHIBIT_CHILD_DELETE) ||
          ((*deleteNode)->parentMode & PNODE_PROHIBIT_DELETE_WITH_PARENT))
      {
        if (this == PNode::nullParent && (*deleteNode)->parentMode & PNODE_REPARENT_TO_NULL)
          delete (*deleteNode);
        else
          (*deleteNode)->reparent();
      }
      else
        delete (*deleteNode);
    }

  if (this->parent != NULL)
   {
     this->parent->eraseChild(this);
     this->parent = NULL;
   }

  // remove all other allocated memory.
  if (this->toCoordinate != NULL)
    delete this->toCoordinate;
  if (this->toDirection != NULL)
    delete this->toDirection;

  if (this == PNode::nullParent)
    PNode::nullParent = NULL;
}


/**
 * @brief loads parameters of the PNode
 * @param root the XML-element to load the properties of
 */
void PNode::loadParams(const TiXmlElement* root)
{
  static_cast<BaseObject*>(this)->loadParams(root);

  LoadParam(root, "rel-coor", this, PNode, setRelCoor)
      .describe("Sets The relative position of the Node to its parent.");

  LoadParam(root, "abs-coor", this, PNode, setAbsCoor)
      .describe("Sets The absolute Position of the Node.");

  LoadParam(root, "rel-dir", this, PNode, setRelDir)
      .describe("Sets The relative rotation of the Node to its parent.");

  LoadParam(root, "abs-dir", this, PNode, setAbsDir)
      .describe("Sets The absolute rotation of the Node.");

  LoadParam(root, "parent", this, PNode, setParent)
      .describe("the Name of the Parent of this PNode");

  LoadParam(root, "parent-mode", this, PNode, setParentMode)
      .describe("the mode to connect this node to its parent ()");

  // cycling properties
  if (root != NULL)
  {
    LOAD_PARAM_START_CYCLE(root, element);
    {
      LoadParam_CYCLE(element, "child", this, PNode, addChild)
          .describe("adds a new Child to the current Node.");

    }
    LOAD_PARAM_END_CYCLE(element);
  }
}


/**
 *  init the pnode to a well definied state
 *
 * this function actualy only updates the PNode tree
 */
void PNode::init()
{
  /* just update all aboslute positions via timestep 0.001ms */
  this->updateNode(0.001f);
  this->updateNode(0.001f);
}


/**
 * @brief set relative coordinates
 * @param relCoord relative coordinates to its parent
 *
 *
 * it is very importand, that you use this function, if you want to update the
 * relCoordinates. If you don't use this, the PNode won't recognize, that something
 * has changed and won't update the children Nodes.
 */
void PNode::setRelCoor (const Vector& relCoord)
{
  if (this->toCoordinate!= NULL)
  {
    delete this->toCoordinate;
    this->toCoordinate = NULL;
  }

  this->relCoordinate = relCoord;
  this->bRelCoorChanged = true;
}

/**
 * @brief set relative coordinates
 * @param x x-relative coordinates to its parent
 * @param y y-relative coordinates to its parent
 * @param z z-relative coordinates to its parent
 * @see  void PNode::setRelCoor (const Vector& relCoord)
 */
void PNode::setRelCoor (float x, float y, float z)
{
  this->setRelCoor(Vector(x, y, z));
}

/**
 * @brief sets a new relative position smoothely
 * @param relCoordSoft the new Position to iterate to
 * @param bias how fast to iterate to this position
 */
void PNode::setRelCoorSoft(const Vector& relCoordSoft, float bias)
{
  if (likely(this->toCoordinate == NULL))
    this->toCoordinate = new Vector();

  *this->toCoordinate = relCoordSoft;
  this->bias = bias;
}


/**
 * @brief set relative coordinates smoothely
 * @param x x-relative coordinates to its parent
 * @param y y-relative coordinates to its parent
 * @param z z-relative coordinates to its parent
 * @see  void PNode::setRelCoorSoft (const Vector&, float)
 */
void PNode::setRelCoorSoft (float x, float y, float z, float bias)
{
  this->setRelCoorSoft(Vector(x, y, z), bias);
}


/**
 * @param absCoord set absolute coordinate
 */
void PNode::setAbsCoor (const Vector& absCoord)
{
  if (this->toCoordinate!= NULL)
  {
    delete this->toCoordinate;
    this->toCoordinate = NULL;
  }

  if( likely(this->parentMode & PNODE_MOVEMENT))
  {
      /* if you have set the absolute coordinates this overrides all other changes */
    if (likely(this->parent != NULL))
      this->relCoordinate = absCoord - parent->getAbsCoor ();
    else
      this->relCoordinate = absCoord;
  }
  if( this->parentMode & PNODE_ROTATE_MOVEMENT)
  {
    if (likely(this->parent != NULL))
      this->relCoordinate = absCoord - parent->getAbsCoor ();
    else
      this->relCoordinate = absCoord;
  }

  this->bRelCoorChanged = true;
//  this->absCoordinate = absCoord;
}


/**
 * @param x x-coordinate.
 * @param y y-coordinate.
 * @param z z-coordinate.
 * @see void PNode::setAbsCoor (const Vector& absCoord)
 */
void PNode::setAbsCoor(float x, float y, float z)
{
  this->setAbsCoor(Vector(x, y, z));
}


/**
 * @param absCoord set absolute coordinate
 * @todo check off
 */
void PNode::setAbsCoorSoft (const Vector& absCoordSoft, float bias)
{
  if (this->toCoordinate == NULL)
    this->toCoordinate = new Vector;

  if( likely(this->parentMode & PNODE_MOVEMENT))
  {
      /* if you have set the absolute coordinates this overrides all other changes */
    if (likely(this->parent != NULL))
      *this->toCoordinate = absCoordSoft - parent->getAbsCoor ();
    else
      *this->toCoordinate = absCoordSoft;
  }
  if( this->parentMode & PNODE_ROTATE_MOVEMENT)
  {
    if (likely(this->parent != NULL))
      *this->toCoordinate = absCoordSoft - parent->getAbsCoor ();
    else
      *this->toCoordinate = absCoordSoft;
  }
}


/**
 * @brief shift coordinate relative
 * @param shift shift vector
 *
 * this function shifts the current coordinates about the vector shift. this is
 * usefull because from some place else you can:
 * PNode* someNode = ...;
 * Vector objectMovement = calculateShift();
 * someNode->shiftCoor(objectMovement);
 *
 * this is the internal method of:
 * PNode* someNode = ...;
 * Vector objectMovement = calculateShift();
 * Vector currentCoor = someNode->getRelCoor();
 * Vector newCoor = currentCoor + objectMovement;
 * someNode->setRelCoor(newCoor);
 *
 */
void PNode::shiftCoor (const Vector& shift)
{
  this->relCoordinate += shift;
  this->bRelCoorChanged = true;
}


/**
 * @brief set relative direction
 * @param relDir to its parent
 */
void PNode::setRelDir (const Quaternion& relDir)
{
  if (this->toDirection!= NULL)
  {
    delete this->toDirection;
    this->toDirection = NULL;
  }
  this->relDirection = relDir;

  this->bRelCoorChanged = true;
}


/**
 * @see void PNode::setRelDir (const Quaternion& relDir)
 * @param x the x direction
 * @param y the y direction
 * @param z the z direction
 *
 * main difference is, that here you give a directional vector, that will be translated into a Quaternion
 */
void PNode::setRelDir (float x, float y, float z)
{
  this->setRelDir(Quaternion(Vector(x,y,z), Vector(0,1,0)));
}


/**
 * @brief sets the Relative Direction of this node to its parent in a Smoothed way
 * @param relDirSoft the direction to iterate to smoothely.
 * @param bias how fast to iterate to the new Direction
 */
void PNode::setRelDirSoft(const Quaternion& relDirSoft, float bias)
{
  if (likely(this->toDirection == NULL))
    this->toDirection = new Quaternion();

  *this->toDirection = relDirSoft;
  this->bias = bias;
  this->bRelDirChanged = true;
}


/**
 * @see void PNode::setRelDirSoft (const Quaternion& relDir)
 * @param x the x direction
 * @param y the y direction
 * @param z the z direction
 *
 * main difference is, that here you give a directional vector, that will be translated into a Quaternion
 */
void PNode::setRelDirSoft(float x, float y, float z, float bias)
{
  this->setRelDirSoft(Quaternion(Vector(x,y,z), Vector(0,1,0)), bias);
}


/**
 * @brief sets the absolute direction
 * @param absDir absolute coordinates
 */
void PNode::setAbsDir (const Quaternion& absDir)
{
  if (this->toDirection!= NULL)
  {
    delete this->toDirection;
    this->toDirection = NULL;
  }

  if (likely(this->parent != NULL))
    this->relDirection = absDir / this->parent->getAbsDir();
  else
   this->relDirection = absDir;

  this->bRelDirChanged = true;
}


/**
 * @see void PNode::setAbsDir (const Quaternion& relDir)
 * @param x the x direction
 * @param y the y direction
 * @param z the z direction
 *
 * main difference is, that here you give a directional vector, that will be translated into a Quaternion
 */
void PNode::setAbsDir (float x, float y, float z)
{
  this->setAbsDir(Quaternion(Vector(x,y,z), Vector(0,1,0)));
}


/**
 * @brief sets the absolute direction
 * @param absDir absolute coordinates
 * @param bias how fast to iterator to the new Position
 */
void PNode::setAbsDirSoft (const Quaternion& absDirSoft, float bias)
{
  if (this->toDirection == NULL)
    this->toDirection = new Quaternion();

  if (likely(this->parent != NULL))
    *this->toDirection = absDirSoft / this->parent->getAbsDir();
  else
   *this->toDirection = absDirSoft;

  this->bias = bias;
  this->bRelDirChanged = true;
}


/**
 * @see void PNode::setAbsDir (const Quaternion& relDir)
 * @param x the x direction
 * @param y the y direction
 * @param z the z direction
 *
 * main difference is, that here you give a directional vector, that will be translated into a Quaternion
 */
void PNode::setAbsDirSoft (float x, float y, float z, float bias)
{
  this->setAbsDirSoft(Quaternion(Vector(x,y,z), Vector(0,1,0)), bias);
}


/**
 * @brief shift Direction
 * @param shift the direction around which to shift.
 */
void PNode::shiftDir (const Quaternion& shift)
{
  this->relDirection = this->relDirection * shift;
  this->bRelDirChanged = true;
}


/**
 * @brief adds a child and makes this node to a parent
 * @param child child reference
 * use this to add a child to this node.
 */
void PNode::addChild (PNode* child)
{
  if( likely(child->parent != NULL))
      child->parent->eraseChild(child);
  if (this->checkIntegrity(child))
  {
    child->parent = this;
    if (unlikely(this != NULL))
      this->children.push_back(child);
    child->parentCoorChanged();
  }
  else
  {
    PRINTF(1)("Tried to reparent to own child '%s::%s' to '%s::%s'.\n",
              this->getClassName(), this->getName(), child->getClassName(), child->getName());
    child->parent = NULL;
    child->parentCoorChanged();
  }
}


/**
 * @see PNode::addChild(PNode* child);
 * @param childName the name of the child to add to this PNode
 */
void PNode::addChild (const char* childName)
{
  PNode* childNode = dynamic_cast<PNode*>(ClassList::getObject(childName, CL_PARENT_NODE));
  if (childNode != NULL)
    this->addChild(childNode);
}


/**
 * @brief removes a child from the node
 * @param child the child to remove from this pNode.
 *
 * Children from pNode will not be lost, they are Reparented by the rules of the ParentMode
 */
void PNode::removeChild (PNode* child)
{
  if (child != NULL)
   child->removeNode();
}


/**
 * !! PRIVATE FUNCTION
 * @brief reparents a node (happens on Parents Node delete or remove and Flags are set.)
 */
void PNode::reparent()
{
  if (this->parentMode & PNODE_REPARENT_TO_NULL)
    this->setParent((PNode*)NULL);
  else if (this->parentMode & PNODE_REPARENT_TO_PARENTS_PARENT && this->parent != NULL)
    this->setParent(this->parent->getParent());
  else
    this->setParent(PNode::getNullParent());
}

/**
 * ereases child from the nodes children
 * @param chuld the child to remove
 */
void PNode::eraseChild(PNode* child)
{
  std::list<PNode*>::iterator childRemover = std::find(this->children.begin(), this->children.end(), child);
  if(childRemover != this->children.end())
    this->children.erase(childRemover);
}


/**
 * @brief remove this pnode from the tree and adds all following to NullParent
 *
 * this can be the case, if an entity in the world is being destroyed.
 */
void PNode::removeNode()
{
  list<PNode*>::iterator child = this->children.begin();
  list<PNode*>::iterator reparenter;
  while (child != this->children.end())
  {
    reparenter = child;
    child++;
    if (this->parentMode & PNODE_REPARENT_CHILDREN_ON_REMOVE ||
        (*reparenter)->parentMode & PNODE_REPARENT_ON_PARENTS_REMOVE)
    {
      printf("TEST----------------%s ---- %s\n", this->getClassName(), (*reparenter)->getClassName());
      (*reparenter)->reparent();
      printf("REPARENTED TO: %s::%s\n",(*reparenter)->getParent()->getClassName(),(*reparenter)->getParent()->getName());
    }
  }
  if (this->parent != NULL)
  {
    this->parent->eraseChild(this);
    this->parent = NULL;
  }
}


/**
 * @see PNode::setParent(PNode* parent);
 * @param parentName the name of the Parent to set to this PNode
 */
void PNode::setParent (const char* parentName)
{
  PNode* parentNode = dynamic_cast<PNode*>(ClassList::getObject(parentName, CL_PARENT_NODE));
  if (parentNode != NULL)
    parentNode->addChild(this);
  else
    PRINTF(2)("Not Found PNode's (%s::%s) new Parent by Name: %s\n",
              this->getClassName(), this->getName(), parentName);
}


/**
 * @brief does the reparenting in a very smooth way
 * @param parentNode the new Node to connect this node to.
 * @param bias the speed to iterate to this new Positions
 */
void PNode::setParentSoft(PNode* parentNode, float bias)
{
  // return if the new parent and the old one match
  if (this->parent == parentNode )
    return;
  if (parentNode == NULL)
    parentNode = PNode::getNullParent();

  // store the Valures to iterate to.
  if (likely(this->toCoordinate == NULL))
  {
    this->toCoordinate = new Vector();
    *this->toCoordinate = this->getRelCoor();
  }
  if (likely(this->toDirection == NULL))
  {
    this->toDirection = new Quaternion();
    *this->toDirection = this->getRelDir();
  }
  this->bias = bias;


  Vector tmpV = this->getAbsCoor();
  Quaternion tmpQ = this->getAbsDir();

  parentNode->addChild(this);

 if (this->parentMode & PNODE_ROTATE_MOVEMENT && this->parent != NULL)
   this->relCoordinate = this->parent->getAbsDir().inverse().apply(tmpV - this->parent->getAbsCoor());
 else
   this->relCoordinate = tmpV - parentNode->getAbsCoor();

 this->relDirection = tmpQ / parentNode->getAbsDir();
}


/**
 * @brief does the reparenting in a very smooth way
 * @param parentName the name of the Parent to reconnect to
 * @param bias the speed to iterate to this new Positions
 */
void PNode::setParentSoft(const char* parentName, float bias)
{
  PNode* parentNode = dynamic_cast<PNode*>(ClassList::getObject(parentName, CL_PARENT_NODE));
  if (parentNode != NULL)
    this->setParentSoft(parentNode, bias);
}

/**
 * @param parentMode sets the parentingMode of this Node
 */
void PNode::setParentMode(PARENT_MODE parentMode)
{
  this->parentMode = ((this->parentMode & 0xfff0) | parentMode);
}

/**
 * @brief sets the mode of this parent manually
 * @param parentMode a String representing this parentingMode
 */
void PNode::setParentMode (const char* parentingMode)
{
  this->setParentMode(PNode::charToParentingMode(parentingMode));
}

/**
 * @brief adds special mode Flags to this PNode
 * @see PARENT_MODE
 * @param nodeFlags a compsition of PARENT_MODE-flags, split by the '|' (or) operator.
 */
void PNode::addNodeFlags(unsigned short nodeFlags)
{
  this->parentMode |= nodeFlags;
}

/**
 * @brief removes special mode Flags to this PNode
 * @see PARENT_MODE
 * @param nodeFlags a compsition of PARENT_MODE-flags, split by the '|' (or) operator.
 */
void PNode::removeNodeFlags(unsigned short nodeFlags)
{
  this->parentMode &= !nodeFlags;
}

/**
 * @returns the NullParent (and if needed creates it)
 */
PNode* PNode::createNullParent()
{
  if (likely(PNode::nullParent == NULL))
  {
    PNode::nullParent = new PNode(NULL, PNODE_PARENT_MODE_DEFAULT | PNODE_REPARENT_TO_NULL);
    PNode::nullParent->setName("NullParent");
  }
  return PNode::nullParent;
}


/**
 * !! PRIVATE FUNCTION
 * @brief checks the upward integrity (e.g if PNode is somewhere up the Node tree.)
 * @param checkParent the Parent to check.
 * @returns true if the integrity-check succeeds, false otherwise.
 *
 * If there is a second occurence of checkParent before NULL, then a loop could get
 * into the Tree, and we do not want this.
 */
bool PNode::checkIntegrity(const PNode* checkParent) const
{
  const PNode* parent = this;
  while ( (parent = parent->getParent()) != NULL)
    if (unlikely(parent == checkParent))
      return false;
  return true;
}


/**
 * @brief updates the absCoordinate/absDirection
 * @param dt The time passed since the last update
 *
 * this is used to go through the parent-tree to update all the absolute coordinates
 * and directions. this update should be done by the engine, so you don't have to
 * worry, normaly...
 */
void PNode::updateNode (float dt)
{
  if (!(this->parentMode & PNODE_STATIC_NODE))
  {
    if( likely(this->parent != NULL))
    {
        // movement for nodes with smoothMove enabled
        if (unlikely(this->toCoordinate != NULL))
        {
          Vector moveVect = (*this->toCoordinate - this->relCoordinate) *fabsf(dt)*bias;
          if (likely(moveVect.len() >= PNODE_ITERATION_DELTA))
          {
            this->shiftCoor(moveVect);
          }
          else
          {
            delete this->toCoordinate;
            this->toCoordinate = NULL;
            PRINTF(5)("SmoothMove of %s finished\n", this->getName());
          }
        }
        if (unlikely(this->toDirection != NULL))
        {
          Quaternion rotQuat = Quaternion::quatSlerp(this->relDirection,*this->toDirection, fabsf(dt)*this->bias);
          if (this->relDirection.distance(rotQuat) >PNODE_ITERATION_DELTA)
          {
            this->relDirection = rotQuat;
            this->bRelDirChanged;
          }
          else
          {
            delete this->toDirection;
            this->toDirection = NULL;
            PRINTF(5)("SmoothRotate of %s finished\n", this->getName());
          }
        }

        // MAIN UPDATE /////////////////////////////////////
        this->lastAbsCoordinate = this->absCoordinate;

        PRINTF(5)("PNode::update - '%s::%s' - (%f, %f, %f)\n", this->getClassName(), this->getName(),
                      this->absCoordinate.x, this->absCoordinate.y, this->absCoordinate.z);


        if(this->bRelDirChanged && this->parentMode & PNODE_LOCAL_ROTATE )
        {
          /* update the current absDirection - remember * means rotation around sth.*/
          this->prevRelCoordinate = this->relCoordinate;
          this->absDirection = parent->getAbsDir() * this->relDirection;
        }

        if(likely(this->bRelCoorChanged && this->parentMode & PNODE_MOVEMENT))
        {
        /* update the current absCoordinate */
          this->prevRelCoordinate = this->relCoordinate;
          this->absCoordinate = this->parent->getAbsCoor() + this->relCoordinate;
        }
        else if( this->parentMode & PNODE_ROTATE_MOVEMENT && this->bRelCoorChanged)
        {
          /* update the current absCoordinate */
          this->prevRelCoordinate = this->relCoordinate;
          this->absCoordinate = this->parent->getAbsCoor() + parent->getAbsDir().apply(this->relCoordinate);
        }
        /////////////////////////////////////////////////
      }

  else // Nodes without a Parent are handled faster :: MOST LIKELY THE NULLPARENT
    {
      PRINTF(4)("update ParentLess Node (%s::%s) - (%f, %f, %f)\n", this->getClassName(), this->getName(),
                this->absCoordinate.x, this->absCoordinate.y, this->absCoordinate.z);
        if (this->bRelCoorChanged)
        {
          this->prevRelCoordinate = this->relCoordinate;
          this->absCoordinate = this->relCoordinate;
        }
        if (this->bRelDirChanged)
        {
          this->prevRelDirection = this->relDirection;
          this->absDirection = this->getAbsDir () * this->relDirection;
        }
      }
    }

    if(!this->children.empty() && (this->bActive || this->parentMode & PNODE_UPDATE_CHILDREN_IF_INACTIVE ))
    {
      list<PNode*>::iterator child;
      for (child = this->children.begin(); child != this->children.end(); child ++)
      {
        /* if this node has changed, make sure, that all children are updated also */
        if( likely(this->bRelCoorChanged))
          (*child)->parentCoorChanged ();
        if( likely(this->bRelDirChanged))
          (*child)->parentDirChanged ();

        (*child)->updateNode(dt);
      }
    }
    this->velocity = (this->absCoordinate - this->lastAbsCoordinate) / dt;
    this->bRelCoorChanged = false;
    this->bRelDirChanged = false;
}





/*************
 * DEBUGGING *
 *************/
/**
 * @brief counts total amount the children walking through the entire tree.
 * @param nodes the counter
 */
void PNode::countChildNodes(int& nodes) const
{
  nodes++;
  list<PNode*>::const_iterator child;
  for (child = this->children.begin(); child != this->children.end(); child ++)
    (*child)->countChildNodes(nodes);
}


/**
 * @brief displays some information about this pNode
 * @param depth The deph into which to debug the children of this PNode to.
 * (0: all children will be debugged, 1: only this PNode, 2: this and direct children, ...)
 * @param level !! INTERNAL !! The n-th level of the Node we draw (this is internal and only for nice output).
 */
void PNode::debugNode(unsigned int depth, unsigned int level) const
{
  for (unsigned int i = 0; i < level; i++)
    PRINT(0)(" |");
  if (this->children.size() > 0)
    PRINT(0)(" +");
  else
    PRINT(0)(" -");

  int childNodeCount = 0;
  this->countChildNodes(childNodeCount);

  PRINT(0)("PNode(%s::%s) - absCoord: (%0.2f, %0.2f, %0.2f), relCoord(%0.2f, %0.2f, %0.2f), direction(%0.2f, %0.2f, %0.2f) - %s - %d childs\n",
           this->getClassName(),
           this->getName(),
           this->absCoordinate.x,
           this->absCoordinate.y,
           this->absCoordinate.z,
           this->relCoordinate.x,
           this->relCoordinate.y,
           this->relCoordinate.z,
           this->getAbsDirV().x,
           this->getAbsDirV().y,
           this->getAbsDirV().z,
           this->parentingModeToChar(parentMode),
           childNodeCount);
  if (depth >= 2 || depth == 0)
  {
    list<PNode*>::const_iterator child;
    for (child = this->children.begin(); child != this->children.end(); child ++)
    {
      if (depth == 0)
        (*child)->debugNode(0, level + 1);
      else
        (*child)->debugNode(depth - 1, level +1);
    }
  }
}

/**
 * @brief displays the PNode at its position with its rotation as a cube.
 * @param  depth The deph into which to debug the children of this PNode to.
 * (0: all children will be displayed, 1: only this PNode, 2: this and direct children, ...)
 * @param size the Size of the Box to draw.
 * @param color the color of the Box to display.
 * @param level !! INTERNAL !! The n-th level of the Node we draw (this is internal and only for nice output).
 */
void PNode::debugDraw(unsigned int depth, float size, const Vector& color, unsigned int level) const
{
  // if this is the first Element we draw
  if (level == 0)
  {
    glPushAttrib(GL_ENABLE_BIT); // save the Enable-attributes
    glMatrixMode(GL_MODELVIEW);  // goto the ModelView Matrix

    glDisable(GL_LIGHTING);      // disable lighting (we do not need them for just lighting)
    glDisable(GL_BLEND);         // ''
    glDisable(GL_TEXTURE_2D);    // ''
    glDisable(GL_DEPTH_TEST);    // ''
  }

  glPushMatrix();                // repush the Matrix-stack
  /* translate */
  glTranslatef (this->getAbsCoor ().x,
                this->getAbsCoor ().y,
                this->getAbsCoor ().z);
//  this->getAbsDir ().matrix (matrix);

  /* rotate */
  Vector tmpRot = this->getAbsDir().getSpacialAxis();
  glRotatef (this->getAbsDir().getSpacialAxisAngle(), tmpRot.x, tmpRot.y, tmpRot.z );
  /* set the new Color */
  glColor3f(color.x, color.y, color.z);
  { /* draw a cube of size size */
    glBegin(GL_LINE_STRIP);
    glVertex3f(-.5*size, -.5*size,  -.5*size);
    glVertex3f(+.5*size, -.5*size,  -.5*size);
    glVertex3f(+.5*size, -.5*size,  +.5*size);
    glVertex3f(-.5*size, -.5*size,  +.5*size);
    glVertex3f(-.5*size, -.5*size,  -.5*size);
    glEnd();
    glBegin(GL_LINE_STRIP);
    glVertex3f(-.5*size, +.5*size,  -.5*size);
    glVertex3f(+.5*size, +.5*size,  -.5*size);
    glVertex3f(+.5*size, +.5*size,  +.5*size);
    glVertex3f(-.5*size, +.5*size,  +.5*size);
    glVertex3f(-.5*size, +.5*size,  -.5*size);
    glEnd();

    glBegin(GL_LINES);
    glVertex3f(-.5*size, -.5*size,  -.5*size);
    glVertex3f(-.5*size, +.5*size,  -.5*size);
    glVertex3f(+.5*size, -.5*size,  -.5*size);
    glVertex3f(+.5*size, +.5*size,  -.5*size);
    glVertex3f(+.5*size, -.5*size,  +.5*size);
    glVertex3f(+.5*size, +.5*size,  +.5*size);
    glVertex3f(-.5*size, -.5*size,  +.5*size);
    glVertex3f(-.5*size, +.5*size,  +.5*size);
    glEnd();
  }

  glPopMatrix();
  if (depth >= 2 || depth == 0)
  {
    /* rotate the current color in HSV space around 20 degree */
    Vector childColor =  Color::HSVtoRGB(Color::RGBtoHSV(color)+Vector(20,0,.0));
    list<PNode*>::const_iterator child;
    for (child = this->children.begin(); child != this->children.end(); child ++)
    {
      // drawing the Dependency graph
     if (this != PNode::getNullParent())
      {
       glBegin(GL_LINES);
       glColor3f(color.x, color.y, color.z);
       glVertex3f(this->getAbsCoor ().x,
                  this->getAbsCoor ().y,
                  this->getAbsCoor ().z);
        glColor3f(childColor.x, childColor.y, childColor.z);
        glVertex3f((*child)->getAbsCoor ().x,
                   (*child)->getAbsCoor ().y,
                   (*child)->getAbsCoor ().z);
        glEnd();
      }

      /* if we want to draw the children too */
      if (depth == 0) /* -> all of them */
        (*child)->debugDraw(0, size, childColor, level+1);
      else            /* -> only the Next one */
        (*child)->debugDraw(depth - 1, size, childColor, level +1);
    }
  }
  if (level == 0)
    glPopAttrib(); /* pop the saved attributes back out */
}



/////////////////////
// HELPER_FUCTIONS //
/////////////////////

/**
 * @brief converts a parentingMode into a string that is the name of it
 * @param parentingMode the ParentingMode to convert
 * @return the converted string
 */
const char* PNode::parentingModeToChar(int parentingMode)
{
  if (parentingMode == PNODE_LOCAL_ROTATE)
    return "local-rotate";
  else if (parentingMode == PNODE_ROTATE_MOVEMENT)
    return "rotate-movement";
  else if (parentingMode == PNODE_MOVEMENT)
    return "movement";
  else if (parentingMode == PNODE_ALL)
    return "all";
  else if (parentingMode == PNODE_ROTATE_AND_MOVE)
    return "rotate-and-move";
}

/**
 * @brief converts a parenting-mode-string into a int
 * @param parentingMode the string naming the parentingMode
 * @return the int corresponding to the named parentingMode
 */
PARENT_MODE PNode::charToParentingMode(const char* parentingMode)
{
  if (!strcmp(parentingMode, "local-rotate"))
    return (PNODE_LOCAL_ROTATE);
  else  if (!strcmp(parentingMode, "rotate-movement"))
    return (PNODE_ROTATE_MOVEMENT);
  else  if (!strcmp(parentingMode, "movement"))
    return (PNODE_MOVEMENT);
  else  if (!strcmp(parentingMode, "all"))
    return (PNODE_ALL);
  else  if (!strcmp(parentingMode, "rotate-and-move"))
    return (PNODE_ROTATE_AND_MOVE);
}

/**
 * Writes data from network containing information about the state
 * @param data pointer to data
 * @param length length of data
 * @param sender hostID of sender
 */
int PNode::writeState( const byte * data, int length, int sender )
{
  SYNCHELP_READ_BEGIN();

  SYNCHELP_READ_FKT( BaseObject::writeState );

  PRINTF(0)("name = %s\n", this->getName());

  char * parentName = NULL;
  SYNCHELP_READ_STRINGM( parentName );

  if ( strcmp(parentName, "")==0 )
  {
    setParent( (char*)NULL );
  }
  else
  {
    setParent( parentName );
  }

  delete[] parentName;

  int parentMode;
  SYNCHELP_READ_INT( parentMode );
  this->setParentMode((PARENT_MODE)parentMode);

  float f1, f2, f3, f4;

  SYNCHELP_READ_FLOAT( f1 );
  SYNCHELP_READ_FLOAT( f2 );
  SYNCHELP_READ_FLOAT( f3 );
  this->setRelCoor( f1, f2, f3 );
  //this->setRelCoor( 10, 0, 0 );

  SYNCHELP_READ_FLOAT( f1 );
  SYNCHELP_READ_FLOAT( f2 );
  SYNCHELP_READ_FLOAT( f3 );
  //this->setAbsCoor( f1, f2, f3 );

  SYNCHELP_READ_FLOAT( f1 );
  SYNCHELP_READ_FLOAT( f2 );
  SYNCHELP_READ_FLOAT( f3 );
  SYNCHELP_READ_FLOAT( f4 );
  this->setRelDir( Quaternion( Vector(f2, f3, f4), f1 ) );

  SYNCHELP_READ_FLOAT( f1 );
  SYNCHELP_READ_FLOAT( f2 );
  SYNCHELP_READ_FLOAT( f3 );
  SYNCHELP_READ_FLOAT( f4 );
  //this->setAbsDir( Quaternion( Vector(f2, f3, f4), f1 ) );

  int n;
  char * childName;

  SYNCHELP_READ_INT( n );

  for (int i = 0; i<n; i++)
  {
    SYNCHELP_READ_STRINGM( childName );
    PRINTF(0)("childname = %s\n", childName);
    addChild( childName );
    delete childName;
    childName = NULL;
  }

  return SYNCHELP_READ_N;
}

/**
 * data copied in data will bee sent to another host
 * @param data pointer to data
 * @param maxLength max length of data
 * @return the number of bytes writen
 */
int PNode::readState( byte * data, int maxLength )
{
  SYNCHELP_WRITE_BEGIN();

  SYNCHELP_WRITE_FKT( BaseObject::readState );

  PRINTF(0)("name = %s\n", this->getName());

  if ( this->parent )
  {
    SYNCHELP_WRITE_STRING( parent->getName() );
  }
  else
  {
    SYNCHELP_WRITE_STRING( "" );
  }

  SYNCHELP_WRITE_INT( this->parentMode );

  SYNCHELP_WRITE_FLOAT( this->relCoordinate.x );
  SYNCHELP_WRITE_FLOAT( this->relCoordinate.y );
  SYNCHELP_WRITE_FLOAT( this->relCoordinate.z );

  PRINTF(0)("%s, %f, %f, %f\n", getClassName(), relCoordinate.x, relCoordinate.y, relCoordinate.z);

  SYNCHELP_WRITE_FLOAT( this->absCoordinate.x );
  SYNCHELP_WRITE_FLOAT( this->absCoordinate.y );
  SYNCHELP_WRITE_FLOAT( this->absCoordinate.z );

  PRINTF(0)("%s, %f, %f, %f\n", getClassName(), absCoordinate.x, absCoordinate.y, absCoordinate.z);

  SYNCHELP_WRITE_FLOAT( this->relDirection.w );
  SYNCHELP_WRITE_FLOAT( this->relDirection.v.x );
  SYNCHELP_WRITE_FLOAT( this->relDirection.v.y );
  SYNCHELP_WRITE_FLOAT( this->relDirection.v.z );

  SYNCHELP_WRITE_FLOAT( this->absDirection.w );
  SYNCHELP_WRITE_FLOAT( this->absDirection.v.x );
  SYNCHELP_WRITE_FLOAT( this->absDirection.v.y );
  SYNCHELP_WRITE_FLOAT( this->absDirection.v.z );

  int n = children.size();
  SYNCHELP_WRITE_INT( n );

  for (std::list<PNode*>::const_iterator it = children.begin(); it!=children.end(); it++)
  {
    PRINTF(0)("childname = %s\n", (*it)->getName() );
    SYNCHELP_WRITE_STRING( (*it)->getName() );
  }
  return SYNCHELP_WRITE_N;
}