SkeletonT.cc

/*===========================================================================*\
*                                                                            *
*                              OpenFlipper                                   *
 *           Copyright (c) 2001-2015, RWTH-Aachen University                 *
 *           Department of Computer Graphics and Multimedia                  *
 *                          All rights reserved.                             *
 *                            www.openflipper.org                            *
 *                                                                           *
 *---------------------------------------------------------------------------*
 * This file is part of OpenFlipper.                                         *
 *---------------------------------------------------------------------------*
 *                                                                           *
 * Redistribution and use in source and binary forms, with or without        *
 * modification, are permitted provided that the following conditions        *
 * are met:                                                                  *
 *                                                                           *
 * 1. Redistributions of source code must retain the above copyright notice, *
 *    this list of conditions and the following disclaimer.                  *
 *                                                                           *
 * 2. Redistributions in binary form must reproduce the above copyright      *
 *    notice, this list of conditions and the following disclaimer in the    *
 *    documentation and/or other materials provided with the distribution.   *
 *                                                                           *
 * 3. Neither the name of the copyright holder nor the names of its          *
 *    contributors may be used to endorse or promote products derived from   *
 *    this software without specific prior written permission.               *
 *                                                                           *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS       *
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED *
 * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A           *
 * PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER *
 * OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,  *
 * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,       *
 * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR        *
 * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF    *
 * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING      *
 * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS        *
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.              *
*                                                                            *
\*===========================================================================*/

/*===========================================================================*\
*                                                                            *
*   $Revision$                                                       *
*   $LastChangedBy$                                                *
*   $Date$                     *
*                                                                            *
\*===========================================================================*/

//-----------------------------------------------------------------------------
//
//  CLASS SkeletonT - IMPLEMENTATION
//
//-----------------------------------------------------------------------------

#define SKELETON_C

//== INCLUDES =================================================================

#include <iostream>
#include <algorithm>
#include <utility>

#include "SkeletonT.hh"

#include "Animation/FrameAnimationT.hh"


//-----------------------------------------------------------------------------
// ITERATOR - IMPLEMENTATION
//-----------------------------------------------------------------------------

template<typename PointT>
SkeletonT<PointT>::Iterator::Iterator()
{
  pCurrent_ = 0;
}

//-----------------------------------------------------------------------------

template<typename PointT>
SkeletonT<PointT>::Iterator::Iterator(Joint *_root)
{
  pCurrent_ = _root;
}

//-----------------------------------------------------------------------------

template<typename PointT>
SkeletonT<PointT>::Iterator::Iterator(const Iterator &other)
{
  pCurrent_ = other.pCurrent_;
  stJoints_ = other.stJoints_;
}

//-----------------------------------------------------------------------------

template<typename PointT>
SkeletonT<PointT>::Iterator::~Iterator()
{
}

//-----------------------------------------------------------------------------

template<typename PointT>
typename SkeletonT<PointT>::Iterator &SkeletonT<PointT>::Iterator::operator=(const Iterator &other)
{
  pCurrent_ = other.pCurrent_;
  stJoints_ = other.stJoints_;
  return *this;
}

//-----------------------------------------------------------------------------

template<typename PointT>
typename SkeletonT<PointT>::Iterator &SkeletonT<PointT>::Iterator::operator++()
{
  if(pCurrent_ == 0)
    return *this;

  // try to make this iterator point to the first child
  if(pCurrent_->size() > 0)
  {
    // there are children, so add the current joint to the stack and choose the first child as new current position
    stJoints_.push(pCurrent_);
    pCurrent_ = pCurrent_->child(0);
  }else{

    if ( pCurrent_->isRoot() ){
      //root without children -> return invalid iterator
      pCurrent_ = 0;

    } else {
      // there are no children left, so try to get the next sibling
      Joint *pSibling = nextSibling(stJoints_.top(), pCurrent_);

      while(pSibling == 0 && !stJoints_.empty())
      {
              // there is no sibling, so try the parents sibling and so on
              Joint *pParent = stJoints_.top();
              stJoints_.pop();
              if(!stJoints_.empty())
                      pSibling = nextSibling(stJoints_.top(), pParent);
      }

      // did we fail to find a next node?
      // if so pSibling is 0 now and will invalidate the iterator
      pCurrent_ = pSibling;
    }
  }
  return *this;
}

