IK_Solver.cpp

Go to the documentation of this file.

/* SPDX-FileCopyrightText: 2001-2002 NaN Holding BV. All rights reserved.
 *
 * SPDX-License-Identifier: GPL-2.0-or-later */

 
#include "../extern/IK_solver.h"
 
#include "IK_QJacobianSolver.h"
#include "IK_QSegment.h"
#include "IK_QTask.h"
 
#include <algorithm>
#include <list>
using namespace std;
 
class IK_QSolver {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  IK_QSolver() = default;
 
  IK_QJacobianSolver solver;
  IK_QSegment *root = nullptr;
  std::list<IK_QTask *> tasks;
};
 
// FIXME: locks still result in small "residual" changes to the locked axes...
static IK_QSegment *CreateSegment(int flag, bool translate)
{
  int ndof = 0;
  ndof += (flag & IK_XDOF) ? 1 : 0;
  ndof += (flag & IK_YDOF) ? 1 : 0;
  ndof += (flag & IK_ZDOF) ? 1 : 0;
 
  IK_QSegment *seg;
 
  if (ndof == 0) {
    return nullptr;
  }
  if (ndof == 1) {
    int axis;
 
    if (flag & IK_XDOF) {
      axis = 0;
    }
    else if (flag & IK_YDOF) {
      axis = 1;
    }
    else {
      axis = 2;
    }
 
    if (translate) {
      seg = new IK_QTranslateSegment(axis);
    }
    else {
      seg = new IK_QRevoluteSegment(axis);
    }
  }
  else if (ndof == 2) {
    int axis1, axis2;
 
    if (flag & IK_XDOF) {
      axis1 = 0;
      axis2 = (flag & IK_YDOF) ? 1 : 2;
    }
    else {
      axis1 = 1;
      axis2 = 2;
    }
 
    if (translate) {
      seg = new IK_QTranslateSegment(axis1, axis2);
    }
    else {
      if (axis1 + axis2 == 2) {
        seg = new IK_QSwingSegment();
      }
      else {
        seg = new IK_QElbowSegment((axis1 == 0) ? 0 : 2);
      }
    }
  }
  else {
    if (translate) {
      seg = new IK_QTranslateSegment();
    }
    else {
      seg = new IK_QSphericalSegment();
    }
  }
 
  return seg;
}
 
IK_Segment *IK_CreateSegment(int flag)
{
  IK_QSegment *rot = CreateSegment(flag, false);
  IK_QSegment *trans = CreateSegment(flag >> 3, true);
 
  IK_QSegment *seg;
 
  if (rot == nullptr && trans == nullptr) {
    seg = new IK_QNullSegment();
  }
  else if (rot == nullptr) {
    seg = trans;
  }
  else {
    seg = rot;
 
    // make it seem from the interface as if the rotation and translation
    // segment are one
    if (trans) {
      seg->SetComposite(trans);
      trans->SetParent(seg);
    }
  }
 
  return seg;
}
 
void IK_FreeSegment(IK_Segment *seg)
{
  IK_QSegment *qseg = (IK_QSegment *)seg;
 
  if (qseg->Composite()) {
    delete qseg->Composite();
  }
  delete qseg;
}
 
void IK_SetParent(IK_Segment *seg, IK_Segment *parent)
{
  IK_QSegment *qseg = (IK_QSegment *)seg;
  IK_QSegment *qparent = (IK_QSegment *)parent;
 
  if (qparent && qparent->Composite()) {
    qseg->SetParent(qparent->Composite());
  }
  else {
    qseg->SetParent(qparent);
  }
}
 
