Add Chromium-only Blender WebEngine parity work

blender-5.2.0/intern/iksolver/CMakeLists.txt

@@ -0,0 +1,32 @@
+# SPDX-FileCopyrightText: 2006 Blender Authors
+#
+# SPDX-License-Identifier: GPL-2.0-or-later
+
+set(INC
+  intern
+  ../memutil
+)
+
+set(INC_SYS
+)
+
+set(SRC
+  intern/IK_QJacobian.cpp
+  intern/IK_QJacobianSolver.cpp
+  intern/IK_QSegment.cpp
+  intern/IK_QTask.cpp
+  intern/IK_Solver.cpp
+
+  extern/IK_solver.h
+  intern/IK_Math.h
+  intern/IK_QJacobian.h
+  intern/IK_QJacobianSolver.h
+  intern/IK_QSegment.h
+  intern/IK_QTask.h
+)
+
+set(LIB
+  PRIVATE bf::dependencies::eigen
+)
+
+blender_add_lib(bf_intern_iksolver "${SRC}" "${INC}" "${INC_SYS}" "${LIB}")

blender-5.2.0/intern/iksolver/extern/IK_solver.h

@@ -0,0 +1,153 @@
+/* SPDX-FileCopyrightText: 2001-2002 NaN Holding BV. All rights reserved.
+ *
+ * SPDX-License-Identifier: GPL-2.0-or-later */
+
+/** \file
+ * \ingroup intern_iksolver
+ */
+
+/**
+ * \page IK - Blender inverse kinematics module.
+ *
+ * \section about About the IK module
+ *
+ * This module allows you to create segments and form them into
+ * tree. You can then define a goal points that the end of a given
+ * segment should attempt to reach - an inverse kinematic problem.
+ * This module will then modify the segments in the tree in order
+ * to get the as near as possible to the goal. This solver uses an
+ * inverse jacobian method to find a solution.
+ *
+ * \section issues Known issues with this IK solver.
+ *
+ * - There is currently no support for joint constraints in the
+ * solver. This is within the realms of possibility - please ask
+ * if this functionality is required.
+ * - The solver is slow, inverse jacobian methods in general give
+ * 'smooth' solutions and the method is also very flexible, it
+ * does not rely on specific angle parameterization and can be
+ * extended to deal with different joint types and joint
+ * constraints. However it is not suitable for real time use.
+ * Other algorithms exist which are more suitable for real-time
+ * applications, please ask if this functionality is required.
+ *
+ * \section dependencies Dependencies
+ *
+ * This module only depends on Moto.
+ */
+
+#ifndef __IK_SOLVER_H__
+#define __IK_SOLVER_H__
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/**
+ * Typical order of calls for solving an IK problem:
+ *
+ * - create number of IK_Segment's and set their parents and transforms
+ * - create an IK_Solver
+ * - set a number of goals for the IK_Solver to solve
+ * - call IK_Solve
+ * - free the IK_Solver
+ * - get basis and translation changes from segments
+ * - free all segments
+ */
+
+/**
+ * IK_Segment defines a single segment of an IK tree.
+ * - Individual segments are always defined in local coordinates.
+ * - The segment is assumed to be oriented in the local
+ *   y-direction.
+ * - start is the start of the segment relative to the end
+ *   of the parent segment.
+ * - rest_basis is a column major matrix defineding the rest
+ *   position (w.r.t. which the limits are defined), must
+ *   be a pure rotation
+ * - basis is a column major matrix defining the current change
+ *   from the rest basis, must be a pure rotation
+ * - length is the length of the bone.
+ *
+ * - basis_change and translation_change respectively define
+ *   the change in rotation or translation. basis_change is a
+ *   column major 3x3 matrix.
+ *
+ * The local transformation is then defined as:
+ * start * rest_basis * basis * basis_change * translation_change * translate(0,length,0)
+ */
+
+using IK_Segment = void;
+
+enum IK_SegmentFlag {
+  IK_XDOF = 1,
+  IK_YDOF = 2,
+  IK_ZDOF = 4,
+  IK_TRANS_XDOF = 8,
+  IK_TRANS_YDOF = 16,
+  IK_TRANS_ZDOF = 32
+};
+
+enum IK_SegmentAxis {
+  IK_X = 0,
+  IK_Y = 1,
+  IK_Z = 2,
+  IK_TRANS_X = 3,
+  IK_TRANS_Y = 4,
+  IK_TRANS_Z = 5
+};
+
+extern IK_Segment *IK_CreateSegment(int flag);
+extern void IK_FreeSegment(IK_Segment *seg);
+
+extern void IK_SetParent(IK_Segment *seg, IK_Segment *parent);
+extern void IK_SetTransform(
+    IK_Segment *seg, float start[3], float rest_basis[][3], float basis[][3], float length);
+extern void IK_SetLimit(IK_Segment *seg, IK_SegmentAxis axis, float lmin, float lmax);
+extern void IK_SetStiffness(IK_Segment *seg, IK_SegmentAxis axis, float stiffness);
+
+extern void IK_GetBasisChange(IK_Segment *seg, float basis_change[][3]);
+extern void IK_GetTranslationChange(IK_Segment *seg, float *translation_change);
+
+/**
+ * An IK_Solver must be created to be able to execute the solver.
+ *
+ * An arbitray number of goals can be created, stating that a given
+ * end effector must have a given position or rotation. If multiple
+ * goals are specified, they can be weighted (range 0..1) to get
+ * some control over their importance.
+ *
+ * IK_Solve will execute the solver, that will run until either the
+ * system converges, or a maximum number of iterations is reached.
+ * It returns 1 if the system converged, 0 otherwise.
+ */
+
+using IK_Solver = void;
+
+IK_Solver *IK_CreateSolver(IK_Segment *root);
+void IK_FreeSolver(IK_Solver *solver);
+
+void IK_SolverAddGoal(IK_Solver *solver, IK_Segment *tip, float goal[3], float weight);
+void IK_SolverAddGoalOrientation(IK_Solver *solver,
+                                 IK_Segment *tip,
+                                 float goal[][3],
+                                 float weight);
+void IK_SolverSetPoleVectorConstraint(IK_Solver *solver,
+                                      IK_Segment *tip,
+                                      float goal[3],
+                                      float polegoal[3],
+                                      float poleangle,
+                                      int getangle);
+float IK_SolverGetPoleAngle(IK_Solver *solver);
+
+int IK_Solve(IK_Solver *solver, float tolerance, int max_iterations);
+
+#define IK_STRETCH_STIFF_EPS 0.01f
+#define IK_STRETCH_STIFF_MIN 0.001f
+#define IK_STRETCH_STIFF_MAX 1e10
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif  // __IK_SOLVER_H__

blender-5.2.0/intern/iksolver/intern/IK_Math.h

@@ -0,0 +1,256 @@
+/* SPDX-FileCopyrightText: 2001-2002 NaN Holding BV. All rights reserved.
+ *
+ * SPDX-License-Identifier: GPL-2.0-or-later */
+
+/** \file
+ * \ingroup intern_iksolver
+ */
+
+#pragma once
+
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+
+#include <algorithm>
+#include <cmath>
+
+using Eigen::Affine3d;
+using Eigen::Matrix3d;
+using Eigen::MatrixXd;
+using Eigen::Vector3d;
+using Eigen::VectorXd;
+
+static const double IK_EPSILON = 1e-20;
+
+static inline bool FuzzyZero(double x)
+{
+  return fabs(x) < IK_EPSILON;
+}
+
+static inline double Clamp(const double x, const double min, const double max)
+{
+  return (x < min) ? min : (x > max) ? max : x;
+}
+
+static inline Eigen::Matrix3d CreateMatrix(double xx,
+                                           double xy,
+                                           double xz,
+                                           double yx,
+                                           double yy,
+                                           double yz,
+                                           double zx,
+                                           double zy,
+                                           double zz)
+{
+  Eigen::Matrix3d M;
+  M(0, 0) = xx;
+  M(0, 1) = xy;
+  M(0, 2) = xz;
+  M(1, 0) = yx;
+  M(1, 1) = yy;
+  M(1, 2) = yz;
+  M(2, 0) = zx;
+  M(2, 1) = zy;
+  M(2, 2) = zz;
+  return M;
+}
+
+static inline Eigen::Matrix3d RotationMatrix(double sine, double cosine, int axis)
+{
+  if (axis == 0) {
+    return CreateMatrix(1.0, 0.0, 0.0, 0.0, cosine, -sine, 0.0, sine, cosine);
+  }
+  if (axis == 1) {
+    return CreateMatrix(cosine, 0.0, sine, 0.0, 1.0, 0.0, -sine, 0.0, cosine);
+  }
+  return CreateMatrix(cosine, -sine, 0.0, sine, cosine, 0.0, 0.0, 0.0, 1.0);
+}
+
+static inline Eigen::Matrix3d RotationMatrix(double angle, int axis)
+{
+  return RotationMatrix(sin(angle), cos(angle), axis);
+}
+
+static inline double EulerAngleFromMatrix(const Eigen::Matrix3d &R, int axis)
+{
+  double t = sqrt(R(0, 0) * R(0, 0) + R(0, 1) * R(0, 1));
+
+  if (t > 16.0 * IK_EPSILON) {
+    if (axis == 0) {
+      return -atan2(R(1, 2), R(2, 2));
+    }
+    if (axis == 1) {
+      return atan2(-R(0, 2), t);
+    }
+    return -atan2(R(0, 1), R(0, 0));
+  }
+
+  if (axis == 0) {
+    return -atan2(-R(2, 1), R(1, 1));
+  }
+  if (axis == 1) {
+    return atan2(-R(0, 2), t);
+  }
+  return 0.0f;
+}
+
+static inline double safe_acos(double f)
+{
+  // acos that does not return NaN with rounding errors
+  if (f <= -1.0) {
+    return M_PI;
+  }
+  if (f >= 1.0) {
+    return 0.0;
+  }
+  return acos(f);
+}
+
+static inline Eigen::Vector3d normalize(const Eigen::Vector3d &v)
+{
+  // a sane normalize function that doesn't give (1, 0, 0) in case
+  // of a zero length vector
+  double len = v.norm();
+  return FuzzyZero(len) ? Eigen::Vector3d(0, 0, 0) : Eigen::Vector3d(v / len);
+}
+
+static inline double angle(const Eigen::Vector3d &v1, const Eigen::Vector3d &v2)
+{
+  return safe_acos(v1.dot(v2));
+}
+
+static inline double ComputeTwist(const Eigen::Matrix3d &R)
+{
+  // qy and qw are the y and w components of the quaternion from R
+  double qy = R(0, 2) - R(2, 0);
+  double qw = R(0, 0) + R(1, 1) + R(2, 2) + 1;
+
+  double tau = 2.0 * atan2(qy, qw);
+
+  return tau;
+}
+
+static inline Eigen::Matrix3d ComputeTwistMatrix(double tau)
+{
+  return RotationMatrix(tau, 1);
+}
+
+static inline void RemoveTwist(Eigen::Matrix3d &R)
+{
+  // compute twist parameter
+  double tau = ComputeTwist(R);
+
+  // compute twist matrix
+  Eigen::Matrix3d T = ComputeTwistMatrix(tau);
+
+  // remove twist
+  R = R * T.transpose();
+}
+
+static inline Eigen::Vector3d SphericalRangeParameters(const Eigen::Matrix3d &R)
+{
+  // compute twist parameter
+  double tau = ComputeTwist(R);
+
+  // compute swing parameters
+  double num = 2.0 * (1.0 + R(1, 1));
+
+  // singularity at pi
+  if (fabs(num) < IK_EPSILON) {
+    // TODO: this does now rotation of size pi over z axis, but could
+    // be any axis, how to deal with this I'm not sure, maybe don't
+    // enforce limits at all then.
+    return Eigen::Vector3d(0.0, tau, 1.0);
+  }
+
+  num = 1.0 / sqrt(num);
+  double ax = -R(2, 1) * num;
+  double az = R(0, 1) * num;
+
+  return Eigen::Vector3d(ax, tau, az);
+}
+
+static inline Eigen::Matrix3d ComputeSwingMatrix(double ax, double az)
+{
+  // length of (ax, 0, az) = sin(theta/2)
+  double sine2 = ax * ax + az * az;
+  double cosine2 = sqrt((sine2 >= 1.0) ? 0.0 : 1.0 - sine2);
+
+  // compute swing matrix
+  Eigen::Matrix3d S(Eigen::Quaterniond(-cosine2, ax, 0.0, az));
+
+  return S;
+}
+
+static inline Eigen::Vector3d MatrixToAxisAngle(const Eigen::Matrix3d &R)
+{
+  Eigen::Vector3d delta = Eigen::Vector3d(R(2, 1) - R(1, 2), R(0, 2) - R(2, 0), R(1, 0) - R(0, 1));
+
+  double c = safe_acos((R(0, 0) + R(1, 1) + R(2, 2) - 1) / 2);
+  double l = delta.norm();
+
+  if (!FuzzyZero(l)) {
+    delta *= c / l;
+  }
+
+  return delta;
+}
+
+static inline bool EllipseClamp(double &ax, double &az, const double *amin, const double *amax)
+{
+  double xlim, zlim, x, z;
+
+  if (ax < 0.0) {
+    x = -ax;
+    xlim = -amin[0];
+  }
+  else {
+    x = ax;
+    xlim = amax[0];
+  }
+
+  if (az < 0.0) {
+    z = -az;
+    zlim = -amin[1];
+  }
+  else {
+    z = az;
+    zlim = amax[1];
+  }
+
+  if (FuzzyZero(xlim) || FuzzyZero(zlim)) {
+    if (x <= xlim && z <= zlim) {
+      return false;
+    }
+
+    x = std::min(x, xlim);
+    z = std::min(z, zlim);
+  }
+  else {
+    double invx = 1.0 / (xlim * xlim);
+    double invz = 1.0 / (zlim * zlim);
+
+    if ((x * x * invx + z * z * invz) <= 1.0) {
+      return false;
+    }
+
+    if (FuzzyZero(x)) {
+      x = 0.0;
+      z = zlim;
+    }
+    else {
+      double rico = z / x;
+      double old_x = x;
+      x = sqrt(1.0 / (invx + invz * rico * rico));
+      if (old_x < 0.0) {
+        x = -x;
+      }
+      z = rico * x;
+    }
+  }
+
+  ax = (ax < 0.0) ? -x : x;
+  az = (az < 0.0) ? -z : z;
+
+  return true;
+}

