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



//=============================================================================
//
//  MyTypes
//
//=============================================================================

#define SKELETONOBJECT_C

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

#include <string>
#include <sstream>
#include <ACG/Scenegraph/TextNode.hh>
#include <ACG/Scenegraph/LineNode.hh>
#include <OpenFlipper/common/Types.hh>
#include "Skeleton.hh"

#include <ACG/Scenegraph/MaterialNode.hh>
#include <ACG/Scenegraph/SeparatorNode.hh>

//== DEFINES ==================================================================

//== TYPEDEFS =================================================================

//== CLASS DEFINITION =========================================================

SkeletonObject::SkeletonObject( ) :
  BaseObjectData( ),
  skeleton_(NULL),
  skeletonNode_(NULL)
{
  setDataType(DATA_SKELETON);
  init();
}

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

SkeletonObject::SkeletonObject( const SkeletonObject &_other ) :
  BaseObjectData(_other)
{
  init(_other.skeleton_);

  setName(name());
}

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

SkeletonObject::~SkeletonObject()
{
  // Delete the data attached to this object ( this will remove all perObject data)
  // Not the best way to do it but it will work.
  // This is only necessary if people use references to the skeleton below and
  // they do something with the skeleton in the destructor of their
  // perObjectData.
  deleteData();

  // Delete the Skeleton only, if this object contains a mesh
  if ( skeleton_ != NULL)  {
    delete skeleton_;
    skeleton_ = NULL;
  } else {
    std::cerr << "Destructor error : Skeleton already deleted" << std::endl;
  }

  // No need to delete the scenegraph Nodes as this will be managed by baseplugin
  skeletonNode_    = NULL;
}

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

void SkeletonObject::cleanup() {
  // Delete the Skeleton only, if this object contains a line
  if ( skeleton_ != NULL)  {
    delete skeleton_;
    skeleton_ = NULL;
  } else {
    std::cerr << "Cleanup error : Skeleton already deleted" << std::endl;
  }

  BaseObjectData::cleanup();

  skeletonNode_   = NULL;

  setDataType(DATA_SKELETON);

  init();

}

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

void SkeletonObject::init() {
  skeleton_ = new Skeleton();

  if ( materialNode() == NULL)
    std::cerr << "Error when creating Skeleton Object! materialNode is NULL!" << std::endl;

  skeletonNode_    = new ACG::SceneGraph::SkeletonNodeT< Skeleton >(*skeleton_, materialNode() , "NEW SkeletonNode");
  skeletonNode_->drawMode( ACG::SceneGraph::DrawModes::SOLID_FLAT_SHADED | ACG::SceneGraph::DrawModes::POINTS );
  
  //set defaults for the material
  materialNode()->set_color(ACG::Vec4f(0.654f, 0.8f, 1.0f, 1.0f));
  materialNode()->set_point_size(8.0);
  materialNode()->set_line_width(3.0);
  materialNode()->set_round_points(true);

}

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

void SkeletonObject::init(Skeleton *_pSkeleton)
{
  skeleton_ = new Skeleton(*_pSkeleton);

  if(materialNode() == NULL)
    std::cerr << "Error when creating Skeleton Object! materialNode is NULL!" << std::endl;

  skeletonNode_ = new ACG::SceneGraph::SkeletonNodeT<Skeleton>(*skeleton_, materialNode(), "NEW SkeletonNode");
  skeletonNode_->drawMode( ACG::SceneGraph::DrawModes::SOLID_FLAT_SHADED | ACG::SceneGraph::DrawModes::POINTS );

  //set defaults for the material
  materialNode()->set_color(ACG::Vec4f(0.654f, 0.8f, 1.0f, 1.0f));
  materialNode()->set_point_size(8.0);
  materialNode()->set_line_width(3.0);
  materialNode()->set_round_points(true);
}

// ===============================================================================
// Name/Path Handling
// ===============================================================================

void SkeletonObject::setName( QString _name ) {
  BaseObjectData::setName(_name);

  std::string nodename = std::string("SkeletonNode for Skeleton "     + _name.toUtf8() );
  skeletonNode_->name( nodename );
}

// ===============================================================================
// Content
// ===============================================================================

Skeleton* SkeletonObject::skeleton() {
  return skeleton_;
}

// ===============================================================================
// Visualization
// ===============================================================================

ACG::SceneGraph::SkeletonNodeT< Skeleton >* SkeletonObject::skeletonNode() {
  return skeletonNode_;
}

// ===============================================================================
// Object information
// ===============================================================================

