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

/*
   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 "stdincl.h"
#include "compiler.h"

#include "error.h"
#include "debug.h"
#include "list.h"
#include "vector.h"
#include "null_parent.h"
#include "load_param.h"

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

using namespace std;


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


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

/**
   \brief 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;
  this->parent = NULL;
}

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

/**
   \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<PNode>(root, "abs-coor", this, &PNode::setAbsCoor)
      .describe("Sets The absolute Position of the Node.");

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

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

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

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

/**
   \brief 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...

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

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

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

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

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

/**
   \brief 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(3)("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);
}

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


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


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


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


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

/**
   \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::update (float dt)
{
  if( likely(this->parent != NULL))
    {
      this->lastAbsCoordinate = this->absCoordinate;

      PRINTF(4)("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) /*&& this->timeStamp != DataTank::timeStamp*/)
            {
              /* if you have set the absolute coordinates this overrides all other changes */
              this->relCoordinate = this->absCoordinate - parent->getAbsCoor ();
            }
          if( likely(this->bRelCoorChanged) /*&& this->timeStamp != DataTank::timeStamp*/)

            {
              /*this is bad style... must be deleted later - just for testing*/

              /*
                if( unlikely(this->parent == NULL))
                {
                *this->absCoordinate = *this->relCoordinate;
                }
                else */
              this->absCoordinate = parent->getAbsCoor() + this->relCoordinate;       /* update the current absCoordinate */
            }
        }

      if( this->parentMode & PNODE_LOCAL_ROTATE)
        {
          if( unlikely(this->bAbsDirChanged) /*&& this->timeStamp != DataTank::timeStamp*/)
            {
              /* if you have set the absolute coordinates this overrides all other changes */
              this->relDirection = this->absDirection - parent->getAbsDir();
            }
          else if( likely(this->bRelDirChanged) /*&& this->timeStamp != DataTank::timeStamp*/)
            {
              /* 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) /*&& this->timeStamp != DataTank::timeStamp*/)
            {
              /* if you have set the absolute coordinates this overrides all other changes */
              this->relCoordinate = this->absCoordinate - parent->getAbsCoor ();
            }
          else if( likely(this->bRelCoorChanged) /*&& this->timeStamp != DataTank::timeStamp*/)
            {
              /*this is bad style... must be deleted later - just for testing*/
              /*if( this->parent == NULL)
               *this->absCoordinate = *this->relCoordinate;
               else*/
              this->absCoordinate = parent->getAbsCoor() + parent->getAbsDir().apply(this->relCoordinate);            /* update the current absCoordinate */
            }
        }


      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( 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;
    }
}

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

/**
  @brief 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,  .5, -.5);
    glVertex3f( .5,  .5,  .5);
    glVertex3f(-.5,  .5,  .5);
    glVertex3f(-.5, -.5,  .5);
    glVertex3f( .5,  .5, -.5);
    glVertex3f( .5, -.5, -.5);
    glVertex3f(-.5, -.5, -.5);
    glVertex3f(-.5,  .5, -.5);
    glEnd();
    glBegin(GL_LINE_STRIP);
    glVertex3f(-.5, -.5, -.5);
    glVertex3f(-.5, -.5,  .5);
    glVertex3f( .5, -.5,  .5);
    glVertex3f( .5, -.5, -.5);
    glEnd();
    glBegin(GL_LINES);
    glVertex3f( .5, -.5,  .5);
    glVertex3f( .5,  .5,  .5);
    glVertex3f(-.5, -.5,  .5);
    glVertex3f(-.5,  .5,  .5);
    glEnd();
  }

  glPopMatrix();
}