blender-5.2.0/intern/iksolver/intern/IK_QJacobian.cpp

@@ -0,0 +1,424 @@
+/* SPDX-FileCopyrightText: 2001-2002 NaN Holding BV. All rights reserved.
+ *
+ * SPDX-License-Identifier: GPL-2.0-or-later */
+
+/** \file
+ * \ingroup intern_iksolver
+ */
+
+#include "IK_QJacobian.h"
+
+#include <algorithm>
+
+IK_QJacobian::IK_QJacobian() : m_sdls(true), m_min_damp(1.0) {}
+
+IK_QJacobian::~IK_QJacobian() = default;
+
+void IK_QJacobian::ArmMatrices(int dof, int task_size)
+{
+  m_dof = dof;
+  m_task_size = task_size;
+
+  m_jacobian.resize(task_size, dof);
+  m_jacobian.setZero();
+
+  m_alpha.resize(dof);
+  m_alpha.setZero();
+
+  m_nullspace.resize(dof, dof);
+
+  m_d_theta.resize(dof);
+  m_d_theta_tmp.resize(dof);
+  m_d_norm_weight.resize(dof);
+
+  m_norm.resize(dof);
+  m_norm.setZero();
+
+  m_beta.resize(task_size);
+
+  m_weight.resize(dof);
+  m_weight_sqrt.resize(dof);
+  m_weight.setOnes();
+  m_weight_sqrt.setOnes();
+
+  if (task_size >= dof) {
+    m_transpose = false;
+
+    m_jacobian_tmp.resize(task_size, dof);
+
+    m_svd_u.resize(task_size, dof);
+    m_svd_v.resize(dof, dof);
+    m_svd_w.resize(dof);
+
+    m_svd_u_beta.resize(dof);
+  }
+  else {
+    // use the SVD of the transpose jacobian, it works just as well
+    // as the original, and often allows using smaller matrices.
+    m_transpose = true;
+
+    m_jacobian_tmp.resize(dof, task_size);
+
+    m_svd_u.resize(task_size, task_size);
+    m_svd_v.resize(dof, task_size);
+    m_svd_w.resize(task_size);
+
+    m_svd_u_beta.resize(task_size);
+  }
+}
+
+void IK_QJacobian::SetBetas(int id, int /*size*/, const Vector3d &v)
+{
+  m_beta[id + 0] = v.x();
+  m_beta[id + 1] = v.y();
+  m_beta[id + 2] = v.z();
+}
+
+void IK_QJacobian::SetDerivatives(int id, int dof_id, const Vector3d &v, double norm_weight)
+{
+  m_jacobian(id + 0, dof_id) = v.x() * m_weight_sqrt[dof_id];
+  m_jacobian(id + 1, dof_id) = v.y() * m_weight_sqrt[dof_id];
+  m_jacobian(id + 2, dof_id) = v.z() * m_weight_sqrt[dof_id];
+
+  m_d_norm_weight[dof_id] = norm_weight;
+}
+
+void IK_QJacobian::Invert()
+{
+  if (m_transpose) {
+    // SVD will decompose Jt into V*W*Ut with U,V orthogonal and W diagonal,
+    // so J = U*W*Vt and Jinv = V*Winv*Ut
+    Eigen::JacobiSVD<MatrixXd> svd(m_jacobian.transpose(),
+                                   Eigen::ComputeThinU | Eigen::ComputeThinV);
+    m_svd_u = svd.matrixV();
+    m_svd_w = svd.singularValues();
+    m_svd_v = svd.matrixU();
+  }
+  else {
+    // SVD will decompose J into U*W*Vt with U,V orthogonal and W diagonal,
+    // so Jinv = V*Winv*Ut
+    Eigen::JacobiSVD<MatrixXd> svd(m_jacobian, Eigen::ComputeThinU | Eigen::ComputeThinV);
+    m_svd_u = svd.matrixU();
+    m_svd_w = svd.singularValues();
+    m_svd_v = svd.matrixV();
+  }
+
+  if (m_sdls) {
+    InvertSDLS();
+  }
+  else {
+    InvertDLS();
+  }
+}
+
+bool IK_QJacobian::ComputeNullProjection()
+{
+  double epsilon = 1e-10;
+
+  // compute null space projection based on V
+  int i, j, rank = 0;
+  for (i = 0; i < m_svd_w.size(); i++) {
+    if (m_svd_w[i] > epsilon) {
+      rank++;
+    }
+  }
+
+  if (rank < m_task_size) {
+    return false;
+  }
+
+  MatrixXd basis(m_svd_v.rows(), rank);
+  int b = 0;
+
+  for (i = 0; i < m_svd_w.size(); i++) {
+    if (m_svd_w[i] > epsilon) {
+      for (j = 0; j < m_svd_v.rows(); j++) {
+        basis(j, b) = m_svd_v(j, i);
+      }
+      b++;
+    }
+  }
+
+  m_nullspace = basis * basis.transpose();
+
+  for (i = 0; i < m_nullspace.rows(); i++) {
+    for (j = 0; j < m_nullspace.cols(); j++) {
+      if (i == j) {
+        m_nullspace(i, j) = 1.0 - m_nullspace(i, j);
+      }
+      else {
+        m_nullspace(i, j) = -m_nullspace(i, j);
+      }
+    }
+  }
+
+  return true;
+}
+
+void IK_QJacobian::SubTask(IK_QJacobian &jacobian)
+{
+  if (!ComputeNullProjection()) {
+    return;
+  }
+
+  // restrict lower priority jacobian
+  jacobian.Restrict(m_d_theta, m_nullspace);
+
+  // add angle update from lower priority
+  jacobian.Invert();
+
+  // note: now damps secondary angles with minimum damping value from
+  // SDLS, to avoid shaking when the primary task is near singularities,
+  // doesn't work well at all
+  int i;
+  for (i = 0; i < m_d_theta.size(); i++) {
+    m_d_theta[i] = m_d_theta[i] + /* `m_min_damp *` */ jacobian.AngleUpdate(i);
+  }
+}
+
+void IK_QJacobian::Restrict(VectorXd &d_theta, MatrixXd &nullspace)
+{
+  // subtract part already moved by higher task from beta
+  m_beta = m_beta - m_jacobian * d_theta;
+
+  // note: should we be using the norm of the unrestricted jacobian for SDLS?
+
+  // project jacobian on to null space of higher priority task
+  m_jacobian = m_jacobian * nullspace;
+}
+
+void IK_QJacobian::InvertSDLS()
+{
+  // Compute the dampeds least squeares pseudo inverse of J.
+  //
+  // Since J is usually not invertible (most of the times it's not even
+  // square), the pseudo inverse is used. This gives us a least squares
+  // solution.
+  //
+  // This is fine when the J*Jt is of full rank. When J*Jt is near to
+  // singular the least squares inverse tries to minimize |J(dtheta) - dX)|
+  // and doesn't try to minimize  dTheta. This results in erratic changes in
+  // angle. The damped least squares minimizes |dtheta| to try and reduce this
+  // erratic behavior.
+  //
+  // The selectively damped least squares (SDLS) is used here instead of the
+  // DLS. The SDLS damps individual singular values, instead of using a single
+  // damping term.
+
+  double max_angle_change = M_PI_4;
+  double epsilon = 1e-10;
+  int i, j;
+
+  m_d_theta.setZero();
+  m_min_damp = 1.0;
+
+  for (i = 0; i < m_dof; i++) {
+    m_norm[i] = 0.0;
+    for (j = 0; j < m_task_size; j += 3) {
+      double n = 0.0;
+      n += m_jacobian(j, i) * m_jacobian(j, i);
+      n += m_jacobian(j + 1, i) * m_jacobian(j + 1, i);
+      n += m_jacobian(j + 2, i) * m_jacobian(j + 2, i);
+      m_norm[i] += sqrt(n);
+    }
+  }
+
+  for (i = 0; i < m_svd_w.size(); i++) {
+    if (m_svd_w[i] <= epsilon) {
+      continue;
+    }
+
+    double wInv = 1.0 / m_svd_w[i];
+    double alpha = 0.0;
+    double N = 0.0;
+
+    // compute alpha and N
+    for (j = 0; j < m_svd_u.rows(); j += 3) {
+      alpha += m_svd_u(j, i) * m_beta[j];
+      alpha += m_svd_u(j + 1, i) * m_beta[j + 1];
+      alpha += m_svd_u(j + 2, i) * m_beta[j + 2];
+
+      // note: for 1 end effector, N will always be 1, since U is
+      // orthogonal, .. so could be optimized
+      double tmp;
+      tmp = m_svd_u(j, i) * m_svd_u(j, i);
+      tmp += m_svd_u(j + 1, i) * m_svd_u(j + 1, i);
+      tmp += m_svd_u(j + 2, i) * m_svd_u(j + 2, i);
+      N += sqrt(tmp);
+    }
+    alpha *= wInv;
+
+    // compute M, dTheta and max_dtheta
+    double M = 0.0;
+    double max_dtheta = 0.0, abs_dtheta;
+
+    for (j = 0; j < m_d_theta.size(); j++) {
+      double v = m_svd_v(j, i);
+      M += fabs(v) * m_norm[j];
+
+      // compute tmporary dTheta's
+      m_d_theta_tmp[j] = v * alpha;
+
+      // find largest absolute dTheta
+      // multiply with weight to prevent unnecessary damping
+      abs_dtheta = fabs(m_d_theta_tmp[j]) * m_weight_sqrt[j];
+      max_dtheta = std::max(abs_dtheta, max_dtheta);
+    }
+
+    M *= wInv;
+
+    // compute damping term and damp the dTheta's
+    double gamma = max_angle_change;
+    if (N < M) {
+      gamma *= N / M;
+    }
+
+    double damp = (gamma < max_dtheta) ? gamma / max_dtheta : 1.0;
+
+    for (j = 0; j < m_d_theta.size(); j++) {
+      // slight hack: we do 0.80*, so that if there is some oscillation,
+      // the system can still converge (for joint limits). also, it's
+      // better to go a little to slow than to far
+
+      double dofdamp = damp / m_weight[j];
+      dofdamp = std::min(dofdamp, 1.0);
+
+      m_d_theta[j] += 0.80 * dofdamp * m_d_theta_tmp[j];
+    }
+
+    m_min_damp = std::min(damp, m_min_damp);
+  }
+
+  // weight + prevent from doing angle updates with angles > max_angle_change
+  double max_angle = 0.0, abs_angle;
+
+  for (j = 0; j < m_dof; j++) {
+    m_d_theta[j] *= m_weight[j];
+
+    abs_angle = fabs(m_d_theta[j]);
+
+    max_angle = std::max(abs_angle, max_angle);
+  }
+
+  if (max_angle > max_angle_change) {
+    double damp = (max_angle_change) / (max_angle_change + max_angle);
+
+    for (j = 0; j < m_dof; j++) {
+      m_d_theta[j] *= damp;
+    }
+  }
+}
+
+void IK_QJacobian::InvertDLS()
+{
+  // Compute damped least squares inverse of pseudo inverse
+  // Compute damping term lambda
+
+  // Note when lambda is zero this is equivalent to the
+  // least squares solution. This is fine when the m_jjt is
+  // of full rank. When m_jjt is near to singular the least squares
+  // inverse tries to minimize |J(dtheta) - dX)| and doesn't
+  // try to minimize  dTheta. This results in erratic changes in angle.
+  // Damped least squares minimizes |dtheta| to try and reduce this
+  // erratic behavior.
+
+  // We don't want to use the damped solution everywhere so we
+  // only increase lamda from zero as we approach a singularity.
+
+  // find the smallest non-zero W value, anything below epsilon is
+  // treated as zero
+
+  double epsilon = 1e-10;
+  double max_angle_change = 0.1;
+  double x_length = sqrt(m_beta.dot(m_beta));
+
+  int i, j;
+  double w_min = std::numeric_limits<double>::max();
+
+  for (i = 0; i < m_svd_w.size(); i++) {
+    if (m_svd_w[i] > epsilon && m_svd_w[i] < w_min) {
+      w_min = m_svd_w[i];
+    }
+  }
+
+  // compute lambda damping term
+
+  double d = x_length / max_angle_change;
+  double lambda;
+
+  if (w_min <= d / 2) {
+    lambda = d / 2;
+  }
+  else if (w_min < d) {
+    lambda = sqrt(w_min * (d - w_min));
+  }
+  else {
+    lambda = 0.0;
+  }
+
+  lambda *= lambda;
+
+  lambda = std::min<double>(lambda, 10);
+
+  // immediately multiply with Beta, so we can do matrix*vector products
+  // rather than matrix*matrix products
+
+  // compute Ut*Beta
+  m_svd_u_beta = m_svd_u.transpose() * m_beta;
+
+  m_d_theta.setZero();
+
+  for (i = 0; i < m_svd_w.size(); i++) {
+    if (m_svd_w[i] > epsilon) {
+      double wInv = m_svd_w[i] / (m_svd_w[i] * m_svd_w[i] + lambda);
+
+      // compute V*Winv*Ut*Beta
+      m_svd_u_beta[i] *= wInv;
+
+      for (j = 0; j < m_d_theta.size(); j++) {
+        m_d_theta[j] += m_svd_v(j, i) * m_svd_u_beta[i];
+      }
+    }
+  }
+
+  for (j = 0; j < m_d_theta.size(); j++) {
+    m_d_theta[j] *= m_weight[j];
+  }
+}
+
+void IK_QJacobian::Lock(int dof_id, double delta)
+{
+  int i;
+
+  for (i = 0; i < m_task_size; i++) {
+    m_beta[i] -= m_jacobian(i, dof_id) * delta;
+    m_jacobian(i, dof_id) = 0.0;
+  }
+
+  m_norm[dof_id] = 0.0;  // unneeded
+  m_d_theta[dof_id] = 0.0;
+}
+
+double IK_QJacobian::AngleUpdate(int dof_id) const
+{
+  return m_d_theta[dof_id];
+}
+
+double IK_QJacobian::AngleUpdateNorm() const
+{
+  int i;
+  double mx = 0.0, dtheta_abs;
+
+  for (i = 0; i < m_d_theta.size(); i++) {
+    dtheta_abs = fabs(m_d_theta[i] * m_d_norm_weight[i]);
+    mx = std::max(dtheta_abs, mx);
+  }
+
+  return mx;
+}
+
+void IK_QJacobian::SetDoFWeight(int dof, double weight)
+{
+  m_weight[dof] = weight;
+  m_weight_sqrt[dof] = sqrt(weight);
+}

