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

/*
   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& absCoor, PNode* parent )
{
  this->init(parent);

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

  this->setAbsCoor(absCoor);
}

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

/**
 *  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->bRelDirChanged = true;
  this->parent = parent;
  this->parentMode = PNODE_PARENT_MODE_DEFAULT;

  // iterators
  this->toCoordinate = 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>(element, "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)
{
  if (this->toCoordinate!= NULL)
  {
    delete this->toCoordinate;
    this->toCoordinate = NULL;
  }

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

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

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

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

   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)
{
  this->relCoordinate += shift;
  this->bRelCoorChanged = true;
}

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


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

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

/**
 *  adds a child and makes this node to a parent
 * @param child 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* child, int parentMode)
{
  if( likely(child->parent != NULL))
    {
      PRINTF(4)("PNode::addChild() - reparenting node: removing it and adding it again\n");
      child->parent->children->remove(child);
    }
  child->parentMode = parentMode;
  child->parent = this;
  this->children->add(child);
  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);
}

/**
 *  removes a child from the node
 * @param child the child to remove from this pNode.
 *
 * Children from pNode will not be lost, they are referenced to NullPointer
*/
void PNode::removeChild (PNode* child)
{
  if (child != NULL)
  {
   child->remove();
//   this->children->remove(child);
//   child->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 being destroyed.
*/
void PNode::remove()
{
  tIterator<PNode>* iterator = this->children->getIterator();
  PNode* pn = iterator->firstElement();

  while( pn != NULL)
    {
      NullParent::getInstance()->addChild(pn, pn->getParentMode());
      pn = iterator->nextElement();
    }
  delete iterator;
  if (this->parent != NULL)
    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.
 * @param bias the speed to iterate to this new Positions
 */
void PNode::softReparent(PNode* parentNode, float bias)
{
  if (this->parent == parentNode)
    return;

  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->setRelCoor(this->parent->getAbsDir().inverse().apply(tmpV - this->parent->getAbsCoor()));
 else
   this->setRelCoor(tmpV - parentNode->getAbsCoor());

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

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

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

/**
 *  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->toCoordinate != NULL))
      {
        Vector moveVect = (*this->toCoordinate - this->getRelCoor()) *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(Quaternion(), (*this->toDirection / this->relDirection), dt*this->bias);
        if (likely(rotQuat.getSpacialAxisAngle() > PNODE_ITERATION_DELTA))
        {
          this->shiftDir(rotQuat);
        }
        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 - (%f, %f, %f)\n", this->getName(), this->absCoordinate.x, this->absCoordinate.y, this->absCoordinate.z);


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

      if(likely(this->parentMode & PNODE_MOVEMENT && this->bRelCoorChanged))
      {
        /* 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
    {
      PRINTF(4)("NullParent::update - (%f, %f, %f)\n", 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->getSize() > 0)
    {
      tIterator<PNode>* iterator = this->children->getIterator();
      PNode* pn = iterator->firstElement();
      while( pn != NULL)
      {
        /* if this node has changed, make sure, that all children are updated also */
        if( likely(this->bRelCoorChanged))
          pn->parentCoorChanged ();
        if( likely(this->bRelDirChanged))
          pn->parentDirChanged ();

        pn->update(dt);
        pn = iterator->nextElement();
      }
      delete iterator;
    }
    this->velocity = (this->absCoordinate - this->lastAbsCoordinate) / dt;
    this->bRelCoorChanged = false;
    this->bRelDirChanged = 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), direction(%0.2f, %0.2f, %0.2f) - %s\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));
  if (depth >= 2 || depth == 0)
  {
    tIterator<PNode>* iterator = this->children->getIterator();
      //PNode* pn = this->children->enumerate ();
    PNode* pn = iterator->firstElement();
    while( pn != NULL)
    {
      if (depth == 0)
        pn->debug(0, level + 1);
      else
        pn->debug(depth - 1, level +1);
      pn = iterator->nextElement();
    }
    delete iterator;
  }
}
#include "color.h"

/**
   displays the PNode at its position with its rotation as a cube.
*/
void PNode::debugDraw(unsigned int depth, float size, Vector color) const
{
  glMatrixMode(GL_MODELVIEW);
  glPushMatrix();
  glDisable(GL_LIGHTING);

  /* translate */
  glTranslatef (this->getAbsCoor ().x,
                this->getAbsCoor ().y,
                this->getAbsCoor ().z);
  /* rotate */
//  this->getAbsDir ().matrix (matrix);
//  glMultMatrixf((float*)matrix);

  Vector tmpRot = this->getAbsDir().getSpacialAxis();
  glColor3f(color.x, color.y, color.z);
  glRotatef (this->getAbsDir().getSpacialAxisAngle(), tmpRot.x, tmpRot.y, tmpRot.z );
  {
    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();
  glEnable(GL_LIGHTING);
  if (depth >= 2 || depth == 0)
  {
    Vector childColor =  Color::HSVtoRGB(Color::RGBtoHSV(color)+Vector(20,0,.0));

    tIterator<PNode>* iterator = this->children->getIterator();
    PNode* pn = iterator->firstElement();
    while( pn != NULL)
    {
      if (depth == 0)
        pn->debugDraw(0, size, childColor);
      else
        pn->debugDraw(depth - 1, size, childColor);
      pn = iterator->nextElement();
    }
    delete iterator;
  }
}



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

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

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