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

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

   @todo Smooth-Parent: delay, speed
*/

#define DEBUG_SPECIAL_MODULE DEBUG_MODULE_PNODE

#include "p_node.h"
#include "null_parent.h"

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

#include "stdincl.h"
#include "compiler.h"
#include "error.h"
#include "debug.h"
#include "list.h"
#include "vector.h"

//#include "vector.h"
//#include "quaternion.h"

using namespace std;


/**
 *  standard constructor
*/
PNode::PNode ()
{
  init(NULL);

  NullParent::getInstance()->addChild(this);
}

/**
 * @param root the load-Element for the PNode
*/
PNode::PNode(const TiXmlElement* root)
{
  this->init(NULL);
  this->loadParams(root);

  NullParent::getInstance()->addChild(this);
}


/**
 *  constructor with coodinates
 * @param absCoordinate the Absolute coordinate of the Object
 * @param parent The parent-node of this node.
*/
PNode::PNode (const Vector& absCoordinate, PNode* parent )
{
  this->init(parent);

  this->absCoordinate = absCoordinate;

  if (likely(parent != NULL))
  {
    this->relCoordinate = this->absCoordinate - parent->getAbsCoor();
    parent->addChild (this);
  }
}

/**
 *  standard deconstructor
*/
PNode::~PNode ()
{
  tIterator<PNode>* iterator = this->children->getIterator();
  PNode* pn = iterator->nextElement();
  while( pn != NULL)
    {
      delete pn;
      pn = iterator->nextElement();
    }
  delete iterator;
  /* this deletes all children in the list */
  delete this->children;
  if (this->parent)
    this->parent->removeChild(this);
}

/**
 *  initializes a PNode
 * @param parent the parent for this PNode
*/
void PNode::init(PNode* parent)
{
  this->setClassID(CL_PARENT_NODE, "PNode");
  this->children = new tList<PNode>();
  this->bRelCoorChanged = true;
  this->bAbsCoorChanged = false;
  this->bRelDirChanged = true;
  this->bAbsDirChanged = false;
  this->parent = parent;

  this->toPosition = NULL;
  this->toDirection = NULL;
  this->bias = 1.0;
}

/**
 *  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<PNode>(root, "rel-coor", this, &PNode::setRelCoor)
      .describe("Sets The relative position of the Node to its parent.");

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

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

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

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

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

  // cycling properties
  if (root != NULL)
  {
    const TiXmlElement* element = root->FirstChildElement();
    while (element != NULL)
    {
      LoadParam<PNode>(root, "parent", this, &PNode::addChild, true)
          .describe("adds a new Child to the current Node.");

      element = element->NextSiblingElement();
    }
  }
}

/**
 *  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)
{
  this->bRelCoorChanged = true;
  this->relCoordinate = relCoord;
}

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

/**
 * 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->toPosition == NULL))
    this->toPosition = new Vector();

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


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

   it is very importand, that you use this function, if you want to update the
   absCoordinates. If you don't use this, the PNode won't recognize, that something
   has changed and won't update the children Nodes.
*/
void PNode::setAbsCoor (const Vector& absCoord)
{
  this->bAbsCoorChanged = 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));
}

/**
 *  shift coordinate (abs and rel)
 * @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);

   elsewhere you would have to:
   PNode* someNode = ...;
   Vector objectMovement = calculateShift();
   Vector currentCoor = someNode->getRelCoor();
   Vector newCoor = currentCoor + objectMovement;
   someNode->setRelCoor(newCoor);

   yea right... shorter...
 *
 * @todo this is ambiguous, from the outside one does not know it absCoor has been changed
 * this might lead to strange artefacts !!

*/
void PNode::shiftCoor (const Vector& shift)
{

  if( unlikely(this->bAbsCoorChanged))
    {
      this->absCoordinate += shift;
    }
  else
    {
      this->relCoordinate += shift;
      this->bRelCoorChanged = true;
    }
}

/**
 *  set relative direction
 * @param relDir to its parent

   it is very importand, that you use this function, if you want to update the
   relDirection. If you don't use this, the PNode won't recognize, that something
   has changed and won't update the children Nodes.
*/
void PNode::setRelDir (const Quaternion& relDir)
{
  this->bRelCoorChanged = true;
  this->relDirection = relDir;
}

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


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

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


/**
 *  sets the absolute direction (0,0,1)
 * @param absDir absolute coordinates

   it is very importand, that you use this function, if you want to update the
   absDirection. If you don't use this, the PNode won't recognize, that something
   has changed and won't update the children Nodes.
*/
void PNode::setAbsDir (const Quaternion& absDir)
{
  this->bAbsDirChanged = true;
  this->absDirection = absDir;
}

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

/**
 *  shift coordinate (abs and rel)
 * @param shift vector

   this function shifts the current coordinates about the vector shift. this is
   usefull because from some place else you can:
   PNode* someNode = ...;
   Quaternion objectMovement = calculateShift();
   someNode->shiftCoor(objectMovement);

   elsewhere you would have to:
   PNode* someNode = ...;
   Quaternion objectMovement = calculateShift();
   Quaternion currentCoor = someNode->getRelCoor();
   Quaternion newCoor = currentCoor + objectMovement;
   someNode->setRelCoor(newCoor);

   yea right... shorter...

   @todo implement this
*/
void PNode::shiftDir (const Quaternion& shift)
{
  this->bRelDirChanged = true;
  this->relDirection = this->relDirection * shift;
}