blender-5.2.0/intern/iksolver/intern/IK_QJacobian.h

@@ -0,0 +1,78 @@
+/* SPDX-FileCopyrightText: 2001-2002 NaN Holding BV. All rights reserved.
+ *
+ * SPDX-License-Identifier: GPL-2.0-or-later */
+
+/** \file
+ * \ingroup intern_iksolver
+ */
+
+#pragma once
+
+#include "IK_Math.h"
+
+class IK_QJacobian {
+ public:
+  IK_QJacobian();
+  ~IK_QJacobian();
+
+  // Call once to initialize
+  void ArmMatrices(int dof, int task_size);
+  void SetDoFWeight(int dof, double weight);
+
+  // Iteratively called
+  void SetBetas(int id, int size, const Vector3d &v);
+  void SetDerivatives(int id, int dof_id, const Vector3d &v, double norm_weight);
+
+  void Invert();
+
+  double AngleUpdate(int dof_id) const;
+  double AngleUpdateNorm() const;
+
+  // DoF locking for inner clamping loop
+  void Lock(int dof_id, double delta);
+
+  // Secondary task
+  bool ComputeNullProjection();
+
+  void Restrict(VectorXd &d_theta, MatrixXd &nullspace);
+  void SubTask(IK_QJacobian &jacobian);
+
+ private:
+  void InvertSDLS();
+  void InvertDLS();
+
+  int m_dof, m_task_size;
+  bool m_transpose;
+
+  // the jacobian matrix and its null space projector
+  MatrixXd m_jacobian, m_jacobian_tmp;
+  MatrixXd m_nullspace;
+
+  /// the vector of intermediate betas
+  VectorXd m_beta;
+
+  /// the vector of computed angle changes
+  VectorXd m_d_theta;
+  VectorXd m_d_norm_weight;
+
+  /// space required for SVD computation
+  VectorXd m_svd_w;
+  MatrixXd m_svd_v;
+  MatrixXd m_svd_u;
+
+  VectorXd m_svd_u_beta;
+
+  // space required for SDLS
+
+  bool m_sdls;
+  VectorXd m_norm;
+  VectorXd m_d_theta_tmp;
+  double m_min_damp;
+
+  // null space task vector
+  VectorXd m_alpha;
+
+  // dof weighting
+  VectorXd m_weight;
+  VectorXd m_weight_sqrt;
+};

blender-5.2.0/intern/iksolver/intern/IK_QJacobianSolver.cpp