//-----------------------------------------------------------------------------

template<typename PointT>
bool SkeletonT<PointT>::Iterator::operator!=(const Iterator &other) const
{
  return pCurrent_ != other.pCurrent_;
}

//-----------------------------------------------------------------------------

template<typename PointT>
bool SkeletonT<PointT>::Iterator::operator==(const Iterator &other) const
{
  return pCurrent_ == other.pCurrent_;
}

//-----------------------------------------------------------------------------

template<typename PointT>
typename SkeletonT<PointT>::Joint *SkeletonT<PointT>::Iterator::operator*() const
{
  return pCurrent_;
}

//-----------------------------------------------------------------------------

template<typename PointT>
typename SkeletonT<PointT>::Joint *SkeletonT<PointT>::Iterator::operator->() const
{
  return pCurrent_;
}

//-----------------------------------------------------------------------------

template<typename PointT>
SkeletonT<PointT>::Iterator::operator bool() const
{
  return pCurrent_ != 0;
}

//-----------------------------------------------------------------------------

template<typename PointT>
typename SkeletonT<PointT>::Joint *SkeletonT<PointT>::Iterator::nextSibling(Joint *_pParent, Joint *_pJoint)
{
  // first find the current node in the parents list of children
  typename Joint::ChildIter it;
  for(it = _pParent->begin(); it != _pParent->end(); ++it)
  {
    if(*it == _pJoint)
    {
      // found it. If there is another child it is the wanted sibling
      ++it;
      if(it == _pParent->end())
        return 0;  // oh no, we ran out of siblings

      return *it;
    }
  }

  return 0;
}

//-----------------------------------------------------------------------------
// AnimationIterator - IMPLEMENTATION
//-----------------------------------------------------------------------------

template<typename PointT>
SkeletonT<PointT>::AnimationIterator::AnimationIterator(std::vector<Animation*>& _animations ) :
  animations_(_animations)
{
  currentIndex_ = 0;

  // Increment, until we are at the end or we found a valid animation which is not deleted ( == 0 )
  while ( currentIndex_ < animations_.size() &&  animations_[currentIndex_] == 0) {
    currentIndex_++;
  }

}

template<typename PointT>
SkeletonT<PointT>::AnimationIterator::AnimationIterator(std::vector<Animation*>& _animations, unsigned int _animationIndex ) :
animations_(_animations)
{
  currentIndex_ = _animationIndex;

  // Increment, until we are at the end or we found a valid animation which is not deleted ( == 0 )
  while ( currentIndex_ < animations_.size() &&  animations_[currentIndex_] == 0) {
    currentIndex_++;
  }
}

template<typename PointT>
typename SkeletonT<PointT>::AnimationIterator& SkeletonT<PointT>::AnimationIterator::operator++() {
  currentIndex_++;

  // Increment, until we are at the end or we found a valid animation which is not deleted ( == 0 )
  while ( currentIndex_ < animations_.size() &&  animations_[currentIndex_] == 0) {
    currentIndex_++;
  }

  return *this;
}

template<typename PointT>
typename SkeletonT<PointT>::AnimationIterator& SkeletonT<PointT>::AnimationIterator::operator=(const AnimationIterator &other) {
  currentIndex_ = other.currentIndex_;
  animations_ = other.animations_;
  return *this;
}

template<typename PointT>
SkeletonT<PointT>::AnimationIterator::operator bool() const {
  return ( currentIndex_ < animations_.size() );
}

template<typename PointT>
AnimationHandle SkeletonT<PointT>::AnimationIterator::operator*() const {
  return AnimationHandle(currentIndex_);
}

//-----------------------------------------------------------------------------
// SKELETONT - IMPLEMENTATION
//-----------------------------------------------------------------------------