/**
 *  adds a child and makes this node to a parent
 * @param pNode child reference
 * @param parentMode on which changes the child should also change ist state

   use this to add a child to this node.
*/
void PNode::addChild (PNode* pNode, int parentMode)
{
  if( likely(pNode->parent != NULL))
    {
      PRINTF(4)("PNode::addChild() - reparenting node: removing it and adding it again\n");
      pNode->parent->children->remove(pNode);
    }
  pNode->parentMode = parentMode;
  pNode->parent = this;
  this->children->add(pNode);
}

/**
 * @see PNode::addChild(PNode* parent);
 * @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);
}


/**
 *  removes a child from the node
 * @param pNode the child to remove from this pNode.

   Children from pNode will not be lost, they are referenced to NullPointer
*/
void PNode::removeChild (PNode* pNode)
{
  pNode->remove();
  this->children->remove (pNode);
  pNode->parent = NULL;
}


/**
 *  remove this pnode from the tree and adds all following to NullParent

   this can be the case, if an entity in the world is been destroyed.
*/
void PNode::remove()
{
  NullParent* nullParent = NullParent::getInstance();

  tIterator<PNode>* iterator = this->children->getIterator();
  PNode* pn = iterator->nextElement();

  while( pn != NULL)
    {
      nullParent->addChild(pn, pn->getParentMode());
      pn = iterator->nextElement();
    }
  delete iterator;
  this->parent->children->remove(this);
}


/**
 *  sets the parent of this PNode
 * @param parent the Parent to set
*/
void PNode::setParent (PNode* parent)
{
  parent->addChild(this);
}

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

/**
 * does the reparenting in a very smooth way
 * @param parentNode the new Node to connect this node to.
 */