@@ -0,0 +1,371 @@
+/* SPDX-FileCopyrightText: 2001-2002 NaN Holding BV. All rights reserved.
+ *
+ * SPDX-License-Identifier: GPL-2.0-or-later */
+
+/** \file
+ * \ingroup intern_iksolver
+ */
+
+#include <algorithm>
+#include <cstdio>
+
+#include "IK_QJacobianSolver.h"
+
+// #include "analyze.h"
+IK_QJacobianSolver::IK_QJacobianSolver()
+{
+  m_poleconstraint = false;
+  m_getpoleangle = false;
+  m_rootmatrix.setIdentity();
+}
+
+double IK_QJacobianSolver::ComputeScale()
+{
+  std::vector<IK_QSegment *>::iterator seg;
+  double length = 0.0f;
+
+  for (seg = m_segments.begin(); seg != m_segments.end(); seg++) {
+    length += (*seg)->MaxExtension();
+  }
+
+  if (length == 0.0) {
+    return 1.0;
+  }
+  return 1.0 / length;
+}
+
+void IK_QJacobianSolver::Scale(double scale, std::list<IK_QTask *> &tasks)
+{
+  std::list<IK_QTask *>::iterator task;
+  std::vector<IK_QSegment *>::iterator seg;
+
+  for (task = tasks.begin(); task != tasks.end(); task++) {
+    (*task)->Scale(scale);
+  }
+
+  for (seg = m_segments.begin(); seg != m_segments.end(); seg++) {
+    (*seg)->Scale(scale);
+  }
+
+  m_rootmatrix.translation() *= scale;
+  m_goal *= scale;
+  m_polegoal *= scale;
+}
+
+void IK_QJacobianSolver::AddSegmentList(IK_QSegment *seg)
+{
+  m_segments.push_back(seg);
+
+  IK_QSegment *child;
+  for (child = seg->Child(); child; child = child->Sibling()) {
+    AddSegmentList(child);
+  }
+}
+
+bool IK_QJacobianSolver::Setup(IK_QSegment *root, std::list<IK_QTask *> &tasks)
+{
+  m_segments.clear();
+  AddSegmentList(root);
+
+  // assign each segment a unique id for the jacobian
+  std::vector<IK_QSegment *>::iterator seg;
+  int num_dof = 0;
+
+  for (seg = m_segments.begin(); seg != m_segments.end(); seg++) {
+    (*seg)->SetDoFId(num_dof);
+    num_dof += (*seg)->NumberOfDoF();
+  }
+
+  if (num_dof == 0) {
+    return false;
+  }
+
+  // compute task ids and assign weights to task
+  int primary_size = 0;
+  int secondary_size = 0, secondary = 0;
+  double primary_weight = 0.0, secondary_weight = 0.0;
+  std::list<IK_QTask *>::iterator task;
+
+  for (task = tasks.begin(); task != tasks.end(); task++) {
+    IK_QTask *qtask = *task;
+
+    if (qtask->Primary()) {
+      qtask->SetId(primary_size);
+      primary_size += qtask->Size();
+      primary_weight += qtask->Weight();
+    }
+    else {
+      qtask->SetId(secondary_size);
+      secondary_size += qtask->Size();
+      secondary_weight += qtask->Weight();
+      secondary++;
+    }
+  }
+
+  if (primary_size == 0 || FuzzyZero(primary_weight)) {
+    return false;
+  }
+
+  m_secondary_enabled = (secondary > 0);
+
+  // rescale weights of tasks to sum up to 1
+  double primary_rescale = 1.0 / primary_weight;
+  double secondary_rescale;
+  if (FuzzyZero(secondary_weight)) {
+    secondary_rescale = 0.0;
+  }
+  else {
+    secondary_rescale = 1.0 / secondary_weight;
+  }
+
+  for (task = tasks.begin(); task != tasks.end(); task++) {
+    IK_QTask *qtask = *task;
+
+    if (qtask->Primary()) {
+      qtask->SetWeight(qtask->Weight() * primary_rescale);
+    }
+    else {
+      qtask->SetWeight(qtask->Weight() * secondary_rescale);
+    }
+  }
+
+  // set matrix sizes
+  m_jacobian.ArmMatrices(num_dof, primary_size);
+  if (secondary > 0) {
+    m_jacobian_sub.ArmMatrices(num_dof, secondary_size);
+  }
+
+  // set dof weights
+  int i;
+
+  for (seg = m_segments.begin(); seg != m_segments.end(); seg++) {
+    for (i = 0; i < (*seg)->NumberOfDoF(); i++) {
+      m_jacobian.SetDoFWeight((*seg)->DoFId() + i, (*seg)->Weight(i));
+    }
+  }
+
+  return true;
+}
+
+void IK_QJacobianSolver::SetPoleVectorConstraint(
+    IK_QSegment *tip, Vector3d &goal, Vector3d &polegoal, float poleangle, bool getangle)
+{
+  m_poleconstraint = true;
+  m_poletip = tip;
+  m_goal = goal;
+  m_polegoal = polegoal;
+  m_poleangle = (getangle) ? 0.0f : poleangle;
+  m_getpoleangle = getangle;
+}
+
+void IK_QJacobianSolver::ConstrainPoleVector(IK_QSegment *root, std::list<IK_QTask *> &tasks)
+{
+  // this function will be called before and after solving. calling it before
+  // solving gives predictable solutions by rotating towards the solution,
+  // and calling it afterwards ensures the solution is exact.
+
+  if (!m_poleconstraint) {
+    return;
+  }
+
+  // disable pole vector constraint in case of multiple position tasks
+  std::list<IK_QTask *>::iterator task;
+  int positiontasks = 0;
+
+  for (task = tasks.begin(); task != tasks.end(); task++) {
+    if ((*task)->PositionTask()) {
+      positiontasks++;
+    }
+  }
+
+  if (positiontasks >= 2) {
+    m_poleconstraint = false;
+    return;
+  }
+
+  // get positions and rotations
+  root->UpdateTransform(m_rootmatrix);
+
+  const Vector3d rootpos = root->GlobalStart();
+  const Vector3d endpos = m_poletip->GlobalEnd();
+  const Matrix3d &rootbasis = root->GlobalTransform().linear();
+
+  // construct "lookat" matrices (like gluLookAt), based on a direction and
+  // an up vector, with the direction going from the root to the end effector
+  // and the up vector going from the root to the pole constraint position.
+  Vector3d dir = normalize(endpos - rootpos);
+  Vector3d rootx = rootbasis.col(0);
+  Vector3d rootz = rootbasis.col(2);
+  Vector3d up = rootx * cos(m_poleangle) + rootz * sin(m_poleangle);
+
+  // in post, don't rotate towards the goal but only correct the pole up
+  Vector3d poledir = (m_getpoleangle) ? dir : normalize(m_goal - rootpos);
+  Vector3d poleup = normalize(m_polegoal - rootpos);
+
+  Matrix3d mat, polemat;
+
+  mat.row(0) = normalize(dir.cross(up));
+  mat.row(1) = mat.row(0).cross(dir);
+  mat.row(2) = -dir;
+
+  polemat.row(0) = normalize(poledir.cross(poleup));
+  polemat.row(1) = polemat.row(0).cross(poledir);
+  polemat.row(2) = -poledir;
+
+  if (m_getpoleangle) {
+    // we compute the pole angle that to rotate towards the target
+    m_poleangle = angle(mat.row(1), polemat.row(1));
+
+    double dt = rootz.dot(mat.row(1) * cos(m_poleangle) + mat.row(0) * sin(m_poleangle));
+    if (dt > 0.0) {
+      m_poleangle = -m_poleangle;
+    }
+
+    // solve again, with the pole angle we just computed
+    m_getpoleangle = false;
+    ConstrainPoleVector(root, tasks);
+  }
+  else {
+    // now we set as root matrix the difference between the current and
+    // desired rotation based on the pole vector constraint. we use
+    // transpose instead of inverse because we have orthogonal matrices
+    // anyway, and in case of a singular matrix we don't get NaN's.
+    Affine3d trans;
+    trans.linear() = polemat.transpose() * mat;
+    trans.translation() = Vector3d(0, 0, 0);
+    m_rootmatrix = trans * m_rootmatrix;
+  }
+}
+
+bool IK_QJacobianSolver::UpdateAngles(double &norm)
+{
+  // assign each segment a unique id for the jacobian
+  std::vector<IK_QSegment *>::iterator seg;
+  IK_QSegment *qseg, *minseg = nullptr;
+  double minabsdelta = 1e10, absdelta;
+  Vector3d delta, mindelta;
+  bool locked = false, clamp[3];
+  int i, mindof = 0;
+
+  // here we check if any angle limits were violated. angles whose clamped
+  // position is the same as it was before, are locked immediate. of the
+  // other violation angles the most violating angle is rememberd
+  for (seg = m_segments.begin(); seg != m_segments.end(); seg++) {
+    qseg = *seg;
+    if (qseg->UpdateAngle(m_jacobian, delta, clamp)) {
+      for (i = 0; i < qseg->NumberOfDoF(); i++) {
+        if (clamp[i] && !qseg->Locked(i)) {
+          absdelta = fabs(delta[i]);
+
+          if (absdelta < IK_EPSILON) {
+            qseg->Lock(i, m_jacobian, delta);
+            locked = true;
+          }
+          else if (absdelta < minabsdelta) {
+            minabsdelta = absdelta;
+            mindelta = delta;
+            minseg = qseg;
+            mindof = i;
+          }
+        }
+      }
+    }
+  }
+
+  // lock most violating angle
+  if (minseg) {
+    minseg->Lock(mindof, m_jacobian, mindelta);
+    locked = true;
+
+    norm = std::max(minabsdelta, norm);
+  }
+
+  if (locked == false) {
+    // no locking done, last inner iteration, apply the angles
+    for (seg = m_segments.begin(); seg != m_segments.end(); seg++) {
+      (*seg)->UnLock();
+      (*seg)->UpdateAngleApply();
+    }
+  }
+
+  // signal if another inner iteration is needed
+  return locked;
+}
+
+bool IK_QJacobianSolver::Solve(IK_QSegment *root,
+                               std::list<IK_QTask *> tasks,
+                               const double /*tolerance*/,
+                               const int max_iterations)
+{
+  float scale = ComputeScale();
+  bool solved = false;
+  // double dt = analyze_time();
+
+  Scale(scale, tasks);
+
+  ConstrainPoleVector(root, tasks);
+
+  root->UpdateTransform(m_rootmatrix);
+
+  // iterate
+  for (int iterations = 0; iterations < max_iterations; iterations++) {
+    // update transform
+    root->UpdateTransform(m_rootmatrix);
+
+    std::list<IK_QTask *>::iterator task;
+
+    // compute jacobian
+    for (task = tasks.begin(); task != tasks.end(); task++) {
+      if ((*task)->Primary()) {
+        (*task)->ComputeJacobian(m_jacobian);
+      }
+      else {
+        (*task)->ComputeJacobian(m_jacobian_sub);
+      }
+    }
+
+    double norm = 0.0;
+
+    do {
+      // invert jacobian
+      try {
+        m_jacobian.Invert();
+        if (m_secondary_enabled) {
+          m_jacobian.SubTask(m_jacobian_sub);
+        }
+      }
+      catch (...) {
+        fprintf(stderr, "IK Exception\n");
+        return false;
+      }
+
+      // update angles and check limits
+    } while (UpdateAngles(norm));
+
+    // unlock segments again after locking in clamping loop
+    std::vector<IK_QSegment *>::iterator seg;
+    for (seg = m_segments.begin(); seg != m_segments.end(); seg++) {
+      (*seg)->UnLock();
+    }
+
+    // compute angle update norm
+    double maxnorm = m_jacobian.AngleUpdateNorm();
+    norm = std::max(maxnorm, norm);
+
+    // check for convergence
+    if (norm < 1e-3 && iterations > 10) {
+      solved = true;
+      break;
+    }
+  }
+
+  if (m_poleconstraint) {
+    root->PrependBasis(m_rootmatrix.linear());
+  }
+
+  Scale(1.0f / scale, tasks);
+
+  // analyze_add_run(max_iterations, analyze_time()-dt);
+
+  return solved;
+}

blender-5.2.0/intern/iksolver/intern/IK_QJacobianSolver.h

@@ -0,0 +1,69 @@
+/* SPDX-FileCopyrightText: 2001-2002 NaN Holding BV. All rights reserved.
+ *
+ * SPDX-License-Identifier: GPL-2.0-or-later */
+
+/** \file
+ * \ingroup intern_iksolver
+ */
+
+#pragma once
+
+/**
+ * @author Laurence Bourn
+ * @date 28/6/2001
+ */
+
+#include <list>
+#include <vector>
+
+#include "IK_Math.h"
+#include "IK_QJacobian.h"
+#include "IK_QSegment.h"
+#include "IK_QTask.h"
+
+class IK_QJacobianSolver {
+ public:
+  IK_QJacobianSolver();
+  ~IK_QJacobianSolver() = default;
+
+  // setup pole vector constraint
+  void SetPoleVectorConstraint(
+      IK_QSegment *tip, Vector3d &goal, Vector3d &polegoal, float poleangle, bool getangle);
+  float GetPoleAngle()
+  {
+    return m_poleangle;
+  }
+
+  // call setup once before solving, if it fails don't solve
+  bool Setup(IK_QSegment *root, std::list<IK_QTask *> &tasks);
+
+  // returns true if converged, false if max number of iterations was used
+  bool Solve(IK_QSegment *root,
+             std::list<IK_QTask *> tasks,
+             const double tolerance,
+             const int max_iterations);
+
+ private:
+  void AddSegmentList(IK_QSegment *seg);
+  bool UpdateAngles(double &norm);
+  void ConstrainPoleVector(IK_QSegment *root, std::list<IK_QTask *> &tasks);
+
+  double ComputeScale();
+  void Scale(double scale, std::list<IK_QTask *> &tasks);
+
+  IK_QJacobian m_jacobian;
+  IK_QJacobian m_jacobian_sub;
+
+  bool m_secondary_enabled;
+
+  std::vector<IK_QSegment *> m_segments;
+
+  Affine3d m_rootmatrix;
+
+  bool m_poleconstraint;
+  bool m_getpoleangle;
+  Vector3d m_goal;
+  Vector3d m_polegoal;
+  float m_poleangle;
+  IK_QSegment *m_poletip;
+};

blender-5.2.0/intern/iksolver/intern/IK_QSegment.cpp