template<typename PointT>
SkeletonT<PointT>::SkeletonT() : referencePose_(this)
{
}

//-----------------------------------------------------------------------------

template<typename PointT>
SkeletonT<PointT>::SkeletonT(const SkeletonT<PointT> &_other) :
  Properties(),
  referencePose_(this)
{
  // create a copy of the joints, not yet linked because they refer to each other using pointers
  for(typename std::vector<Joint*>::const_iterator it = _other.joints_.begin(); it != _other.joints_.end(); ++it)
  {
    joints_.push_back(new Joint(**it));
    insert_property_at( (*it)->id() );
  }

  // construct the links
  for(typename std::vector<Joint*>::const_iterator it = _other.joints_.begin(); it != _other.joints_.end(); ++it)
  {
    Joint *pJoint = *it;

    if(pJoint->parent() != 0)
      joint(pJoint->id())->parent_ = joint(pJoint->parent()->id());
    else
      joint(pJoint->id())->parent_ = 0;
    for(typename Joint::ChildIter it_ch = pJoint->begin(); it_ch != pJoint->end(); ++it_ch)
      joint(pJoint->id())->children_.push_back( joint((*it_ch)->id()) );
  }

  names_.insert(_other.names_.begin(), _other.names_.end());

  for(typename std::vector<Animation*>::const_iterator it = _other.animations_.begin(); it != _other.animations_.end(); ++it)
    if (*it) {
        animations_.push_back((**it).copy());
    }

  referencePose_ = _other.referencePose_;
}

//-----------------------------------------------------------------------------

template<typename PointT>
SkeletonT<PointT>& SkeletonT<PointT>::operator= (const SkeletonT<PointT>& _other){

  if (this != &_other){ // protect against invalid self-assignment

    // clear the current skeleton
    clear();

    // create a copy of the joints, not yet linked because they refer to each other using pointers
    for(typename std::vector<Joint*>::const_iterator it = _other.joints_.begin(); it != _other.joints_.end(); ++it){
      joints_.push_back(new Joint(**it));
      insert_property_at( (*it)->id() );
    }

    // construct the links
    for(typename std::vector<Joint*>::const_iterator it = _other.joints_.begin(); it != _other.joints_.end(); ++it){
      Joint *pJoint = *it;

      if(pJoint->parent() != 0)
        joint(pJoint->id())->parent_ = joint(pJoint->parent()->id());
      else
        joint(pJoint->id())->parent_ = 0;

      for(typename Joint::ChildIter it_ch = pJoint->begin(); it_ch != pJoint->end(); ++it_ch)
        joint(pJoint->id())->children_.push_back( joint((*it_ch)->id()) );
    }

    names_.insert(_other.names_.begin(), _other.names_.end());

    for(typename std::vector<Animation*>::const_iterator it = _other.animations_.begin(); it != _other.animations_.end(); ++it)
      if (*it)
        animations_.push_back((**it).copy());

    referencePose_ = _other.referencePose_;
  }

  return *this;
}

//-----------------------------------------------------------------------------

template<typename PointT>
SkeletonT<PointT>::~SkeletonT( )
{
  // clear the joints and animations
  clear();
}

//-----------------------------------------------------------------------------

template<typename PointT>
void SkeletonT<PointT>::addJoint(typename SkeletonT<PointT>::Joint *_pParent, typename SkeletonT<PointT>::Joint *_pJoint)
{
  unsigned int newJointID;

  if(_pParent == 0)
  {
    clear();
    clean_properties();

    _pJoint->setId(0);
    joints_.push_back(_pJoint);

    newJointID = 0;
  }else{
    _pParent->children_.push_back(_pJoint); // tell the parent about the new child
    _pJoint->setId(joints_.size());         // set its id
    joints_.push_back(_pJoint);             // add it to the skeleton vector

    newJointID = joints_.size() - 1;
  }

  //onAddJoint
  insert_property_at(newJointID);

  referencePose_.insertJointAt(newJointID);
  for(typename std::vector<Animation*>::iterator it = animations_.begin(); it != animations_.end(); ++it)
      if (*it)
          (*it)->insertJointAt(newJointID);

  referencePose_.updateFromGlobal(0, true);
}

