Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
#include "toonz/ikengine.h"
Toshihiro Shimizu 890ddd
#include "toonz/ikjacobian.h"
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
enum Method { JACOB_TRANS, PURE_PSEUDO, DLS, SDLS, COMPARE };
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
IKEngine::IKEngine() {}
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
int IKEngine::addJoint(const TPointD &pos, int indexParent) {
Shinya Kitaoka 120a6e
  // TODO: evitare che si formino segmenti nulli
Shinya Kitaoka 120a6e
  // assert(m_joints.empty() || norm2(pos-m_joints.back())>0.000001);
Shinya Kitaoka 120a6e
  // assert(m_nodes[indexParent]);
Shinya Kitaoka 120a6e
  assert(m_skeleton.getNode(indexParent));
Shinya Kitaoka 120a6e
  m_skeleton.addNode(new IKNode());
Shinya Kitaoka 120a6e
  int index = m_skeleton.getNodeCount() - 1;
Shinya Kitaoka 120a6e
  m_skeleton.setNode(index, pos, IKNode::JOINT);
Shinya Kitaoka 120a6e
  m_skeleton.setParent(index, indexParent);
Shinya Kitaoka 120a6e
  return index;
Toshihiro Shimizu 890ddd
}
Shinya Kitaoka 120a6e
// la root deve coincidere con un punto bloccato!
Shinya Kitaoka 120a6e
void IKEngine::setRoot(const TPointD &pos) {
Shinya Kitaoka 120a6e
  m_skeleton.addNode(new IKNode());
Shinya Kitaoka 120a6e
  m_skeleton.setNode(0, pos, IKNode::JOINT);
Shinya Kitaoka 120a6e
  // m_skeleton.setParent(0,0);
Shinya Kitaoka 120a6e
  m_skeleton.setRoot(0);
Toshihiro Shimizu 890ddd
}
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
void IKEngine::lock(int index) {
Shinya Kitaoka 120a6e
  assert(index > -1 && index < m_skeleton.getNodeCount());
Shinya Kitaoka 120a6e
  m_skeleton.setPurpose(index, IKNode::EFFECTOR);
Shinya Kitaoka 120a6e
  IKNode *n   = m_skeleton.getNode(index);
Shinya Kitaoka 120a6e
  TPointD pos = n->getPos();
Shinya Kitaoka 120a6e
  target.push_back(pos);
Toshihiro Shimizu 890ddd
}
Shinya Kitaoka 120a6e
void IKEngine::unlock(int index) {
Shinya Kitaoka 120a6e
  assert(index > -1 && index < m_skeleton.getNodeCount());
Shinya Kitaoka 120a6e
  m_skeleton.setPurpose(index, IKNode::JOINT);
Toshihiro Shimizu 890ddd
}
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
bool IKEngine::isLocked(int index) {
Shinya Kitaoka 120a6e
  assert(index > -1 && index < m_skeleton.getNodeCount());
Shinya Kitaoka 120a6e
  return m_skeleton.getNode(index)->IsEffector();
Toshihiro Shimizu 890ddd
}
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
double IKEngine::getJointAngle(int index) {
Shinya Kitaoka 120a6e
  assert(index > -1 && index < m_skeleton.getNodeCount());
Shinya Kitaoka 120a6e
  TPointD pos = m_skeleton.getNode(index)->getPos();
Shinya Kitaoka 120a6e
  TPointD u(1, 0);
Shinya Kitaoka 120a6e
  if (index != 0) {
Shinya Kitaoka 120a6e
    int parent      = getJointParent(index);
Shinya Kitaoka 120a6e
    TPointD prevPos = m_skeleton.getNode(parent)->getPos();
Shinya Kitaoka 120a6e
    u               = normalize(pos - prevPos);
Shinya Kitaoka 120a6e
  }
Shinya Kitaoka 120a6e
  TPointD v(-u.y, u.x);
Shinya Kitaoka 120a6e
  TPointD nextPos = m_skeleton.getNode(index + 1)->getPos();
Shinya Kitaoka 120a6e
  TPointD d       = nextPos - pos;
Shinya Kitaoka 120a6e
  double theta    = atan2(d * v, d * u);
Shinya Kitaoka 120a6e
  return theta;
Toshihiro Shimizu 890ddd
}
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
void IKEngine::drag(TPointD &pos) {
Shinya Kitaoka 120a6e
  // assert(index>-1 && index
Shinya Kitaoka 120a6e
  // se lo scheletro รจ vuoto non succede nulla
Shinya Kitaoka 120a6e
  if (m_skeleton.getNodeCount() == 0) return;
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
  // afferro l'ultimo punto della catena
Shinya Kitaoka 120a6e
  int indexDrag = m_skeleton.getNodeCount() - 1;
Shinya Kitaoka 120a6e
  if (m_skeleton.getNode(indexDrag)->getParent()->IsEffector()) return;
Shinya Kitaoka 120a6e
  m_skeleton.setPurpose(indexDrag, IKNode::EFFECTOR);
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
  // assegno un indice alla sequenza dei giunti (nodi -end effectors)
Shinya Kitaoka 120a6e
  setSequenceJoints();
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
  target.push_back(pos);
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
  Jacobian jacobian(&m_skeleton, target);
Shinya Kitaoka 120a6e
  target.pop_back();
Shinya Kitaoka 120a6e
  for (int i = 0; i < 250; i++) doUpdateStep(jacobian);
Toshihiro Shimizu 890ddd
}
Toshihiro Shimizu 890ddd
Shinya Kitaoka 120a6e
void IKEngine::doUpdateStep(Jacobian &jacobian) {
Shinya Kitaoka 120a6e
  jacobian.computeJacobian();  // calcolo Jacobiano e il vettore deltaS
Shinya Kitaoka 120a6e
  int WhichMethod = DLS;
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
  bool clampingDetected = true;
Shinya Kitaoka 120a6e
  while (clampingDetected) {
Shinya Kitaoka 120a6e
    // Calcolo i cambiamenti dell'angolo
Shinya Kitaoka 120a6e
    switch (WhichMethod) {
Shinya Kitaoka 120a6e
    case JACOB_TRANS:
Shinya Kitaoka 120a6e
      jacobian.CalcDeltaThetasTranspose();  // Jacobian transpose method
Shinya Kitaoka 120a6e
      break;
Shinya Kitaoka 120a6e
    case DLS:
Shinya Kitaoka 120a6e
      jacobian.CalcDeltaThetasDLS();  // Damped least squares method
Shinya Kitaoka 120a6e
      break;
Shinya Kitaoka 120a6e
    case PURE_PSEUDO:
Shinya Kitaoka 120a6e
      jacobian.CalcDeltaThetasPseudoinverse();  // Pure pseudoinverse method
Shinya Kitaoka 120a6e
      break;
Shinya Kitaoka 120a6e
    case SDLS:
Shinya Kitaoka 120a6e
      jacobian
Shinya Kitaoka 120a6e
          .CalcDeltaThetasSDLS();  // Selectively damped least squares method
Shinya Kitaoka 120a6e
      break;
Shinya Kitaoka 120a6e
    default:
Shinya Kitaoka 120a6e
      jacobian.ZeroDeltaThetas();
Shinya Kitaoka 120a6e
      break;
Shinya Kitaoka 120a6e
    }
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
    jacobian.UpdateThetas();  // Aggiorno i valori di theta
Shinya Kitaoka 120a6e
Shinya Kitaoka 120a6e
    clampingDetected = jacobian.checkJointsLimit();
Shinya Kitaoka 120a6e
    // jacobian.UpdatedSClampValue();
Shinya Kitaoka 120a6e
  }
Toshihiro Shimizu 890ddd
}
Toshihiro Shimizu 890ddd
Toshihiro Shimizu 890ddd
// Assegno gli indici nella sequenza dei giunti
Shinya Kitaoka 120a6e
void IKEngine::setSequenceJoints() {
Shinya Kitaoka 120a6e
  int indexJoint = 0;
Shinya Kitaoka 120a6e
  for (int i = 0; i < (int)m_skeleton.getNodeCount(); i++) {
Shinya Kitaoka 120a6e
    IKNode *n               = m_skeleton.getNode(i);
Shinya Kitaoka 120a6e
    IKNode::Purpose purpose = n->getPurpose();
Shinya Kitaoka 120a6e
    if (purpose != IKNode::EFFECTOR) {
Shinya Kitaoka 120a6e
      n->setSeqNumJoint(indexJoint);
Shinya Kitaoka 120a6e
      indexJoint++;
Shinya Kitaoka 120a6e
    }
Shinya Kitaoka 120a6e
  }
Toshihiro Shimizu 890ddd
}