@@ -0,0 +1,892 @@
+/* SPDX-FileCopyrightText: 2001-2002 NaN Holding BV. All rights reserved.
+ *
+ * SPDX-License-Identifier: GPL-2.0-or-later */
+
+/** \file
+ * \ingroup intern_iksolver
+ */
+
+#include "IK_QSegment.h"
+
+// IK_QSegment
+
+IK_QSegment::IK_QSegment(int num_DoF, bool translational)
+    : m_parent(nullptr),
+      m_child(nullptr),
+      m_sibling(nullptr),
+      m_composite(nullptr),
+      m_num_DoF(num_DoF),
+      m_translational(translational)
+{
+  m_locked[0] = m_locked[1] = m_locked[2] = false;
+  m_weight[0] = m_weight[1] = m_weight[2] = 1.0;
+
+  m_max_extension = 0.0;
+
+  m_start = Vector3d(0, 0, 0);
+  m_rest_basis.setIdentity();
+  m_basis.setIdentity();
+  m_translation = Vector3d(0, 0, 0);
+
+  m_orig_basis = m_basis;
+  m_orig_translation = m_translation;
+}
+
+void IK_QSegment::Reset()
+{
+  m_locked[0] = m_locked[1] = m_locked[2] = false;
+
+  m_basis = m_orig_basis;
+  m_translation = m_orig_translation;
+  SetBasis(m_basis);
+
+  for (IK_QSegment *seg = m_child; seg; seg = seg->m_sibling) {
+    seg->Reset();
+  }
+}
+
+void IK_QSegment::SetTransform(const Vector3d &start,
+                               const Matrix3d &rest_basis,
+                               const Matrix3d &basis,
+                               const double length)
+{
+  m_max_extension = start.norm() + length;
+
+  m_start = start;
+  m_rest_basis = rest_basis;
+
+  m_orig_basis = basis;
+  SetBasis(basis);
+
+  m_translation = Vector3d(0, length, 0);
+  m_orig_translation = m_translation;
+}
+
+Matrix3d IK_QSegment::BasisChange() const
+{
+  return m_orig_basis.transpose() * m_basis;
+}
+
+Vector3d IK_QSegment::TranslationChange() const
+{
+  return m_translation - m_orig_translation;
+}
+
+IK_QSegment::~IK_QSegment()
+{
+  if (m_parent) {
+    m_parent->RemoveChild(this);
+  }
+
+  for (IK_QSegment *seg = m_child; seg; seg = seg->m_sibling) {
+    seg->m_parent = nullptr;
+  }
+}
+
+void IK_QSegment::SetParent(IK_QSegment *parent)
+{
+  if (m_parent == parent) {
+    return;
+  }
+
+  if (m_parent) {
+    m_parent->RemoveChild(this);
+  }
+
+  if (parent) {
+    m_sibling = parent->m_child;
+    parent->m_child = this;
+  }
+
+  m_parent = parent;
+}
+
+void IK_QSegment::SetComposite(IK_QSegment *seg)
+{
+  m_composite = seg;
+}
+
+void IK_QSegment::RemoveChild(IK_QSegment *child)
+{
+  if (m_child == nullptr) {
+    return;
+  }
+  if (m_child == child) {
+    m_child = m_child->m_sibling;
+  }
+  else {
+    IK_QSegment *seg = m_child;
+
+    while (seg->m_sibling != child) {
+      seg = seg->m_sibling;
+    }
+
+    if (child == seg->m_sibling) {
+      seg->m_sibling = child->m_sibling;
+    }
+  }
+}
+
+void IK_QSegment::UpdateTransform(const Affine3d &global)
+{
+  // compute the global transform at the end of the segment
+  m_global_start = global.translation() + global.linear() * m_start;
+
+  m_global_transform.translation() = m_global_start;
+  m_global_transform.linear() = global.linear() * m_rest_basis * m_basis;
+  m_global_transform.translate(m_translation);
+
+  // update child transforms
+  for (IK_QSegment *seg = m_child; seg; seg = seg->m_sibling) {
+    seg->UpdateTransform(m_global_transform);
+  }
+}
+
+void IK_QSegment::PrependBasis(const Matrix3d &mat)
+{
+  m_basis = m_rest_basis.inverse() * mat * m_rest_basis * m_basis;
+}
+
+void IK_QSegment::Scale(double scale)
+{
+  m_start *= scale;
+  m_translation *= scale;
+  m_orig_translation *= scale;
+  m_global_start *= scale;
+  m_global_transform.translation() *= scale;
+  m_max_extension *= scale;
+}
+
+// IK_QSphericalSegment
+
+IK_QSphericalSegment::IK_QSphericalSegment()
+    : IK_QSegment(3, false), m_limit_x(false), m_limit_y(false), m_limit_z(false)
+{
+}
+
+Vector3d IK_QSphericalSegment::Axis(int dof) const
+{
+  return m_global_transform.linear().col(dof);
+}
+
+void IK_QSphericalSegment::SetLimit(int axis, double lmin, double lmax)
+{
+  if (lmin > lmax) {
+    return;
+  }
+
+  if (axis == 1) {
+    lmin = Clamp(lmin, -M_PI, M_PI);
+    lmax = Clamp(lmax, -M_PI, M_PI);
+
+    m_min_y = lmin;
+    m_max_y = lmax;
+
+    m_limit_y = true;
+  }
+  else {
+    // clamp and convert to axis angle parameters
+    lmin = Clamp(lmin, -M_PI, M_PI);
+    lmax = Clamp(lmax, -M_PI, M_PI);
+
+    lmin = sin(lmin * 0.5);
+    lmax = sin(lmax * 0.5);
+
+    if (axis == 0) {
+      m_min[0] = -lmax;
+      m_max[0] = -lmin;
+      m_limit_x = true;
+    }
+    else if (axis == 2) {
+      m_min[1] = -lmax;
+      m_max[1] = -lmin;
+      m_limit_z = true;
+    }
+  }
+}
+
+void IK_QSphericalSegment::SetWeight(int axis, double weight)
+{
+  m_weight[axis] = weight;
+}
+
+bool IK_QSphericalSegment::UpdateAngle(const IK_QJacobian &jacobian, Vector3d &delta, bool *clamp)
+{
+  if (m_locked[0] && m_locked[1] && m_locked[2]) {
+    return false;
+  }
+
+  Vector3d dq;
+  dq.x() = jacobian.AngleUpdate(m_DoF_id);
+  dq.y() = jacobian.AngleUpdate(m_DoF_id + 1);
+  dq.z() = jacobian.AngleUpdate(m_DoF_id + 2);
+
+  // Directly update the rotation matrix, with Rodrigues' rotation formula,
+  // to avoid singularities and allow smooth integration.
+
+  double theta = dq.norm();
+
+  if (!FuzzyZero(theta)) {
+    Vector3d w = dq * (1.0 / theta);
+
+    double sine = sin(theta);
+    double cosine = cos(theta);
+    double cosineInv = 1 - cosine;
+
+    double xsine = w.x() * sine;
+    double ysine = w.y() * sine;
+    double zsine = w.z() * sine;
+
+    double xxcosine = w.x() * w.x() * cosineInv;
+    double xycosine = w.x() * w.y() * cosineInv;
+    double xzcosine = w.x() * w.z() * cosineInv;
+    double yycosine = w.y() * w.y() * cosineInv;
+    double yzcosine = w.y() * w.z() * cosineInv;
+    double zzcosine = w.z() * w.z() * cosineInv;
+
+    Matrix3d M = CreateMatrix(cosine + xxcosine,
+                              -zsine + xycosine,
+                              ysine + xzcosine,
+                              zsine + xycosine,
+                              cosine + yycosine,
+                              -xsine + yzcosine,
+                              -ysine + xzcosine,
+                              xsine + yzcosine,
+                              cosine + zzcosine);
+
+    m_new_basis = m_basis * M;
+  }
+  else {
+    m_new_basis = m_basis;
+  }
+
+  if (m_limit_y == false && m_limit_x == false && m_limit_z == false) {
+    return false;
+  }
+
+  Vector3d a = SphericalRangeParameters(m_new_basis);
+
+  if (m_locked[0]) {
+    a.x() = m_locked_ax;
+  }
+  if (m_locked[1]) {
+    a.y() = m_locked_ay;
+  }
+  if (m_locked[2]) {
+    a.z() = m_locked_az;
+  }
+
+  double ax = a.x(), ay = a.y(), az = a.z();
+
+  clamp[0] = clamp[1] = clamp[2] = false;
+
+  if (m_limit_y) {
+    if (a.y() > m_max_y) {
+      ay = m_max_y;
+      clamp[1] = true;
+    }
+    else if (a.y() < m_min_y) {
+      ay = m_min_y;
+      clamp[1] = true;
+    }
+  }
+
+  if (m_limit_x && m_limit_z) {
+    if (EllipseClamp(ax, az, m_min, m_max)) {
+      clamp[0] = clamp[2] = true;
+    }
+  }
+  else if (m_limit_x) {
+    if (ax < m_min[0]) {
+      ax = m_min[0];
+      clamp[0] = true;
+    }
+    else if (ax > m_max[0]) {
+      ax = m_max[0];
+      clamp[0] = true;
+    }
+  }
+  else if (m_limit_z) {
+    if (az < m_min[1]) {
+      az = m_min[1];
+      clamp[2] = true;
+    }
+    else if (az > m_max[1]) {
+      az = m_max[1];
+      clamp[2] = true;
+    }
+  }
+
+  if (clamp[0] == false && clamp[1] == false && clamp[2] == false) {
+    if (m_locked[0] || m_locked[1] || m_locked[2]) {
+      m_new_basis = ComputeSwingMatrix(ax, az) * ComputeTwistMatrix(ay);
+    }
+    return false;
+  }
+
+  m_new_basis = ComputeSwingMatrix(ax, az) * ComputeTwistMatrix(ay);
+
+  delta = MatrixToAxisAngle(m_basis.transpose() * m_new_basis);
+
+  if (!(m_locked[0] || m_locked[2]) && (clamp[0] || clamp[2])) {
+    m_locked_ax = ax;
+    m_locked_az = az;
+  }
+
+  if (!m_locked[1] && clamp[1]) {
+    m_locked_ay = ay;
+  }
+
+  return true;
+}
+
+void IK_QSphericalSegment::Lock(int dof, IK_QJacobian &jacobian, Vector3d &delta)
+{
+  if (dof == 1) {
+    m_locked[1] = true;
+    jacobian.Lock(m_DoF_id + 1, delta[1]);
+  }
+  else {
+    m_locked[0] = m_locked[2] = true;
+    jacobian.Lock(m_DoF_id, delta[0]);
+    jacobian.Lock(m_DoF_id + 2, delta[2]);
+  }
+}
+
+void IK_QSphericalSegment::UpdateAngleApply()
+{
+  m_basis = m_new_basis;
+}
+
+// IK_QNullSegment
+
+IK_QNullSegment::IK_QNullSegment() : IK_QSegment(0, false) {}
+
+// IK_QRevoluteSegment
+
+IK_QRevoluteSegment::IK_QRevoluteSegment(int axis)
+    : IK_QSegment(1, false), m_axis(axis), m_angle(0.0), m_limit(false)
+{
+}
+
+void IK_QRevoluteSegment::SetBasis(const Matrix3d &basis)
+{
+  if (m_axis == 1) {
+    m_angle = ComputeTwist(basis);
+    m_basis = ComputeTwistMatrix(m_angle);
+  }
+  else {
+    m_angle = EulerAngleFromMatrix(basis, m_axis);
+    m_basis = RotationMatrix(m_angle, m_axis);
+  }
+}
+
+Vector3d IK_QRevoluteSegment::Axis(int /*dof*/) const
+{
+  return m_global_transform.linear().col(m_axis);
+}
+
+bool IK_QRevoluteSegment::UpdateAngle(const IK_QJacobian &jacobian, Vector3d &delta, bool *clamp)
+{
+  if (m_locked[0]) {
+    return false;
+  }
+
+  m_new_angle = m_angle + jacobian.AngleUpdate(m_DoF_id);
+
+  clamp[0] = false;
+
+  if (m_limit == false) {
+    return false;
+  }
+
+  if (m_new_angle > m_max) {
+    delta[0] = m_max - m_angle;
+  }
+  else if (m_new_angle < m_min) {
+    delta[0] = m_min - m_angle;
+  }
+  else {
+    return false;
+  }
+
+  clamp[0] = true;
+  m_new_angle = m_angle + delta[0];
+
+  return true;
+}
+
+void IK_QRevoluteSegment::Lock(int /*dof*/, IK_QJacobian &jacobian, Vector3d &delta)
+{
+  m_locked[0] = true;
+  jacobian.Lock(m_DoF_id, delta[0]);
+}
+
+void IK_QRevoluteSegment::UpdateAngleApply()
+{
+  m_angle = m_new_angle;
+  m_basis = RotationMatrix(m_angle, m_axis);
+}
+
+void IK_QRevoluteSegment::SetLimit(int axis, double lmin, double lmax)
+{
+  if (lmin > lmax || m_axis != axis) {
+    return;
+  }
+
+  // clamp and convert to axis angle parameters
+  lmin = Clamp(lmin, -M_PI, M_PI);
+  lmax = Clamp(lmax, -M_PI, M_PI);
+
+  m_min = lmin;
+  m_max = lmax;
+
+  m_limit = true;
+}
+
+void IK_QRevoluteSegment::SetWeight(int axis, double weight)
+{
+  if (axis == m_axis) {
+    m_weight[0] = weight;
+  }
+}
+
+// IK_QSwingSegment
+
+IK_QSwingSegment::IK_QSwingSegment() : IK_QSegment(2, false), m_limit_x(false), m_limit_z(false) {}
+
+void IK_QSwingSegment::SetBasis(const Matrix3d &basis)
+{
+  m_basis = basis;
+  RemoveTwist(m_basis);
+}
+
+Vector3d IK_QSwingSegment::Axis(int dof) const
+{
+  return m_global_transform.linear().col((dof == 0) ? 0 : 2);
+}
+
+bool IK_QSwingSegment::UpdateAngle(const IK_QJacobian &jacobian, Vector3d &delta, bool *clamp)
+{
+  if (m_locked[0] && m_locked[1]) {
+    return false;
+  }
+
+  Vector3d dq;
+  dq.x() = jacobian.AngleUpdate(m_DoF_id);
+  dq.y() = 0.0;
+  dq.z() = jacobian.AngleUpdate(m_DoF_id + 1);
+
+  // Directly update the rotation matrix, with Rodrigues' rotation formula,
+  // to avoid singularities and allow smooth integration.
+
+  double theta = dq.norm();
+
+  if (!FuzzyZero(theta)) {
+    Vector3d w = dq * (1.0 / theta);
+
+    double sine = sin(theta);
+    double cosine = cos(theta);
+    double cosineInv = 1 - cosine;
+
+    double xsine = w.x() * sine;
+    double zsine = w.z() * sine;
+
+    double xxcosine = w.x() * w.x() * cosineInv;
+    double xzcosine = w.x() * w.z() * cosineInv;
+    double zzcosine = w.z() * w.z() * cosineInv;
+
+    Matrix3d M = CreateMatrix(cosine + xxcosine,
+                              -zsine,
+                              xzcosine,
+                              zsine,
+                              cosine,
+                              -xsine,
+                              xzcosine,
+                              xsine,
+                              cosine + zzcosine);
+
+    m_new_basis = m_basis * M;
+
+    RemoveTwist(m_new_basis);
+  }
+  else {
+    m_new_basis = m_basis;
+  }
+
+  if (m_limit_x == false && m_limit_z == false) {
+    return false;
+  }
+
+  Vector3d a = SphericalRangeParameters(m_new_basis);
+  double ax = 0, az = 0;
+
+  clamp[0] = clamp[1] = false;
+
+  if (m_limit_x && m_limit_z) {
+    ax = a.x();
+    az = a.z();
+
+    if (EllipseClamp(ax, az, m_min, m_max)) {
+      clamp[0] = clamp[1] = true;
+    }
+  }
+  else if (m_limit_x) {
+    if (ax < m_min[0]) {
+      ax = m_min[0];
+      clamp[0] = true;
+    }
+    else if (ax > m_max[0]) {
+      ax = m_max[0];
+      clamp[0] = true;
+    }
+  }
+  else if (m_limit_z) {
+    if (az < m_min[1]) {
+      az = m_min[1];
+      clamp[1] = true;
+    }
+    else if (az > m_max[1]) {
+      az = m_max[1];
+      clamp[1] = true;
+    }
+  }
+
+  if (clamp[0] == false && clamp[1] == false) {
+    return false;
+  }
+
+  m_new_basis = ComputeSwingMatrix(ax, az);
+
+  delta = MatrixToAxisAngle(m_basis.transpose() * m_new_basis);
+  delta[1] = delta[2];
+  delta[2] = 0.0;
+
+  return true;
+}
+
+void IK_QSwingSegment::Lock(int /*dof*/, IK_QJacobian &jacobian, Vector3d &delta)
+{
+  m_locked[0] = m_locked[1] = true;
+  jacobian.Lock(m_DoF_id, delta[0]);
+  jacobian.Lock(m_DoF_id + 1, delta[1]);
+}
+
+void IK_QSwingSegment::UpdateAngleApply()
+{
+  m_basis = m_new_basis;
+}
+
+void IK_QSwingSegment::SetLimit(int axis, double lmin, double lmax)
+{
+  if (lmin > lmax) {
+    return;
+  }
+
+  // clamp and convert to axis angle parameters
+  lmin = Clamp(lmin, -M_PI, M_PI);
+  lmax = Clamp(lmax, -M_PI, M_PI);
+
+  lmin = sin(lmin * 0.5);
+  lmax = sin(lmax * 0.5);
+
+  // put center of ellispe in the middle between min and max
+  double offset = 0.5 * (lmin + lmax);
+  // lmax = lmax - offset;
+
+  if (axis == 0) {
+    m_min[0] = -lmax;
+    m_max[0] = -lmin;
+
+    m_limit_x = true;
+    m_offset_x = offset;
+    m_max_x = lmax;
+  }
+  else if (axis == 2) {
+    m_min[1] = -lmax;
+    m_max[1] = -lmin;
+
+    m_limit_z = true;
+    m_offset_z = offset;
+    m_max_z = lmax;
+  }
+}
+
+void IK_QSwingSegment::SetWeight(int axis, double weight)
+{
+  if (axis == 0) {
+    m_weight[0] = weight;
+  }
+  else if (axis == 2) {
+    m_weight[1] = weight;
+  }
+}
+
+// IK_QElbowSegment
+
+IK_QElbowSegment::IK_QElbowSegment(int axis)
+    : IK_QSegment(2, false),
+      m_axis(axis),
+      m_twist(0.0),
+      m_angle(0.0),
+      m_cos_twist(1.0),
+      m_sin_twist(0.0),
+      m_limit(false),
+      m_limit_twist(false)
+{
+}
+
+void IK_QElbowSegment::SetBasis(const Matrix3d &basis)
+{
+  m_basis = basis;
+
+  m_twist = ComputeTwist(m_basis);
+  RemoveTwist(m_basis);
+  m_angle = EulerAngleFromMatrix(basis, m_axis);
+
+  m_basis = RotationMatrix(m_angle, m_axis) * ComputeTwistMatrix(m_twist);
+}
+
+Vector3d IK_QElbowSegment::Axis(int dof) const
+{
+  if (dof == 0) {
+    Vector3d v;
+    if (m_axis == 0) {
+      v = Vector3d(m_cos_twist, 0, m_sin_twist);
+    }
+    else {
+      v = Vector3d(-m_sin_twist, 0, m_cos_twist);
+    }
+
+    return m_global_transform.linear() * v;
+  }
+  return m_global_transform.linear().col(1);
+}
+
+bool IK_QElbowSegment::UpdateAngle(const IK_QJacobian &jacobian, Vector3d &delta, bool *clamp)
+{
+  if (m_locked[0] && m_locked[1]) {
+    return false;
+  }
+
+  clamp[0] = clamp[1] = false;
+
+  if (!m_locked[0]) {
+    m_new_angle = m_angle + jacobian.AngleUpdate(m_DoF_id);
+
+    if (m_limit) {
+      if (m_new_angle > m_max) {
+        delta[0] = m_max - m_angle;
+        m_new_angle = m_max;
+        clamp[0] = true;
+      }
+      else if (m_new_angle < m_min) {
+        delta[0] = m_min - m_angle;
+        m_new_angle = m_min;
+        clamp[0] = true;
+      }
+    }
+  }
+
+  if (!m_locked[1]) {
+    m_new_twist = m_twist + jacobian.AngleUpdate(m_DoF_id + 1);
+
+    if (m_limit_twist) {
+      if (m_new_twist > m_max_twist) {
+        delta[1] = m_max_twist - m_twist;
+        m_new_twist = m_max_twist;
+        clamp[1] = true;
+      }
+      else if (m_new_twist < m_min_twist) {
+        delta[1] = m_min_twist - m_twist;
+        m_new_twist = m_min_twist;
+        clamp[1] = true;
+      }
+    }
+  }
+
+  return (clamp[0] || clamp[1]);
+}
+
+void IK_QElbowSegment::Lock(int dof, IK_QJacobian &jacobian, Vector3d &delta)
+{
+  if (dof == 0) {
+    m_locked[0] = true;
+    jacobian.Lock(m_DoF_id, delta[0]);
+  }
+  else {
+    m_locked[1] = true;
+    jacobian.Lock(m_DoF_id + 1, delta[1]);
+  }
+}
+
+void IK_QElbowSegment::UpdateAngleApply()
+{
+  m_angle = m_new_angle;
+  m_twist = m_new_twist;
+
+  m_sin_twist = sin(m_twist);
+  m_cos_twist = cos(m_twist);
+
+  Matrix3d A = RotationMatrix(m_angle, m_axis);
+  Matrix3d T = RotationMatrix(m_sin_twist, m_cos_twist, 1);
+
+  m_basis = A * T;
+}
+
+void IK_QElbowSegment::SetLimit(int axis, double lmin, double lmax)
+{
+  if (lmin > lmax) {
+    return;
+  }
+
+  // clamp and convert to axis angle parameters
+  lmin = Clamp(lmin, -M_PI, M_PI);
+  lmax = Clamp(lmax, -M_PI, M_PI);
+
+  if (axis == 1) {
+    m_min_twist = lmin;
+    m_max_twist = lmax;
+    m_limit_twist = true;
+  }
+  else if (axis == m_axis) {
+    m_min = lmin;
+    m_max = lmax;
+    m_limit = true;
+  }
+}
+
+void IK_QElbowSegment::SetWeight(int axis, double weight)
+{
+  if (axis == m_axis) {
+    m_weight[0] = weight;
+  }
+  else if (axis == 1) {
+    m_weight[1] = weight;
+  }
+}
+
+// IK_QTranslateSegment
+
+IK_QTranslateSegment::IK_QTranslateSegment(int axis1) : IK_QSegment(1, true)
+{
+  m_axis_enabled[0] = m_axis_enabled[1] = m_axis_enabled[2] = false;
+  m_axis_enabled[axis1] = true;
+
+  m_axis[0] = axis1;
+
+  m_limit[0] = m_limit[1] = m_limit[2] = false;
+}
+
+IK_QTranslateSegment::IK_QTranslateSegment(int axis1, int axis2) : IK_QSegment(2, true)
+{
+  m_axis_enabled[0] = m_axis_enabled[1] = m_axis_enabled[2] = false;
+  m_axis_enabled[axis1] = true;
+  m_axis_enabled[axis2] = true;
+
+  m_axis[0] = axis1;
+  m_axis[1] = axis2;
+
+  m_limit[0] = m_limit[1] = m_limit[2] = false;
+}
+
+IK_QTranslateSegment::IK_QTranslateSegment() : IK_QSegment(3, true)
+{
+  m_axis_enabled[0] = m_axis_enabled[1] = m_axis_enabled[2] = true;
+
+  m_axis[0] = 0;
+  m_axis[1] = 1;
+  m_axis[2] = 2;
+
+  m_limit[0] = m_limit[1] = m_limit[2] = false;
+}
+
+Vector3d IK_QTranslateSegment::Axis(int dof) const
+{
+  return m_global_transform.linear().col(m_axis[dof]);
+}
+
+bool IK_QTranslateSegment::UpdateAngle(const IK_QJacobian &jacobian, Vector3d &delta, bool *clamp)
+{
+  int dof_id = m_DoF_id, dof = 0, i, clamped = false;
+
+  Vector3d dx(0.0, 0.0, 0.0);
+
+  for (i = 0; i < 3; i++) {
+    if (!m_axis_enabled[i]) {
+      m_new_translation[i] = m_translation[i];
+      continue;
+    }
+
+    clamp[dof] = false;
+
+    if (!m_locked[dof]) {
+      m_new_translation[i] = m_translation[i] + jacobian.AngleUpdate(dof_id);
+
+      if (m_limit[i]) {
+        if (m_new_translation[i] > m_max[i]) {
+          delta[dof] = m_max[i] - m_translation[i];
+          m_new_translation[i] = m_max[i];
+          clamped = clamp[dof] = true;
+        }
+        else if (m_new_translation[i] < m_min[i]) {
+          delta[dof] = m_min[i] - m_translation[i];
+          m_new_translation[i] = m_min[i];
+          clamped = clamp[dof] = true;
+        }
+      }
+    }
+
+    dof_id++;
+    dof++;
+  }
+
+  return clamped;
+}
+
+void IK_QTranslateSegment::UpdateAngleApply()
+{
+  m_translation = m_new_translation;
+}
+
+void IK_QTranslateSegment::Lock(int dof, IK_QJacobian &jacobian, Vector3d &delta)
+{
+  m_locked[dof] = true;
+  jacobian.Lock(m_DoF_id + dof, delta[dof]);
+}
+
+void IK_QTranslateSegment::SetWeight(int axis, double weight)
+{
+  int i;
+
+  for (i = 0; i < m_num_DoF; i++) {
+    if (m_axis[i] == axis) {
+      m_weight[i] = weight;
+    }
+  }
+}
+
+void IK_QTranslateSegment::SetLimit(int axis, double lmin, double lmax)
+{
+  if (lmax < lmin) {
+    return;
+  }
+
+  m_min[axis] = lmin;
+  m_max[axis] = lmax;
+  m_limit[axis] = true;
+}
+
+void IK_QTranslateSegment::Scale(double scale)
+{
+  int i;
+
+  IK_QSegment::Scale(scale);
+
+  for (i = 0; i < 3; i++) {
+    m_min[0] *= scale;
+    m_max[1] *= scale;
+  }
+
+  m_new_translation *= scale;
+}