QString SkeletonObject::getObjectinfo() {
  QString output;

  output += "========================================================================\n";
  output += BaseObjectData::getObjectinfo();

  if ( dataType( DATA_SKELETON ) )
    output += "Object Contains a Skeleton : ";

        output += QString::number( skeleton()->jointCount()) + " joints.\n";

  output += "========================================================================\n";
  return output;
}

// ===============================================================================
// Picking
// ===============================================================================

bool SkeletonObject::picked( uint _node_idx ) {
  return ( _node_idx == skeletonNode_->id() );
}

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

void SkeletonObject::enablePicking( bool _enable ) {
  skeletonNode_->enablePicking( _enable );
}

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

bool SkeletonObject::pickingEnabled() {
  return skeletonNode_->pickingEnabled();
}

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

AnimationHandle SkeletonObject::activePose() {
  return skeletonNode()->activePose();
}

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

void SkeletonObject::setActivePose(const AnimationHandle &_hAni)
{
  skeletonNode()->setActivePose(_hAni);

  updateIndices(_hAni);
  updateMotionPath(_hAni);
}

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

void SkeletonObject::updateIndices()
{
  updateIndices(skeletonNode()->activePose());
}

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

void SkeletonObject::updateIndices(const AnimationHandle &_hAni)
{
  ACG::SceneGraph::MaterialNode *pMatNode;
  if(!getAdditionalNode(pMatNode, "SkeletonPlugin", "Text material node"))
    return;

  // update the position of existing nodes
  PoseT<OpenMesh::Vec3d> *pose = skeleton_->pose(_hAni);
  for(unsigned int i = 0; i < skeleton_->jointCount(); ++i)
  {
    std::stringstream buf;
    std::string nameTransformNode;

    buf << "TextNode " << i << " Transform";
    nameTransformNode = buf.str();

    ACG::SceneGraph::TransformNode *pTransNode;
    if(!getAdditionalNode(pTransNode, "SkeletonPlugin", nameTransformNode.c_str()))
      continue;

    pTransNode->loadIdentity();
    pTransNode->translate(pose->globalTranslation(i));
    pTransNode->scale(skeletonNode_->frameSize()*0.5);
  }

  // find and prune redundant nodes
  unsigned int i = skeleton_->jointCount();
  bool bAdditionalNodes = true;
  do{
    std::stringstream buf;
    std::string nameTransformNode;

    buf << "TextNode " << i << " Transform";
    nameTransformNode = buf.str();

    ACG::SceneGraph::TransformNode *pTransNode;
    if(getAdditionalNode(pTransNode, "SkeletonPlugin", nameTransformNode.c_str()))
    {
      removeAdditionalNode(pTransNode, "SkeletonPlugin", nameTransformNode.c_str());
      ++i;
      continue;
    }
    bAdditionalNodes = false;
  }while(bAdditionalNodes);
}

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

void SkeletonObject::showIndices(bool _bVisible)
{
  ACG::SceneGraph::MaterialNode *pMatNode;
  if(!getAdditionalNode(pMatNode, "SkeletonPlugin", "Text material node"))
  {
    pMatNode = new ACG::SceneGraph::MaterialNode(baseNode(), "Text material node");
    addAdditionalNode(pMatNode, "SkeletonPlugin", "Text material node");
  }
  
  pMatNode->set_color(ACG::Vec4f(0, 0, 0, 1.0));
  if(_bVisible)
    pMatNode->show();
  else
    pMatNode->set_status(ACG::SceneGraph::BaseNode::HideSubtree);

  // find and prune redundant nodes
  unsigned int i = skeleton_->jointCount();
  bool bAdditionalNodes = true;
  do{
    std::stringstream buf;
    std::string nameTransformNode;

    buf << "TextNode " << i << " Transform";
    nameTransformNode = buf.str();

    ACG::SceneGraph::TransformNode *pTransNode;
    if(getAdditionalNode(pTransNode, "SkeletonPlugin", nameTransformNode.c_str()))
    {
      removeAdditionalNode(pTransNode, "SkeletonPlugin", nameTransformNode.c_str());
      ++i;
      continue;
    }
    bAdditionalNodes = false;
  }while(bAdditionalNodes);

  // add or update nodes
  PoseT<OpenMesh::Vec3d>* ref = skeleton_->referencePose();
  for(unsigned int i = 0; i < skeleton_->jointCount(); ++i)
  {
    std::stringstream buf;
    std::string nameJoint, nameTextNode, nameTransformNode, nameSubMatNode;

    buf.str("");
    buf << i;
    nameJoint = buf.str();

    buf.str("");
    buf << "TextNode " << i;
    nameTextNode = buf.str();

    buf << " Transform";
    nameTransformNode = buf.str();

    buf << " Material";
    nameSubMatNode = buf.str();

    ACG::SceneGraph::TransformNode *pTransNode;
    if(!getAdditionalNode(pTransNode, "SkeletonPlugin", nameTransformNode.c_str()))
    {
      pTransNode = new ACG::SceneGraph::TransformNode(pMatNode, nameTransformNode.c_str());
      addAdditionalNode(pTransNode, "SkeletonPlugin", nameTransformNode.c_str());
    }
    pTransNode->loadIdentity();
    pTransNode->translate(ref->globalTranslation(i));
    pTransNode->scale(skeletonNode_->frameSize()*0.5);

    ACG::SceneGraph::MaterialNode *pSubMatNode;
    if(!getAdditionalNode(pSubMatNode, "SkeletonPlugin", nameSubMatNode.c_str()))
    {
      pSubMatNode = new ACG::SceneGraph::MaterialNode(pTransNode, nameSubMatNode.c_str());
      addAdditionalNode(pSubMatNode, "SkeletonPlugin", nameSubMatNode.c_str());
    }
    //     pSubMatNode->set_color(skeleton_->joint(i)->color());

    ACG::SceneGraph::TextNode *pTextNode = NULL;
    if(!getAdditionalNode(pTextNode, "SkeletonPlugin", nameTextNode.c_str()))
    {
      pTextNode = new ACG::SceneGraph::TextNode(pSubMatNode,
                                                nameTextNode.c_str(),
                                                ACG::SceneGraph::TextNode::SCREEN_ALIGNED,
                                                true);
          addAdditionalNode(pTextNode, "SkeletonPlugin", nameTextNode.c_str());
    }
    pTextNode->setText("  " + nameJoint);
    pTextNode->setSize(1);
    pTextNode->multipassNodeSetActive(8, true);
  }
  
  //update the indices with the current animationhandle
  updateIndices( skeletonNode()->activePose() );
}

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