void IK_SetTransform(
    IK_Segment *seg, float start[3], float rest[][3], float basis[][3], float length)
{
  IK_QSegment *qseg = (IK_QSegment *)seg;
 
  Vector3d mstart(start[0], start[1], start[2]);
  // convert from blender column major
  Matrix3d mbasis = CreateMatrix(basis[0][0],
                                 basis[1][0],
                                 basis[2][0],
                                 basis[0][1],
                                 basis[1][1],
                                 basis[2][1],
                                 basis[0][2],
                                 basis[1][2],
                                 basis[2][2]);
  Matrix3d mrest = CreateMatrix(rest[0][0],
                                rest[1][0],
                                rest[2][0],
                                rest[0][1],
                                rest[1][1],
                                rest[2][1],
                                rest[0][2],
                                rest[1][2],
                                rest[2][2]);
  double mlength(length);
 
  if (qseg->Composite()) {
    Vector3d cstart(0, 0, 0);
    Matrix3d cbasis;
    cbasis.setIdentity();
 
    qseg->SetTransform(mstart, mrest, mbasis, 0.0);
    qseg->Composite()->SetTransform(cstart, cbasis, cbasis, mlength);
  }
  else {
    qseg->SetTransform(mstart, mrest, mbasis, mlength);
  }
}
 
void IK_SetLimit(IK_Segment *seg, IK_SegmentAxis axis, float lmin, float lmax)
{
  IK_QSegment *qseg = (IK_QSegment *)seg;
 
  if (axis >= IK_TRANS_X) {
    if (!qseg->Translational()) {
      if (qseg->Composite() && qseg->Composite()->Translational()) {
        qseg = qseg->Composite();
      }
      else {
        return;
      }
    }
 
    if (axis == IK_TRANS_X) {
      axis = IK_X;
    }
    else if (axis == IK_TRANS_Y) {
      axis = IK_Y;
    }
    else {
      axis = IK_Z;
    }
  }
 
  qseg->SetLimit(axis, lmin, lmax);
}
 
void IK_SetStiffness(IK_Segment *seg, IK_SegmentAxis axis, float stiffness)
{
  if (stiffness < 0.0f) {
    return;
  }
 
  stiffness = std::min<double>(stiffness, 1.0 - IK_STRETCH_STIFF_EPS);
 
  IK_QSegment *qseg = (IK_QSegment *)seg;
  double weight = 1.0f - stiffness;
 
  if (axis >= IK_TRANS_X) {
    if (!qseg->Translational()) {
      if (qseg->Composite() && qseg->Composite()->Translational()) {
        qseg = qseg->Composite();
      }
      else {
        return;
      }
    }
 
    if (axis == IK_TRANS_X) {
      axis = IK_X;
    }
    else if (axis == IK_TRANS_Y) {
      axis = IK_Y;
    }
    else {
      axis = IK_Z;
    }
  }
 
  qseg->SetWeight(axis, weight);
}
 
void IK_GetBasisChange(IK_Segment *seg, float basis_change[][3])
{
  IK_QSegment *qseg = (IK_QSegment *)seg;
 
  if (qseg->Translational() && qseg->Composite()) {
    qseg = qseg->Composite();
  }
 
  const Matrix3d &change = qseg->BasisChange();
 
  // convert to blender column major
  basis_change[0][0] = (float)change(0, 0);
  basis_change[1][0] = (float)change(0, 1);
  basis_change[2][0] = (float)change(0, 2);
  basis_change[0][1] = (float)change(1, 0);
  basis_change[1][1] = (float)change(1, 1);
  basis_change[2][1] = (float)change(1, 2);
  basis_change[0][2] = (float)change(2, 0);
  basis_change[1][2] = (float)change(2, 1);
  basis_change[2][2] = (float)change(2, 2);
}
 
void IK_GetTranslationChange(IK_Segment *seg, float *translation_change)
{
  IK_QSegment *qseg = (IK_QSegment *)seg;
 
  if (!qseg->Translational() && qseg->Composite()) {
    qseg = qseg->Composite();
  }
 
  const Vector3d &change = qseg->TranslationChange();
 
  translation_change[0] = (float)change[0];
  translation_change[1] = (float)change[1];
  translation_change[2] = (float)change[2];
}
 
IK_Solver *IK_CreateSolver(IK_Segment *root)
{
  if (root == nullptr) {
    return nullptr;
  }
 
  IK_QSolver *solver = new IK_QSolver();
  solver->root = (IK_QSegment *)root;
 
  return (IK_Solver *)solver;
}
 
void IK_FreeSolver(IK_Solver *solver)
{
  if (solver == nullptr) {
    return;
  }
 
  IK_QSolver *qsolver = (IK_QSolver *)solver;
  std::list<IK_QTask *> &tasks = qsolver->tasks;
  std::list<IK_QTask *>::iterator task;
 
  for (task = tasks.begin(); task != tasks.end(); task++) {
    delete (*task);
  }
 
  delete qsolver;
}
 