blender-5.2.0/intern/iksolver/intern/IK_QSegment.h

@@ -0,0 +1,354 @@
+/* SPDX-FileCopyrightText: 2001-2002 NaN Holding BV. All rights reserved.
+ *
+ * SPDX-License-Identifier: GPL-2.0-or-later */
+
+/** \file
+ * \ingroup intern_iksolver
+ */
+
+#pragma once
+
+#include "IK_Math.h"
+#include "IK_QJacobian.h"
+
+/**
+ * An IK_Qsegment encodes information about a segments
+ * local coordinate system.
+ *
+ * These segments always point along the y-axis.
+ *
+ * Here we define the local coordinates of a joint as
+ * local_transform =
+ * translate(tr1) * rotation(A) * rotation(q) * translate(0,length,0)
+ * You can read this as:
+ * - first translate by (0,length,0)
+ * - multiply by the rotation matrix derived from the current
+ * angle parameterization q.
+ * - multiply by the user defined matrix representing the rest
+ * position of the bone.
+ * - translate by the used defined translation (tr1)
+ * The ordering of these transformations is vital, you must
+ * use exactly the same transformations when displaying the segments
+ */
+class IK_QSegment {
+ public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  virtual ~IK_QSegment();
+
+  // start: a user defined translation
+  // rest_basis: a user defined rotation
+  // basis: a user defined rotation
+  // length: length of this segment
+
+  void SetTransform(const Vector3d &start,
+                    const Matrix3d &rest_basis,
+                    const Matrix3d &basis,
+                    const double length);
+
+  // tree structure access
+  void SetParent(IK_QSegment *parent);
+
+  IK_QSegment *Child() const
+  {
+    return m_child;
+  }
+
+  IK_QSegment *Sibling() const
+  {
+    return m_sibling;
+  }
+
+  IK_QSegment *Parent() const
+  {
+    return m_parent;
+  }
+
+  // for combining two joints into one from the interface
+  void SetComposite(IK_QSegment *seg);
+
+  IK_QSegment *Composite() const
+  {
+    return m_composite;
+  }
+
+  // number of degrees of freedom
+  int NumberOfDoF() const
+  {
+    return m_num_DoF;
+  }
+
+  // unique id for this segment, for identification in the jacobian
+  int DoFId() const
+  {
+    return m_DoF_id;
+  }
+
+  void SetDoFId(int dof_id)
+  {
+    m_DoF_id = dof_id;
+  }
+
+  // the max distance of the end of this bone from the local origin.
+  double MaxExtension() const
+  {
+    return m_max_extension;
+  }
+
+  // the change in rotation and translation w.r.t. the rest pose
+  Matrix3d BasisChange() const;
+  Vector3d TranslationChange() const;
+
+  // the start and end of the segment
+  Vector3d GlobalStart() const
+  {
+    return m_global_start;
+  }
+
+  Vector3d GlobalEnd() const
+  {
+    return m_global_transform.translation();
+  }
+
+  // the global transformation at the end of the segment
+  const Affine3d &GlobalTransform() const
+  {
+    return m_global_transform;
+  }
+
+  // is a translational segment?
+  bool Translational() const
+  {
+    return m_translational;
+  }
+
+  // locking (during inner clamping loop)
+  bool Locked(int dof) const
+  {
+    return m_locked[dof];
+  }
+
+  void UnLock()
+  {
+    m_locked[0] = m_locked[1] = m_locked[2] = false;
+  }
+
+  // per dof joint weighting
+  double Weight(int dof) const
+  {
+    return m_weight[dof];
+  }
+
+  void ScaleWeight(int dof, double scale)
+  {
+    m_weight[dof] *= scale;
+  }
+
+  // recursively update the global coordinates of this segment, 'global'
+  // is the global transformation from the parent segment
+  void UpdateTransform(const Affine3d &global);
+
+  // get axis from rotation matrix for derivative computation
+  virtual Vector3d Axis(int dof) const = 0;
+
+  // update the angles using the dTheta's computed using the jacobian matrix
+  virtual bool UpdateAngle(const IK_QJacobian &, Vector3d &, bool *) = 0;
+  virtual void Lock(int /*dof*/, IK_QJacobian & /*jacobian*/, Vector3d & /*delta*/) {}
+  virtual void UpdateAngleApply() = 0;
+
+  // set joint limits
+  virtual void SetLimit(int /*axis*/, double /*lmin*/, double /*lmmax*/) {}
+
+  // set joint weights (per axis)
+  virtual void SetWeight(int /*axis*/, double /*weight*/) {}
+
+  virtual void SetBasis(const Matrix3d &basis)
+  {
+    m_basis = basis;
+  }
+
+  // functions needed for pole vector constraint
+  void PrependBasis(const Matrix3d &mat);
+  void Reset();
+
+  // scale
+  virtual void Scale(double scale);
+
+ protected:
+  // num_DoF: number of degrees of freedom
+  IK_QSegment(int num_DoF, bool translational);
+
+  // remove child as a child of this segment
+  void extracted();
+  void RemoveChild(IK_QSegment *child);
+
+  // tree structure variables
+  IK_QSegment *m_parent;
+  IK_QSegment *m_child;
+  IK_QSegment *m_sibling;
+  IK_QSegment *m_composite;
+
+  // full transform =
+  // start * rest_basis * basis * translation
+  Vector3d m_start;
+  Matrix3d m_rest_basis;
+  Matrix3d m_basis;
+  Vector3d m_translation;
+
+  // original basis
+  Matrix3d m_orig_basis;
+  Vector3d m_orig_translation;
+
+  // maximum extension of this segment
+  double m_max_extension;
+
+  // accumulated transformations starting from root
+  Vector3d m_global_start;
+  Affine3d m_global_transform;
+
+  // number degrees of freedom, (first) id of this segments DOF's
+  int m_num_DoF, m_DoF_id;
+
+  bool m_locked[3];
+  bool m_translational;
+  double m_weight[3];
+};
+
+class IK_QSphericalSegment : public IK_QSegment {
+ public:
+  IK_QSphericalSegment();
+
+  Vector3d Axis(int dof) const override;
+
+  bool UpdateAngle(const IK_QJacobian &jacobian, Vector3d &delta, bool *clamp) override;
+  void Lock(int dof, IK_QJacobian &jacobian, Vector3d &delta) override;
+  void UpdateAngleApply() override;
+
+  bool ComputeClampRotation(Vector3d &clamp);
+
+  void SetLimit(int axis, double lmin, double lmax) override;
+  void SetWeight(int axis, double weight) override;
+
+ private:
+  Matrix3d m_new_basis;
+  bool m_limit_x, m_limit_y, m_limit_z;
+  double m_min[2], m_max[2];
+  double m_min_y, m_max_y, m_max_x, m_max_z, m_offset_x, m_offset_z;
+  double m_locked_ax, m_locked_ay, m_locked_az;
+};
+
+class IK_QNullSegment : public IK_QSegment {
+ public:
+  IK_QNullSegment();
+
+  bool UpdateAngle(const IK_QJacobian & /*jacobian*/,
+                   Vector3d & /*delta*/,
+                   bool * /*clamp*/) override
+  {
+    return false;
+  }
+  void UpdateAngleApply() override {}
+
+  Vector3d Axis(int /*dof*/) const override
+  {
+    return Vector3d(0, 0, 0);
+  }
+  void SetBasis(const Matrix3d & /*basis*/) override
+  {
+    m_basis.setIdentity();
+  }
+};
+
+class IK_QRevoluteSegment : public IK_QSegment {
+ public:
+  // axis: the axis of the DoF, in range 0..2
+  IK_QRevoluteSegment(int axis);
+
+  Vector3d Axis(int dof) const override;
+
+  bool UpdateAngle(const IK_QJacobian &jacobian, Vector3d &delta, bool *clamp) override;
+  void Lock(int dof, IK_QJacobian &jacobian, Vector3d &delta) override;
+  void UpdateAngleApply() override;
+
+  void SetLimit(int axis, double lmin, double lmax) override;
+  void SetWeight(int axis, double weight) override;
+  void SetBasis(const Matrix3d &basis) override;
+
+ private:
+  int m_axis;
+  double m_angle, m_new_angle;
+  bool m_limit;
+  double m_min, m_max;
+};
+
+class IK_QSwingSegment : public IK_QSegment {
+ public:
+  // XZ DOF, uses one direct rotation
+  IK_QSwingSegment();
+
+  Vector3d Axis(int dof) const override;
+
+  bool UpdateAngle(const IK_QJacobian &jacobian, Vector3d &delta, bool *clamp) override;
+  void Lock(int dof, IK_QJacobian &jacobian, Vector3d &delta) override;
+  void UpdateAngleApply() override;
+
+  void SetLimit(int axis, double lmin, double lmax) override;
+  void SetWeight(int axis, double weight) override;
+  void SetBasis(const Matrix3d &basis) override;
+
+ private:
+  Matrix3d m_new_basis;
+  bool m_limit_x, m_limit_z;
+  double m_min[2], m_max[2];
+  double m_max_x, m_max_z, m_offset_x, m_offset_z;
+};
+
+class IK_QElbowSegment : public IK_QSegment {
+ public:
+  // XY or ZY DOF, uses two sequential rotations: first rotate around
+  // X or Z, then rotate around Y (twist)
+  IK_QElbowSegment(int axis);
+
+  Vector3d Axis(int dof) const override;
+
+  bool UpdateAngle(const IK_QJacobian &jacobian, Vector3d &delta, bool *clamp) override;
+  void Lock(int dof, IK_QJacobian &jacobian, Vector3d &delta) override;
+  void UpdateAngleApply() override;
+
+  void SetLimit(int axis, double lmin, double lmax) override;
+  void SetWeight(int axis, double weight) override;
+  void SetBasis(const Matrix3d &basis) override;
+
+ private:
+  int m_axis;
+
+  double m_twist, m_angle, m_new_twist, m_new_angle;
+  double m_cos_twist, m_sin_twist;
+
+  bool m_limit, m_limit_twist;
+  double m_min, m_max, m_min_twist, m_max_twist;
+};
+
+class IK_QTranslateSegment : public IK_QSegment {
+ public:
+  // 1DOF, 2DOF or 3DOF translational segments
+  IK_QTranslateSegment(int axis1);
+  IK_QTranslateSegment(int axis1, int axis2);
+  IK_QTranslateSegment();
+
+  Vector3d Axis(int dof) const override;
+
+  bool UpdateAngle(const IK_QJacobian &jacobian, Vector3d &delta, bool *clamp) override;
+  void Lock(int /*dof*/, IK_QJacobian & /*jacobian*/, Vector3d & /*delta*/) override;
+  void UpdateAngleApply() override;
+
+  void SetWeight(int axis, double weight) override;
+  void SetLimit(int axis, double lmin, double lmax) override;
+
+  void Scale(double scale) override;
+
+ private:
+  int m_axis[3];
+  bool m_axis_enabled[3], m_limit[3];
+  Vector3d m_new_translation;
+  double m_min[3], m_max[3];
+};