bool SkeletonObject::indicesVisible()
{
  ACG::SceneGraph::MaterialNode *pMatNode;
  if(!getAdditionalNode(pMatNode, "SkeletonPlugin", "Text material node"))
    return false;
  
  return ( pMatNode->status() == ACG::SceneGraph::BaseNode::Active );
}

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

bool SkeletonObject::motionPathVisible()
{
  ACG::SceneGraph::MaterialNode *pMatNode;
  if(!getAdditionalNode(pMatNode, "SkeletonPlugin", "Motion material node"))
    return false;
  
  return ( pMatNode->status() == ACG::SceneGraph::BaseNode::Active );
}

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

void SkeletonObject::updateMotionPath()
{
  updateMotionPath(skeletonNode()->activePose());
}

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

void SkeletonObject::updateMotionPath(const AnimationHandle &_hAni)
{

  ACG::SceneGraph::MaterialNode *pMatNode;
  if(!getAdditionalNode(pMatNode, "SkeletonPlugin", "Motion material node"))
    return;

  // update the position of existing nodes
  PoseT<OpenMesh::Vec3d>* pose = skeleton_->pose(_hAni);

  for(unsigned int i = 0; i < skeleton_->jointCount(); ++i)
  {

    std::stringstream buf;
    std::string nameTransformNode;

    buf << "LineNode " << i << " Transform";
    nameTransformNode = buf.str();

    ACG::SceneGraph::TransformNode *pTransNode;
    if(!getAdditionalNode(pTransNode, "SkeletonPlugin", nameTransformNode.c_str()))
      continue;

    pTransNode->loadIdentity();
    pTransNode->translate( pose->globalTranslation(i) );
  }

  // find and prune redundant nodes
  unsigned int i = skeleton_->jointCount();
  bool bAdditionalNodes = true;
  do{
    std::stringstream buf;
    std::string nameTransformNode;

    buf << "LineNode " << i << " Transform";
    nameTransformNode = buf.str();

    ACG::SceneGraph::TransformNode *pTransNode;
    if(getAdditionalNode(pTransNode, "SkeletonPlugin", nameTransformNode.c_str()))
    {
      removeAdditionalNode(pTransNode, "SkeletonPlugin", nameTransformNode.c_str());
      ++i;
      continue;
    }
    bAdditionalNodes = false;
  }while(bAdditionalNodes);
}

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