void IK_SolverAddGoal(IK_Solver *solver, IK_Segment *tip, float goal[3], float weight)
{
  if (solver == nullptr || tip == nullptr) {
    return;
  }
 
  IK_QSolver *qsolver = (IK_QSolver *)solver;
  IK_QSegment *qtip = (IK_QSegment *)tip;
 
  // in case of composite segment the second segment is the tip
  if (qtip->Composite()) {
    qtip = qtip->Composite();
  }
 
  Vector3d pos(goal[0], goal[1], goal[2]);
 
  IK_QTask *ee = new IK_QPositionTask(true, qtip, pos);
  ee->SetWeight(weight);
  qsolver->tasks.push_back(ee);
}
 
void IK_SolverAddGoalOrientation(IK_Solver *solver, IK_Segment *tip, float goal[][3], float weight)
{
  if (solver == nullptr || tip == nullptr) {
    return;
  }
 
  IK_QSolver *qsolver = (IK_QSolver *)solver;
  IK_QSegment *qtip = (IK_QSegment *)tip;
 
  // in case of composite segment the second segment is the tip
  if (qtip->Composite()) {
    qtip = qtip->Composite();
  }
 
  // convert from blender column major
  Matrix3d rot = CreateMatrix(goal[0][0],
                              goal[1][0],
                              goal[2][0],
                              goal[0][1],
                              goal[1][1],
                              goal[2][1],
                              goal[0][2],
                              goal[1][2],
                              goal[2][2]);
 
  IK_QTask *orient = new IK_QOrientationTask(true, qtip, rot);
  orient->SetWeight(weight);
  qsolver->tasks.push_back(orient);
}
 
void IK_SolverSetPoleVectorConstraint(IK_Solver *solver,
                                      IK_Segment *tip,
                                      float goal[3],
                                      float polegoal[3],
                                      float poleangle,
                                      int getangle)
{
  if (solver == nullptr || tip == nullptr) {
    return;
  }
 
  IK_QSolver *qsolver = (IK_QSolver *)solver;
  IK_QSegment *qtip = (IK_QSegment *)tip;
 
  // in case of composite segment the second segment is the tip
  if (qtip->Composite()) {
    qtip = qtip->Composite();
  }
 
  Vector3d qgoal(goal[0], goal[1], goal[2]);
  Vector3d qpolegoal(polegoal[0], polegoal[1], polegoal[2]);
 
  qsolver->solver.SetPoleVectorConstraint(qtip, qgoal, qpolegoal, poleangle, getangle);
}
 
float IK_SolverGetPoleAngle(IK_Solver *solver)
{
  if (solver == nullptr) {
    return 0.0f;
  }
 
  IK_QSolver *qsolver = (IK_QSolver *)solver;
 
  return qsolver->solver.GetPoleAngle();
}
 
#if 0
static void IK_SolverAddCenterOfMass(IK_Solver *solver,
                                     IK_Segment *root,
                                     float goal[3],
                                     float weight)
{
  if (solver == nullptr || root == nullptr) {
    return;
  }
 
  IK_QSolver *qsolver = (IK_QSolver *)solver;
  IK_QSegment *qroot = (IK_QSegment *)root;
 
  // convert from blender column major
  Vector3d center(goal);
 
  IK_QTask *com = new IK_QCenterOfMassTask(true, qroot, center);
  com->SetWeight(weight);
  qsolver->tasks.push_back(com);
}
#endif
 
int IK_Solve(IK_Solver *solver, float tolerance, int max_iterations)
{
  if (solver == nullptr) {
    return 0;
  }
 
  IK_QSolver *qsolver = (IK_QSolver *)solver;
 
  IK_QSegment *root = qsolver->root;
  IK_QJacobianSolver &jacobian = qsolver->solver;
  std::list<IK_QTask *> &tasks = qsolver->tasks;
  double tol = tolerance;
 
  if (!jacobian.Setup(root, tasks)) {
    return 0;
  }
 
  bool result = jacobian.Solve(root, tasks, tol, max_iterations);
 
  return ((result) ? 1 : 0);
}