blender-5.2.0/intern/iksolver/intern/IK_QTask.cpp

@@ -0,0 +1,211 @@
+/* SPDX-FileCopyrightText: 2001-2002 NaN Holding BV. All rights reserved.
+ *
+ * SPDX-License-Identifier: GPL-2.0-or-later */
+
+/** \file
+ * \ingroup intern_iksolver
+ */
+
+#include "IK_QTask.h"
+
+// IK_QTask
+
+IK_QTask::IK_QTask(int size, bool primary, bool active, const IK_QSegment *segment)
+    : m_size(size), m_primary(primary), m_active(active), m_segment(segment), m_weight(1.0)
+{
+}
+
+// IK_QPositionTask
+
+IK_QPositionTask::IK_QPositionTask(bool primary, const IK_QSegment *segment, const Vector3d &goal)
+    : IK_QTask(3, primary, true, segment), m_goal(goal)
+{
+  // computing clamping length
+  int num;
+  const IK_QSegment *seg;
+
+  m_clamp_length = 0.0;
+  num = 0;
+
+  for (seg = m_segment; seg; seg = seg->Parent()) {
+    m_clamp_length += seg->MaxExtension();
+    num++;
+  }
+
+  m_clamp_length /= 2 * num;
+}
+
+void IK_QPositionTask::ComputeJacobian(IK_QJacobian &jacobian)
+{
+  // compute beta
+  const Vector3d &pos = m_segment->GlobalEnd();
+
+  Vector3d d_pos = m_goal - pos;
+  double length = d_pos.norm();
+
+  if (length > m_clamp_length) {
+    d_pos = (m_clamp_length / length) * d_pos;
+  }
+
+  jacobian.SetBetas(m_id, m_size, m_weight * d_pos);
+
+  // compute derivatives
+  int i;
+  const IK_QSegment *seg;
+
+  for (seg = m_segment; seg; seg = seg->Parent()) {
+    Vector3d p = seg->GlobalStart() - pos;
+
+    for (i = 0; i < seg->NumberOfDoF(); i++) {
+      Vector3d axis = seg->Axis(i) * m_weight;
+
+      if (seg->Translational()) {
+        jacobian.SetDerivatives(m_id, seg->DoFId() + i, axis, 1e2);
+      }
+      else {
+        Vector3d pa = p.cross(axis);
+        jacobian.SetDerivatives(m_id, seg->DoFId() + i, pa, 1e0);
+      }
+    }
+  }
+}
+
+double IK_QPositionTask::Distance() const
+{
+  const Vector3d &pos = m_segment->GlobalEnd();
+  Vector3d d_pos = m_goal - pos;
+  return d_pos.norm();
+}
+
+// IK_QOrientationTask
+
+IK_QOrientationTask::IK_QOrientationTask(bool primary,
+                                         const IK_QSegment *segment,
+                                         const Matrix3d &goal)
+    : IK_QTask(3, primary, true, segment), m_goal(goal), m_distance(0.0)
+{
+}
+
+void IK_QOrientationTask::ComputeJacobian(IK_QJacobian &jacobian)
+{
+  // compute betas
+  const Matrix3d &rot = m_segment->GlobalTransform().linear();
+
+  Matrix3d d_rotm = (m_goal * rot.transpose()).transpose();
+
+  Vector3d d_rot;
+  d_rot = -0.5 * Vector3d(d_rotm(2, 1) - d_rotm(1, 2),
+                          d_rotm(0, 2) - d_rotm(2, 0),
+                          d_rotm(1, 0) - d_rotm(0, 1));
+
+  m_distance = d_rot.norm();
+
+  jacobian.SetBetas(m_id, m_size, m_weight * d_rot);
+
+  // compute derivatives
+  int i;
+  const IK_QSegment *seg;
+
+  for (seg = m_segment; seg; seg = seg->Parent()) {
+    for (i = 0; i < seg->NumberOfDoF(); i++) {
+
+      if (seg->Translational()) {
+        jacobian.SetDerivatives(m_id, seg->DoFId() + i, Vector3d(0, 0, 0), 1e2);
+      }
+      else {
+        Vector3d axis = seg->Axis(i) * m_weight;
+        jacobian.SetDerivatives(m_id, seg->DoFId() + i, axis, 1e0);
+      }
+    }
+  }
+}
+
+// IK_QCenterOfMassTask
+// NOTE: implementation not finished!
+
+IK_QCenterOfMassTask::IK_QCenterOfMassTask(bool primary,
+                                           const IK_QSegment *segment,
+                                           const Vector3d &goal_center)
+    : IK_QTask(3, primary, true, segment), m_goal_center(goal_center)
+{
+  m_total_mass_inv = ComputeTotalMass(m_segment);
+  if (!FuzzyZero(m_total_mass_inv)) {
+    m_total_mass_inv = 1.0 / m_total_mass_inv;
+  }
+}
+
+double IK_QCenterOfMassTask::ComputeTotalMass(const IK_QSegment *segment)
+{
+  double mass = /*seg->Mass()*/ 1.0;
+
+  const IK_QSegment *seg;
+  for (seg = segment->Child(); seg; seg = seg->Sibling()) {
+    mass += ComputeTotalMass(seg);
+  }
+
+  return mass;
+}
+
+Vector3d IK_QCenterOfMassTask::ComputeCenter(const IK_QSegment *segment)
+{
+  Vector3d center = /*seg->Mass()**/ segment->GlobalStart();
+
+  const IK_QSegment *seg;
+  for (seg = segment->Child(); seg; seg = seg->Sibling()) {
+    center += ComputeCenter(seg);
+  }
+
+  return center;
+}
+
+void IK_QCenterOfMassTask::JacobianSegment(IK_QJacobian &jacobian,
+                                           Vector3d &center,
+                                           const IK_QSegment *segment)
+{
+  int i;
+  Vector3d p = center - segment->GlobalStart();
+
+  for (i = 0; i < segment->NumberOfDoF(); i++) {
+    Vector3d axis = segment->Axis(i) * m_weight;
+    axis *= /*segment->Mass()**/ m_total_mass_inv;
+
+    if (segment->Translational()) {
+      jacobian.SetDerivatives(m_id, segment->DoFId() + i, axis, 1e2);
+    }
+    else {
+      Vector3d pa = axis.cross(p);
+      jacobian.SetDerivatives(m_id, segment->DoFId() + i, pa, 1e0);
+    }
+  }
+
+  const IK_QSegment *seg;
+  for (seg = segment->Child(); seg; seg = seg->Sibling()) {
+    JacobianSegment(jacobian, center, seg);
+  }
+}
+
+void IK_QCenterOfMassTask::ComputeJacobian(IK_QJacobian &jacobian)
+{
+  Vector3d center = ComputeCenter(m_segment) * m_total_mass_inv;
+
+  // compute beta
+  Vector3d d_pos = m_goal_center - center;
+
+  m_distance = d_pos.norm();
+
+#if 0
+  if (m_distance > m_clamp_length) {
+    d_pos = (m_clamp_length / m_distance) * d_pos;
+  }
+#endif
+
+  jacobian.SetBetas(m_id, m_size, m_weight * d_pos);
+
+  // compute derivatives
+  JacobianSegment(jacobian, center, m_segment);
+}
+
+double IK_QCenterOfMassTask::Distance() const
+{
+  return m_distance;
+}