//-----------------------------------------------------------------------------

template<typename PointT>
void SkeletonT<PointT>::removeJoint(typename SkeletonT<PointT>::Joint *_pJoint)
{

  if (joints_.size() == 1){
    std::cerr << "Cannot delete last joint. Delete the skeleton instead." << std::endl;
    return;
  }

  remove_property_at(_pJoint->id());
  referencePose_.removeJointAt(_pJoint->id());

  for(typename std::vector<Animation*>::iterator it = animations_.begin(); it != animations_.end(); ++it)
      if (*it) (*it)->removeJointAt(_pJoint->id());

  // Reattach the deleted joint's children to the joint's parent
  typename SkeletonT<PointT>::Joint::ChildIter c_it = _pJoint->begin();

  if( _pJoint->parent() == 0 ){
    //root removal
    typename SkeletonT<PointT>::Joint* newRoot = *c_it; //first child is new root
    newRoot->parent_ = 0;
    ++c_it;

    for ( ; c_it!=_pJoint->end(); ++c_it) {
      (*c_it)->parent_ = newRoot;
      newRoot->children_.push_back(*c_it);
    }

  } else {

    for ( ; c_it!=_pJoint->end(); ++c_it) {
      (*c_it)->parent_ = _pJoint->parent_;
      _pJoint->parent_->children_.push_back(*c_it);
    }

    if(std::remove(_pJoint->parent_->children_.begin(), _pJoint->parent_->children_.end(), _pJoint) != _pJoint->parent_->children_.end())       // remove the joint from its parent
      _pJoint->parent_->children_.resize(_pJoint->parent_->children_.size() - 1);
  }

  typename std::vector<Joint*>::iterator it = joints_.begin() + _pJoint->id(); // iterator pointing to the element that has to be erased
  it = joints_.erase(it);                                                      // erase the element
  for(; it != joints_.end(); ++it)                                             // for all following elements
    (*it)->setId((*it)->id() - 1);                                             // reduce their index by one (since they have been moved there)

  referencePose_.updateFromGlobal(0, true);
  for (typename std::vector<Animation*>::iterator a_it = animations_.begin(); a_it != animations_.end(); ++a_it) {
    if (*a_it)
      (*a_it)->updateFromGlobal(0);
  }
}

//-----------------------------------------------------------------------------

template<typename PointT>
inline void SkeletonT<PointT>::clear()
{
  // no joints, so no animation either
  clean_properties();
  clearAnimations();

  // clear the joints
  typename std::vector<Joint*>::iterator it;
  for(it = joints_.begin(); it != joints_.end(); ++it)
    delete *it;

  joints_.clear();
  names_.clear();
}

//-----------------------------------------------------------------------------

template<typename PointT>
inline typename SkeletonT<PointT>::Joint *SkeletonT<PointT>::root()
{
  if(joints_.empty())
    return 0;
  return joints_[0];
}

//-----------------------------------------------------------------------------

template<typename PointT>
inline typename SkeletonT<PointT>::Joint *SkeletonT<PointT>::joint(const unsigned int& _index)
{
  if(_index >= joints_.size())
    return 0;
  return joints_[_index];
}

//-----------------------------------------------------------------------------

template<typename PointT>
int SkeletonT<PointT>::parent(unsigned int _joint)
{
  if(joints_[_joint]->parent() == 0)
    return -1;
  return joints_[_joint]->parent()->id();
}

//-----------------------------------------------------------------------------

template<typename PointT>
unsigned int SkeletonT<PointT>::childCount(unsigned int _joint)
{
  if ( _joint >= joints_.size() ){
    std::cerr << "SkeletonT : childCount() called with non-existing joint " << _joint << std::endl;
    return 0;
  }

  return joints_[_joint]->size();
}

//-----------------------------------------------------------------------------