void PNode::softReparent(PNode* parentNode, float bias)
{
  if (this->parent == parentNode)
    return;

  //this->setRelCoorSoft(this->getRelCoor());
  if (likely(this->toPosition == NULL))
  {
    this->toPosition = new Vector();
    *this->toPosition = 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->setRelCoor(parent->getRelDir().apply(tmpV - parent->getAbsCoor()));
  else*/
  this->setRelCoor(tmpV - parentNode->getAbsCoor());

  this->setRelDir(tmpQ - parentNode->getAbsDir());
}

void PNode::softReparent(const char* parentName, float bias)
{
  PNode* parentNode = dynamic_cast<PNode*>(ClassList::getObject(parentName, CL_PARENT_NODE));
  if (parentNode != NULL)
    this->softReparent(parentNode, bias);
}


/**
 *  set the mode of this parent manualy
 * @param parentMode the mode of the bind-type.
*/
void PNode::setParentMode (PARENT_MODE parentMode)
{
  this->parentMode = parentMode;
}

/**
 *  sets the mode of this parent manually
 * @param parentMode a String representing this parentingMode
 */
void PNode::setParentMode (const char* parentingMode)
{
  if (!strcmp(parentingMode, "local-rotate"))
    this->setParentMode(PNODE_LOCAL_ROTATE);
  else  if (!strcmp(parentingMode, "rotate-movement"))
    this->setParentMode(PNODE_ROTATE_MOVEMENT);
  else  if (!strcmp(parentingMode, "movement"))
    this->setParentMode(PNODE_MOVEMENT);
  else  if (!strcmp(parentingMode, "all"))
    this->setParentMode(PNODE_ALL);
  else  if (!strcmp(parentingMode, "rotate-and-move"))
    this->setParentMode(PNODE_ROTATE_AND_MOVE);
}


/**
 *  has to be called, if the parent coordinate has changed

   normaly this will be done by the parent itself automaticaly. If you call this, you
   will force an update of the coordinated of the node.
*/
/*
void PNode::parentCoorChanged ()
{
  this->bRelCoorChanged = true;
}
*/

/**
 *  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::update (float dt)
{
  if( likely(this->parent != NULL))
    {
      // movement for nodes with smoothMove enabled
      if (unlikely(this->toPosition != NULL))
      {
        Vector moveVect = (*this->toPosition - this->getRelCoor()) *dt*bias;

        if (likely(moveVect.len() >= .001))
        {
          this->shiftCoor(moveVect);
        }
        else
        {
          delete this->toPosition;
          this->toPosition = NULL;
          PRINTF(5)("SmoothMove of %s finished\n", this->getName());
        }
      }
      if (unlikely(this->toDirection != NULL))
      {
        Quaternion rotQuat = (*this->toDirection - this->getRelDir()) *dt*bias;

//        if (likely(rotQuat.len() >= .001))
        {
          this->shiftDir(rotQuat);
        }
/*        else
        {
          delete this->toPosition;
          this->toPosition = NULL;
          PRINTF(5)("SmoothMove of %s finished\n", this->getName());
        }*/
      }

      this->lastAbsCoordinate = this->absCoordinate;

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

      if( likely(this->parentMode & PNODE_MOVEMENT))
        {
          if( unlikely(this->bAbsCoorChanged))
            {
              /* if you have set the absolute coordinates this overrides all other changes */
              this->relCoordinate = this->absCoordinate - parent->getAbsCoor ();
            }
          if( likely(this->bRelCoorChanged))
            {
              /* update the current absCoordinate */
              this->absCoordinate = parent->getAbsCoor() + this->relCoordinate;
            }
        }

      if( this->parentMode & PNODE_LOCAL_ROTATE)
        {
          if( unlikely(this->bAbsDirChanged))
            {
              /* if you have set the absolute coordinates this overrides all other changes */
              this->relDirection = this->absDirection - parent->getAbsDir();
            }
          else if( likely(this->bRelDirChanged))
            {
              /* update the current absDirection - remember * means rotation around sth.*/
              this->absDirection = parent->getAbsDir() * this->relDirection;
            }
        }

      if( this->parentMode & PNODE_ROTATE_MOVEMENT)
        {
          if( unlikely(this->bAbsCoorChanged))
            {
              /* if you have set the absolute coordinates this overrides all other changes */
              this->relCoordinate = this->absCoordinate - parent->getAbsCoor ();
            }
          else if( likely(this->bRelCoorChanged))
            {
              /* update the current absCoordinate */
              this->absCoordinate = parent->getAbsCoor() + parent->getAbsDir().apply(this->relCoordinate);
            }
        }


      tIterator<PNode>* iterator = this->children->getIterator();
      PNode* pn = iterator->nextElement();
      while( pn != NULL)
        {
          /* if this node has changed, make sure, that all children are updated also */
          if( likely(this->bRelCoorChanged || this->bAbsCoorChanged))
            pn->parentCoorChanged ();
          if( likely(this->bRelDirChanged || this->bAbsDirChanged))
            pn->parentDirChanged ();

          pn->update(dt);
          //pn = this->children->nextElement();
          pn = iterator->nextElement();
        }
      delete iterator;

      this->velocity = (this->absCoordinate - this->lastAbsCoordinate) / dt;
      this->bRelCoorChanged = false;
      this->bAbsCoorChanged = false;
      this->bRelDirChanged = false;
      this->bAbsDirChanged = false;
    }
  else
    {
      PRINTF(4)("NullParent::update - (%f, %f, %f)\n", this->absCoordinate.x, this->absCoordinate.y, this->absCoordinate.z);
      this->absCoordinate = this->relCoordinate;
      this->absDirection = this->getAbsDir () * this->relDirection;

      tIterator<PNode>* iterator = this->children->getIterator();
      //PNode* pn = this->children->enumerate ();
      PNode* pn = iterator->nextElement();
      while( pn != NULL)
      {
        /* if this node has changed, make sure, that all children are updated also */
          if( this->bRelCoorChanged || this->bAbsCoorChanged)
            pn->parentCoorChanged ();
          if( this->bRelDirChanged || this->bAbsDirChanged)
            pn->parentDirChanged ();
          pn->update (dt);
          //pn = this->children->nextElement ();
          pn = iterator->nextElement();
        }
      delete iterator;
      this->bRelCoorChanged = false;
      this->bAbsCoorChanged = false;
      this->bRelDirChanged = false;
      this->bAbsDirChanged = false;
    }
}

/**
 *  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 The n-th level of the Node we draw (this is internal and only for nice output)
*/
void PNode::debug(unsigned int depth, unsigned int level) const
{
  for (unsigned int i = 0; i < level; i++)
    PRINT(0)(" |");
  if (this->children->getSize() > 0)
    PRINT(0)(" +");
  else
    PRINT(0)(" -");
  PRINT(0)("PNode(%s::%s) - absCoord: (%0.2f, %0.2f, %0.2f), relCoord(%0.2f, %0.2f, %0.2f)\n",
           this->getClassName(),
           this->getName(),
           this->absCoordinate.x,
           this->absCoordinate.y,
           this->absCoordinate.z,
           this->relCoordinate.x,
           this->relCoordinate.y,
           this->relCoordinate.z );
  if (depth >= 2 || depth == 0)
  {
    tIterator<PNode>* iterator = this->children->getIterator();
      //PNode* pn = this->children->enumerate ();
    PNode* pn = iterator->nextElement();
    while( pn != NULL)
    {
      if (depth == 0)
        pn->debug(0, level + 1);
      else
        pn->debug(depth - 1, level +1);
      pn = iterator->nextElement();
    }
    delete iterator;
  }
}

/**
   displays the PNode at its position with its rotation as a cube.
*/
void PNode::debugDraw(float size) const
{
  glMatrixMode(GL_MODELVIEW);
  glPushMatrix();
  float matrix[4][4];

  /* translate */
  glTranslatef (this->getAbsCoor ().x,
                this->getAbsCoor ().y,
                this->getAbsCoor ().z);
  /* rotate */
  this->getAbsDir ().matrix (matrix);
  glMultMatrixf((float*)matrix);
  {
    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);
    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);
    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);
    glEnd();
  }

  glPopMatrix();
}