blender-5.2.0/intern/iksolver/intern/IK_QTask.h

@@ -0,0 +1,135 @@
+/* SPDX-FileCopyrightText: 2001-2002 NaN Holding BV. All rights reserved.
+ *
+ * SPDX-License-Identifier: GPL-2.0-or-later */
+
+/** \file
+ * \ingroup intern_iksolver
+ */
+
+#pragma once
+
+#include "IK_Math.h"
+#include "IK_QJacobian.h"
+#include "IK_QSegment.h"
+
+class IK_QTask {
+ public:
+  IK_QTask(int size, bool primary, bool active, const IK_QSegment *segment);
+  virtual ~IK_QTask() = default;
+
+  int Id() const
+  {
+    return m_size;
+  }
+
+  void SetId(int id)
+  {
+    m_id = id;
+  }
+
+  int Size() const
+  {
+    return m_size;
+  }
+
+  bool Primary() const
+  {
+    return m_primary;
+  }
+
+  bool Active() const
+  {
+    return m_active;
+  }
+
+  double Weight() const
+  {
+    return m_weight * m_weight;
+  }
+
+  void SetWeight(double weight)
+  {
+    m_weight = sqrt(weight);
+  }
+
+  virtual void ComputeJacobian(IK_QJacobian &jacobian) = 0;
+
+  virtual double Distance() const = 0;
+
+  virtual bool PositionTask() const
+  {
+    return false;
+  }
+
+  virtual void Scale(double /*scale*/) {}
+
+ protected:
+  int m_id;
+  int m_size;
+  bool m_primary;
+  bool m_active;
+  const IK_QSegment *m_segment;
+  double m_weight;
+};
+
+class IK_QPositionTask : public IK_QTask {
+ public:
+  IK_QPositionTask(bool primary, const IK_QSegment *segment, const Vector3d &goal);
+
+  void ComputeJacobian(IK_QJacobian &jacobian) override;
+
+  double Distance() const override;
+
+  bool PositionTask() const override
+  {
+    return true;
+  }
+  void Scale(double scale) override
+  {
+    m_goal *= scale;
+    m_clamp_length *= scale;
+  }
+
+ private:
+  Vector3d m_goal;
+  double m_clamp_length;
+};
+
+class IK_QOrientationTask : public IK_QTask {
+ public:
+  IK_QOrientationTask(bool primary, const IK_QSegment *segment, const Matrix3d &goal);
+
+  double Distance() const override
+  {
+    return m_distance;
+  }
+  void ComputeJacobian(IK_QJacobian &jacobian) override;
+
+ private:
+  Matrix3d m_goal;
+  double m_distance;
+};
+
+class IK_QCenterOfMassTask : public IK_QTask {
+ public:
+  IK_QCenterOfMassTask(bool primary, const IK_QSegment *segment, const Vector3d &center);
+
+  void ComputeJacobian(IK_QJacobian &jacobian) override;
+
+  double Distance() const override;
+
+  void Scale(double scale) override
+  {
+    m_goal_center *= scale;
+    m_distance *= scale;
+  }
+
+ private:
+  double ComputeTotalMass(const IK_QSegment *segment);
+  Vector3d ComputeCenter(const IK_QSegment *segment);
+  void JacobianSegment(IK_QJacobian &jacobian, Vector3d &center, const IK_QSegment *segment);
+
+  Vector3d m_goal_center;
+  double m_total_mass_inv;
+  double m_distance;
+};

blender-5.2.0/intern/iksolver/intern/IK_Solver.cpp

@@ -0,0 +1,446 @@
+/* SPDX-FileCopyrightText: 2001-2002 NaN Holding BV. All rights reserved.
+ *
+ * SPDX-License-Identifier: GPL-2.0-or-later */
+
+/** \file
+ * \ingroup intern_iksolver
+ */
+
+#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);
+}