template<typename PointT>
unsigned int SkeletonT<PointT>::child(unsigned int _joint, unsigned int _child)
{
  return joints_[_joint]->child(_child)->id();
}

//-----------------------------------------------------------------------------

template<typename PointT>
unsigned int SkeletonT<PointT>::jointCount()
{
  return joints_.size();
}

//-----------------------------------------------------------------------------

template<typename PointT>
typename SkeletonT<PointT>::Iterator SkeletonT<PointT>::begin()
{
  return Iterator(root());
}

//-----------------------------------------------------------------------------

template<typename PointT>
typename SkeletonT<PointT>::Iterator SkeletonT<PointT>::end()
{
  // why return Iterator(0)? operator= will compare the current pointer, and 0 is used once the iterator
  // passed the last joint
  return Iterator(0);
}


//-----------------------------------------------------------------------------

template<typename PointT>
inline typename SkeletonT<PointT>::Pose* SkeletonT<PointT>::pose(const AnimationHandle &_hAni)
{
  if(_hAni.isValid() && _hAni.animationIndex() < animations_.size() && animations_[_hAni.animationIndex()] != NULL)
    return animations_[_hAni.animationIndex()]->pose(_hAni.frame());
  else
    return &referencePose_;
}

//-----------------------------------------------------------------------------

template<typename PointT>
inline typename SkeletonT<PointT>::Pose* SkeletonT<PointT>::referencePose()
{
  return &referencePose_;
}

//-----------------------------------------------------------------------------

template<typename PointT>
AnimationHandle SkeletonT<PointT>::addAnimation(std::string _name, Animation *_animation)
{
  // try to find an unused animation slot first
  typename std::vector<Animation*>::iterator f;
  for(f = animations_.begin(); f != animations_.end(); ++f)
    if(*f == 0)
      break;

  if(f == animations_.end())
  {
    // all in use, append
    names_.insert( std::pair<std::string, unsigned int>(_name, animations_.size()) );
    animations_.push_back(_animation);
  }else{
    // found an empty one, use it
    names_.insert( std::pair<std::string, unsigned int>(_name, f - animations_.begin()) );
    *f = _animation;
  }

  if (_animation)
    _animation->setName(_name);

  return AnimationHandle(names_[_name]);
}

//-----------------------------------------------------------------------------

template<typename PointT>
AnimationHandle SkeletonT<PointT>::cloneAnimation(std::string _name, const AnimationHandle &_hAni)
{
  // try to find an unused animation slot first
  typename std::vector<Animation*>::iterator f;
  for(f = animations_.begin(); f != animations_.end(); ++f)
    if(*f == 0)
      break;

  if(f == animations_.end())
  {
    // all in use, append
    names_.insert( std::pair<std::string, unsigned int>(_name, animations_.size()) );
    if(animation(_hAni) != 0)
      animations_.push_back((*animation(_hAni)).copy());
    else
      animations_.push_back(new FrameAnimationT<Point>(referencePose_));
  }else{
    // found an empty one, use it
    names_.insert( std::pair<std::string, unsigned int>(_name, f - animations_.begin()) );
    if(animation(_hAni) != 0)
      *f = (*animation(_hAni)).copy();
    else
      *f = new FrameAnimationT<Point>(referencePose_);
  }

  return AnimationHandle(names_[_name]);
}

//-----------------------------------------------------------------------------

template<typename PointT>
AnimationHandle SkeletonT<PointT>::animationHandle(std::string _name)
{
  std::map<std::string, unsigned int>::iterator f = names_.find(_name);
  if(f == names_.end())
    return AnimationHandle();

  return AnimationHandle(f->second);
}

//-----------------------------------------------------------------------------

template<typename PointT>
typename SkeletonT<PointT>::Animation *SkeletonT<PointT>::animation(std::string _name)
{
  std::map<std::string, unsigned int>::iterator f = names_.find(_name);
  if(f == names_.end())
    return 0;

  return animations_[f->second];
}

//-----------------------------------------------------------------------------