void SkeletonObject::showMotionPath(bool _visible)
{
  ACG::SceneGraph::MaterialNode *pMatNode;
  if(!getAdditionalNode(pMatNode, "SkeletonPlugin", "Motion material node"))
  {
    pMatNode = new ACG::SceneGraph::MaterialNode(baseNode(), "Motion material node");
    addAdditionalNode(pMatNode, "SkeletonPlugin", "Motion material node");
  }
  pMatNode->set_color(ACG::Vec4f(0, 0, 0, 1.0));

  if(_visible)
    pMatNode->show();
  else
    pMatNode->set_status(ACG::SceneGraph::BaseNode::HideSubtree);

  // find and prune redundant nodes
  unsigned int i = skeleton_->jointCount();
  bool bAdditionalNodes = true;
  do{
    std::stringstream buf;
    std::string nameTransformNode;

    buf << "LineNode " << i << " Transform";
    nameTransformNode = buf.str();

    ACG::SceneGraph::TransformNode *pTransNode;
    if(getAdditionalNode(pTransNode, "SkeletonPlugin", nameTransformNode.c_str()))
    {
      removeAdditionalNode(pTransNode, "SkeletonPlugin", nameTransformNode.c_str());
      ++i;
      continue;
    }
    bAdditionalNodes = false;
  }while(bAdditionalNodes);


  // add or update nodes
  PoseT<OpenMesh::Vec3d>* ref = skeleton_->referencePose();

  for(unsigned int i = 0; i < skeleton_->jointCount(); ++i)
  {
    std::stringstream buf;
    std::string nameLineNode, nameTransformNode;

    buf.str("");
    buf << "LineNode " << i;
    nameLineNode = buf.str();

    buf << " Transform";
    nameTransformNode = buf.str();

    ACG::SceneGraph::TransformNode *pTransNode;
    if(!getAdditionalNode(pTransNode, "SkeletonPlugin", nameTransformNode.c_str()))
    {
      pTransNode = new ACG::SceneGraph::TransformNode(pMatNode, nameTransformNode.c_str());
      addAdditionalNode(pTransNode, "SkeletonPlugin", nameTransformNode.c_str());
    }
    pTransNode->loadIdentity();
    ACG::GLMatrixd mat( ref->globalMatrix(i).get_raw_data() );
    pTransNode->scale(mat); //this is just a hack to set the global matrix

    ACG::SceneGraph::LineNode *pLineNode = NULL;
    if(!getAdditionalNode(pLineNode, "SkeletonPlugin", nameLineNode.c_str()))
    {
      pLineNode = new ACG::SceneGraph::LineNode( ACG::SceneGraph::LineNode::LineSegmentsMode, pTransNode, nameLineNode.c_str());
      addAdditionalNode(pLineNode, "SkeletonPlugin", nameLineNode.c_str());
    }
    pLineNode->clear();

    Skeleton::Joint* joint = skeleton_->joint(i);

    if ( joint->size() > 0 ){

      for (uint c=0; c < joint->size(); c++ ){
        Skeleton::Joint* child = joint->child(c);

        for (unsigned int a = 0; a < skeleton_->animationCount(); a++){

          AnimationHandle animHandle = AnimationHandle(a, 0 );

          //get animation
          AnimationT<ACG::Vec3d>* animation = skeleton_->animation( animHandle );

          if (animation->name() == "")
            continue;

          ACG::Vec3d lastPos(0.0, 0.0, 0.0);

          // every frame of that animation
          for(unsigned int k = 0; k < animation->frameCount(); ++k){
            animHandle.setFrame(k);
            Skeleton::Pose* pose = skeleton_->pose( animHandle );

            ACG::Matrix4x4d globalInv = pose->globalMatrix( joint->id() );
            globalInv.invert();

            ACG::Matrix4x4d local   = pose->globalMatrix(child->id());
            ACG::Matrix4x4d unified = pose->unifiedMatrix( joint->id() );

            ACG::Vec3d trans = pose->globalTranslation(child->id()) - pose->globalTranslation(joint->id());
            trans *= 0.5;

            pLineNode->add_line( ACG::Vec3d(0.0, 0.0, 0.0), trans );
            pLineNode->add_color( ACG::Vec3uc(180,255,180));

            pLineNode->add_line( lastPos, trans );
            pLineNode->add_color( ACG::Vec3uc(110,160,110));

            lastPos = trans;
          }
        }

        ACG::Vec3d trans = ref->globalTranslation(child->id())-ref->globalTranslation(joint->id());
        trans *= 0.6;

        pLineNode->add_line( ACG::Vec3d(0.0, 0.0, 0.0), trans);
        pLineNode->add_color( ACG::Vec3uc(255,0,0));
      }
    }
  }
  
  //update the indices with the current animationhandle
  updateMotionPath( skeletonNode_->activePose() );
}

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

BaseObject* SkeletonObject::copy()
{
  SkeletonObject *pObject = new SkeletonObject(*this);
  return dynamic_cast<BaseObject*>(pObject);
}

// ===============================================================================
// Update
// ===============================================================================


void SkeletonObject::update(UpdateType _type) {
  BaseObject::update(_type);
}

//=============================================================================