ElementInterpolation.h

Go to the documentation of this file.

#pragma once
 
#include <SpaceVecAlg/SpaceVecAlg>
 
namespace TrajColl
{
template<class T>
inline T interpolate(const T & start, const T & end, double ratio)
{
  return (1 - ratio) * start + ratio * end;
}
 
template<>
inline Eigen::Quaterniond interpolate(const Eigen::Quaterniond & start, const Eigen::Quaterniond & end, double ratio)
{
  return start.slerp(ratio, end);
}
 
template<>
inline Eigen::Matrix3d interpolate(const Eigen::Matrix3d & start, const Eigen::Matrix3d & end, double ratio)
{
  return interpolate<Eigen::Quaterniond>(Eigen::Quaterniond(start), Eigen::Quaterniond(end), ratio).toRotationMatrix();
}
 
template<>
inline sva::PTransformd interpolate(const sva::PTransformd & start, const sva::PTransformd & end, double ratio)
{
  return sva::interpolate(start, end, ratio);
}
 
template<class T, class U = T>
inline U interpolateDerivative(const T & start,
                               const T & end,
                               double, // ratio
                               int order = 1)
{
  if(order == 1)
  {
    return end - start;
  }
  else
  {
    T ret = start; // Dummy initialization for dynamic size class
    ret.setZero();
    return ret;
  }
}
 
template<>
inline double interpolateDerivative(const double & start,
                                    const double & end,
                                    double, // ratio
                                    int order)
{
  if(order == 1)
  {
    return end - start;
  }
  else
  {
    return 0.0;
  }
}
 
template<>
inline Eigen::Vector3d interpolateDerivative(const Eigen::Quaterniond & start,
                                             const Eigen::Quaterniond & end,
                                             double, // ratio
                                             int order)
{
  if(order == 1)
  {
    Eigen::AngleAxisd aa(start.inverse() * end);
    return aa.angle() * aa.axis();
  }
  else
  {
    return Eigen::Vector3d::Zero();
  }
}
 
template<>
inline Eigen::Vector3d interpolateDerivative(const Eigen::Matrix3d & start,
                                             const Eigen::Matrix3d & end,
                                             double, // ratio
                                             int order)
{
  if(order == 1)
  {
    Eigen::AngleAxisd aa(start.transpose() * end);
    return aa.angle() * aa.axis();
  }
  else
  {
    return Eigen::Vector3d::Zero();
  }
}
 
template<>
inline sva::MotionVecd interpolateDerivative(const sva::PTransformd & start,
                                             const sva::PTransformd & end,
                                             double, // ratio
                                             int order)
{
  if(order == 1)
  {
    return sva::transformError(start, end);
  }
  else
  {
    return sva::MotionVecd::Zero();
  }
}
 
template<>
inline sva::ForceVecd interpolateDerivative(const sva::ForceVecd & start,
                                            const sva::ForceVecd & end,
                                            double, // ratio
                                            int order)
{
  if(order == 1)
  {
    return end - start;
  }
  else
  {
    return sva::ForceVecd::Zero();
  }
}
} // namespace TrajColl