template<typename PointT>
typename SkeletonT<PointT>::Animation *SkeletonT<PointT>::animation(const AnimationHandle &_hAni)
{
  if(!_hAni.isValid())
    return 0;
  return animations_[_hAni.animationIndex()];
}

//-----------------------------------------------------------------------------

template<typename PointT>
void SkeletonT<PointT>::removeAnimation(std::string _name)
{
  // get an iterator for the animation
  std::map<std::string, unsigned int>::iterator f = names_.find(_name);
  if(f == names_.end())
    return;

  // delete the animation
  delete animations_[f->second];
  animations_[f->second] = 0;
  // remove the name entry
  names_.erase(f);
}

//-----------------------------------------------------------------------------

template<typename PointT>
void SkeletonT<PointT>::removeAnimation(const AnimationHandle &_hAni)
{
  // delete the animation
  delete animations_[_hAni.animationIndex()];
  animations_[_hAni.animationIndex()] = 0;

  // remove the name entry
  for(typename std::map<std::string, unsigned int>::iterator it = names_.begin(); it != names_.end(); ++it)
  {
    if(it->second == _hAni.animationIndex())
    {
      names_.erase(it);
      break;
    }
  }
}

//-----------------------------------------------------------------------------

template<typename PointT>
void SkeletonT<PointT>::clearAnimations()
{
  names_.clear();

  for(typename std::vector<Animation*>::iterator it = animations_.begin(); it != animations_.end(); ++it)
    delete *it;
  animations_.clear();
}

//-----------------------------------------------------------------------------

template<typename PointT>
typename SkeletonT<PointT>::AnimationIterator SkeletonT<PointT>::animationsBegin()
{
  return AnimationIterator(animations_);
}

//-----------------------------------------------------------------------------

template<typename PointT>
typename SkeletonT<PointT>::AnimationIterator SkeletonT<PointT>::animationsEnd()
{

  return AnimationIterator(animations_.size());
}


//-----------------------------------------------------------------------------

template<typename PointT>
unsigned int SkeletonT<PointT>::animationCount()
{
  return names_.size();
}

//-----------------------------------------------------------------------------

template<typename PointT>
const std::string &SkeletonT<PointT>::animationName(unsigned int _index)
{
  unsigned int i = 0;
  std::map<std::string, unsigned int>::iterator pos = names_.begin();

  while(pos->second != _index && pos != names_.end())
  {
    ++i;
    ++pos;
  }

  return pos->first;
}

//-----------------------------------------------------------------------------

template<typename PointT>
void SkeletonT<PointT>::updateFromGlobal(unsigned int _idJoint)
{
  referencePose_.updateFromGlobal(_idJoint);
  for(typename std::vector<Animation*>::iterator it = animations_.begin(); it != animations_.end(); ++it) {
    if (*it)
      (*it)->updateFromGlobal(_idJoint);
  }
}

//-----------------------------------------------------------------------------

template<typename PointT>
void SkeletonT<PointT>::insertJoint(typename SkeletonT<PointT>::Joint *_pChild, typename SkeletonT<PointT>::Joint *_pInsert)
{
        if (!_pChild || !_pChild->parent() || !_pInsert)
                return;

        Joint* parent = _pChild->parent();

        //update IDs of our joints
        unsigned int childID = _pChild->id();
        for(typename std::vector<Joint*>::iterator it = joints_.begin() + childID; it !=  joints_.end(); ++it)
                (*it)->setId((*it)->id() + 1);

        //insert our new joint into this skeleton
        joints_.insert(joints_.begin() + childID, _pInsert);
        _pInsert->setId(childID);

        //update the parents
        //note: pChild will be automatically erased in parent->children_
        _pInsert->setParent(parent, *this);
        _pChild->setParent(_pInsert, *this);

        insert_property_at(childID);

        referencePose_.insertJointAt(childID);
        for(typename std::vector<Animation*>::iterator it = animations_.begin(); it != animations_.end(); ++it)
                if (*it)
                        (*it)->insertJointAt(childID);

        referencePose_.updateFromGlobal(0, true);
}

//-----------------------------------------------------------------------------