PoseT_impl.hh

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



#define POSET_C

#include <cassert>

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

template<typename PointT>
PoseT<PointT>::PoseT(SkeletonT<PointT>* _skeleton) : skeleton_(_skeleton)
{
  assert(_skeleton != 0);

  // add joints until we have the same size as the reference pose
  while( skeleton_->jointCount() > local_.size() )
    insertJointAt( local_.size() );
}

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

template<typename PointT>
PoseT<PointT>::PoseT(const PoseT<PointT> &_other) : skeleton_(_other.skeleton_)
{
  local_.insert(local_.begin(), _other.local_.begin(), _other.local_.end());
  global_.insert(global_.begin(), _other.global_.begin(), _other.global_.end());
  unified_.insert(unified_.begin(), _other.unified_.begin(), _other.unified_.end());
  unifiedDualQuaternion_.insert(unifiedDualQuaternion_.begin(), _other.unifiedDualQuaternion_.begin(), _other.unifiedDualQuaternion_.end());
}

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

template<typename PointT>
PoseT<PointT>::~PoseT()
{

}

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

template<typename PointT>
inline const typename PoseT<PointT>::Matrix& PoseT<PointT>::localMatrix(unsigned int _joint) const
{
  return local_[_joint];
}

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

template<typename PointT>
void PoseT<PointT>::setLocalMatrix(unsigned int _joint, const Matrix& _local, bool _keepLocalChildPositions)
{
  local_[_joint] = _local;

  updateFromLocal(_joint, _keepLocalChildPositions);
}

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

template<typename PointT>
inline typename PoseT<PointT>::Vector PoseT<PointT>::localTranslation(unsigned int _joint)
{
  Vector ret;
  Matrix &mat = local_[_joint];
  for(int i = 0; i < 3; ++i)
    ret[i] = mat(i, 3);
  return ret;
}

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

template<typename PointT>
void PoseT<PointT>::setLocalTranslation(unsigned int _joint, const Vector &_position, bool _keepLocalChildPositions)
{
  Matrix &mat = local_[_joint];
  for(int i = 0; i < 3; ++i)
    mat(i, 3) = _position[i];

  updateFromLocal(_joint, _keepLocalChildPositions);
}

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

template<typename PointT>
typename PoseT<PointT>::Matrix PoseT<PointT>::localMatrixInv(unsigned int _joint) const
{
  Matrix ret = local_[_joint];
  ret.invert();
  return ret;
}

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

template<typename PointT>
inline const typename PoseT<PointT>::Matrix& PoseT<PointT>::globalMatrix(unsigned int _joint) const
{
  return global_[_joint];
}

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

template<typename PointT>
void PoseT<PointT>::setGlobalMatrix(unsigned int _joint, const Matrix &_global, bool _keepGlobalChildPositions)
{
  global_[_joint] = _global;

  updateFromGlobal(_joint, _keepGlobalChildPositions);
}

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

template<typename PointT>
inline typename PoseT<PointT>::Vector PoseT<PointT>::globalTranslation(unsigned int _joint)
{
  Vector ret;
  Matrix &mat = global_[_joint];
  for(int i = 0; i < 3; ++i)
    ret[i] = mat(i, 3);
  return ret;
}

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

template<typename PointT>
void PoseT<PointT>::setGlobalTranslation(unsigned int _joint, const Vector &_position, bool _keepGlobalChildPositions)
{
  Matrix &mat = global_[_joint];
  for(int i = 0; i < 3; ++i)
    mat(i, 3) = _position[i];

  updateFromGlobal(_joint, _keepGlobalChildPositions);
}

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

template<typename PointT>
typename PoseT<PointT>::Matrix PoseT<PointT>::globalMatrixInv(unsigned int _joint) const
{
  if (_joint >= global_.size()) {
    std::cerr << "Illegal joint number: " << _joint << std::endl;
    return global_[0];
  }
  Matrix ret = global_[_joint];
  ret.invert();
  return ret;
}

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

template<typename PointT>
void PoseT<PointT>::insertJointAt(size_t _index)
{
  Matrix id;
  id.identity();

  DualQuaternion idDQ;
  idDQ.identity();

  local_.insert(local_.begin() + _index, id);
  global_.insert(global_.begin() + _index, id);
  unified_.insert(unified_.begin() + _index, id);
  unifiedDualQuaternion_.insert(unifiedDualQuaternion_.begin() + _index, idDQ);
}

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

template<typename PointT>
void PoseT<PointT>::removeJointAt(size_t _index)
{
  local_.erase(local_.begin() + _index);
  global_.erase(global_.begin() + _index);
  unified_.erase(unified_.begin() + _index);
  unifiedDualQuaternion_.erase(unifiedDualQuaternion_.begin() + _index);
}

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

template<typename PointT>
void PoseT<PointT>::updateFromLocal(size_t _joint, bool _keepChildPositions)
{
  // first update the global coordinate system
  if(skeleton_->parent(_joint) == -1)
    global_[_joint] = local_[_joint];
  else
    global_[_joint] = globalMatrix(skeleton_->parent(_joint)) * localMatrix(_joint);

  // update the unified matrices
  Matrix matRefGlobalInv = skeleton_->referencePose()->globalMatrix(_joint);
  matRefGlobalInv.invert();

  unified_[_joint] = globalMatrix(_joint) * matRefGlobalInv;
  unifiedDualQuaternion_[_joint] = DualQuaternion(unified_[_joint]);

  // update children
  if (_keepChildPositions) {
    // finally update all children as well
    for(size_t i = 0; i < skeleton_->childCount(_joint); ++i) {
      updateFromLocal(skeleton_->child(_joint, i));
    }
  } else {
    updateFromGlobal(_joint, true);    //this will adjust the childrens' positions according to the _joint position.
  }
}

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

template<typename PointT>
void PoseT<PointT>::updateFromGlobal(size_t _joint, bool _keepChildPositions)
{
  // first update the local coordinate system
  if(skeleton_->parent(_joint) == -1)
    local_[_joint] = global_[_joint];
  else
    local_[_joint] = globalMatrixInv(skeleton_->parent(_joint)) * globalMatrix(_joint);

  // update the unified matrices
  Matrix matRefGlobalInv = skeleton_->referencePose()->globalMatrix(_joint);
  matRefGlobalInv.invert();

  unified_[_joint] = globalMatrix(_joint) * matRefGlobalInv;
  unifiedDualQuaternion_[_joint] = DualQuaternion(unified_[_joint]);

  // update children
  if (_keepChildPositions) {
    for(size_t i = 0; i < skeleton_->childCount(_joint); ++i) {
      updateFromGlobal(skeleton_->child(_joint, i));
    }
  } else {
    updateFromLocal(_joint, true);    //this will adjust the childrens' positions according to the _joint position.
  }
}

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

template<typename PointT>
inline const typename PoseT<PointT>::Matrix& PoseT<PointT>::unifiedMatrix(size_t _joint)
{
  return unified_[_joint];
}

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

template<typename PointT>
inline const typename PoseT<PointT>::Quaternion& PoseT<PointT>::unifiedRotation(size_t _joint)
{
  return unifiedDualQuaternion_[_joint].real();
}

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

template<typename PointT>
inline const typename PoseT<PointT>::DualQuaternion& PoseT<PointT>::unifiedDualQuaternion(size_t _joint)
{
  return unifiedDualQuaternion_[_joint];
}

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