armature_update.cc

Go to the documentation of this file.

/* SPDX-FileCopyrightText: 2015 Blender Authors
 *
 * SPDX-License-Identifier: GPL-2.0-or-later */

 
#include <algorithm>
 
#include "MEM_guardedalloc.h"
 
#include "BLI_listbase.h"
#include "BLI_math_matrix.h"
#include "BLI_math_rotation.h"
#include "BLI_math_vector.h"
#include "BLI_utildefines.h"
 
#include "DNA_armature_types.h"
#include "DNA_constraint_types.h"
#include "DNA_curve_types.h"
#include "DNA_object_types.h"
 
#include "BKE_action.hh"
#include "BKE_anim_path.h"
#include "BKE_armature.hh"
#include "BKE_curve.hh"
#include "BKE_object_types.hh"
#include "BKE_scene.hh"
 
#include "BIK_api.h"
 
#include "DEG_depsgraph.hh"
 
/* ********************** SPLINE IK SOLVER ******************* */
 
/* Temporary evaluation tree data used for Spline IK */
struct tSplineIK_Tree {
  tSplineIK_Tree *next, *prev;
 
  int type; /* type of IK that this serves (CONSTRAINT_TYPE_KINEMATIC or ..._SPLINEIK) */
 
  short chainlen;  /* number of bones in the chain */
  float totlength; /* total length of bones in the chain */
 
  const float *points;  /* parametric positions for the joints along the curve */
  bPoseChannel **chain; /* chain of bones to affect using Spline IK (ordered from the tip) */
 
  bPoseChannel *root; /* bone that is the root node of the chain */
 
  bConstraint *con;             /* constraint for this chain */
  bSplineIKConstraint *ik_data; /* constraint settings for this chain */
};
 
/* ----------- */
 
/* Tag the bones in the chain formed by the given bone for IK. */
static void splineik_init_tree_from_pchan(Scene * /*scene*/,
                                          Object * /*ob*/,
                                          bPoseChannel *pchan_tip)
{
  bPoseChannel *pchan, *pchan_root = nullptr;
  bPoseChannel *pchan_chain[255];
  bConstraint *con = nullptr;
  bSplineIKConstraint *ik_data = nullptr;
  float bone_lengths[255];
  float totlength = 0.0f;
  int segcount = 0;
 
  /* Find the SplineIK constraint. */
  for (con = static_cast<bConstraint *>(pchan_tip->constraints.first); con; con = con->next) {
    if (con->type == CONSTRAINT_TYPE_SPLINEIK) {
      ik_data = static_cast<bSplineIKConstraint *>(con->data);
 
      /* Target can only be a curve. */
      if ((ik_data->tar == nullptr) || (ik_data->tar->type != OB_CURVES_LEGACY)) {
        continue;
      }
      /* Skip if disabled. */
      if ((con->enforce == 0.0f) || (con->flag & (CONSTRAINT_DISABLE | CONSTRAINT_OFF))) {
        continue;
      }
 
      /* Otherwise, constraint is ok... */
      break;
    }
  }
  if (con == nullptr) {
    return;
  }
 
  /* Find the root bone and the chain of bones from the root to the tip.
   * NOTE: this assumes that the bones are connected, but that may not be true... */
  for (pchan = pchan_tip; pchan && (segcount < ik_data->chainlen);
       pchan = pchan->parent, segcount++)
  {
    /* Store this segment in the chain. */
    pchan_chain[segcount] = pchan;
 
    /* If performing rebinding, calculate the length of the bone. */
    bone_lengths[segcount] = pchan->bone->length;
    totlength += bone_lengths[segcount];
  }
 
  if (segcount == 0) {
    return;
  }
 
  pchan_root = pchan_chain[segcount - 1];
 
  /* Perform binding step if required. */
  if ((ik_data->flag & CONSTRAINT_SPLINEIK_BOUND) == 0) {
    float segmentLen = (1.0f / float(segcount));
 
    /* Setup new empty array for the points list. */
    if (ik_data->points) {
      MEM_freeN(ik_data->points);
    }
    ik_data->numpoints = ik_data->chainlen + 1;
    ik_data->points = MEM_malloc_arrayN<float>(size_t(ik_data->numpoints), "Spline IK Binding");
 
    /* Bind 'tip' of chain (i.e. first joint = tip of bone with the Spline IK Constraint). */
    ik_data->points[0] = 1.0f;
 
    /* Perform binding of the joints to parametric positions along the curve based
     * proportion of the total length that each bone occupies.
     */
    for (int i = 0; i < segcount; i++) {
      /* 'head' joints, traveling towards the root of the chain.
       * - 2 methods; the one chosen depends on whether we've got usable lengths.
       */
      if ((ik_data->flag & CONSTRAINT_SPLINEIK_EVENSPLITS) || (totlength == 0.0f)) {
        /* 1) Equi-spaced joints. */
        ik_data->points[i + 1] = ik_data->points[i] - segmentLen;
      }
      else {
        /* 2) To find this point on the curve, we take a step from the previous joint
         *    a distance given by the proportion that this bone takes.
         */
        ik_data->points[i + 1] = ik_data->points[i] - (bone_lengths[i] / totlength);
      }
    }
 
    /* Spline has now been bound. */
    ik_data->flag |= CONSTRAINT_SPLINEIK_BOUND;
  }
 
  /* Disallow negative values (happens with float precision). */
  CLAMP_MIN(ik_data->points[segcount], 0.0f);
 
  /* Make a new Spline-IK chain, and store it in the IK chains. */
  /* TODO: we should check if there is already an IK chain on this,
   * since that would take precedence... */
  {
    /* Make a new tree. */
    tSplineIK_Tree *tree = MEM_callocN<tSplineIK_Tree>("SplineIK Tree");
    tree->type = CONSTRAINT_TYPE_SPLINEIK;
 
    tree->chainlen = segcount;
    tree->totlength = totlength;
 
    /* Copy over the array of links to bones in the chain (from tip to root). */
    tree->chain = MEM_malloc_arrayN<bPoseChannel *>(size_t(segcount), "SplineIK Chain");
    memcpy(tree->chain, pchan_chain, sizeof(bPoseChannel *) * segcount);
 
    /* Store reference to joint position array. */
    tree->points = ik_data->points;
 
    /* Store references to different parts of the chain. */
    tree->root = pchan_root;
    tree->con = con;
    tree->ik_data = ik_data;
 
    /* AND! Link the tree to the root. */
    BLI_addtail(&pchan_root->siktree, tree);
  }
 
  /* Mark root channel having an IK tree. */
  pchan_root->flag |= POSE_IKSPLINE;
}
 
/* Tag which bones are members of Spline IK chains. */
static void splineik_init_tree(Scene *scene, Object *ob, float /*ctime*/)
{
  /* Find the tips of Spline IK chains,
   * which are simply the bones which have been tagged as such. */
  LISTBASE_FOREACH (bPoseChannel *, pchan, &ob->pose->chanbase) {
    if (pchan->constflag & PCHAN_HAS_SPLINEIK) {
      splineik_init_tree_from_pchan(scene, ob, pchan);
    }
  }
}
 
/* ----------- */
 
struct tSplineIk_EvalState {
  float curve_position;      /* Current position along the curve. */
  float curve_scale;         /* Global scale to apply to curve positions. */
  float locrot_offset[4][4]; /* Bone rotation and location offset inherited from parent. */
  float prev_tail_loc[3];    /* Tail location of the previous bone. */
  float prev_tail_radius;    /* Tail curve radius of the previous bone. */
  int prev_tail_seg_idx;     /* Curve segment the previous tail bone belongs to. */
};
 
/* Prepare data to evaluate spline IK. */
static bool splineik_evaluate_init(tSplineIK_Tree *tree, tSplineIk_EvalState *state)
{
  bSplineIKConstraint *ik_data = tree->ik_data;
 
  /* Make sure that the constraint targets are ok, to avoid crashes
   * in case of a depsgraph bug or dependency cycle.
   */
  if (ik_data->tar == nullptr) {
    return false;
  }
 
  CurveCache *cache = ik_data->tar->runtime->curve_cache;
 
  if (ELEM(nullptr, cache, cache->anim_path_accum_length)) {
    return false;
  }
 
  /* Initialize the evaluation state. */
  state->curve_position = 0.0f;
  state->curve_scale = 1.0f;
  unit_m4(state->locrot_offset);
  zero_v3(state->prev_tail_loc);
  state->prev_tail_radius = 1.0f;
  state->prev_tail_seg_idx = 0;
 
  /* Apply corrections for sensitivity to scaling. */
  if ((ik_data->yScaleMode != CONSTRAINT_SPLINEIK_YS_FIT_CURVE) && (tree->totlength != 0.0f)) {
    /* Get the current length of the curve. */
    /* NOTE: This is assumed to be correct even after the curve was resized. */
    const float spline_len = BKE_anim_path_get_length(cache);
 
    /* Calculate the scale factor to multiply all the path values by so that the
     * bone chain retains its current length, such that:
     *     maxScale * splineLen = totLength
     */
    state->curve_scale = tree->totlength / spline_len;
  }
 
  return true;
}
 
static void apply_curve_transform(
    bSplineIKConstraint *ik_data, Object *ob, float radius, float r_vec[3], float *r_radius)
{
  /* Apply the curve's object-mode transforms to the position
   * unless the option to allow curve to be positioned elsewhere is activated (i.e. no root).
   */
  if ((ik_data->flag & CONSTRAINT_SPLINEIK_NO_ROOT) == 0) {
    mul_m4_v3(ik_data->tar->object_to_world().ptr(), r_vec);
  }
 
  /* Convert the position to pose-space. */
  mul_m4_v3(ob->world_to_object().ptr(), r_vec);
 
  /* Set the new radius (it should be the average value). */
  *r_radius = (radius + *r_radius) / 2;
}
 
static float dist_to_sphere_shell(const float sphere_origin[3],
                                  const float sphere_radius,
                                  const float point[3])
{
  float vec[3];
  sub_v3_v3v3(vec, sphere_origin, point);
  return sphere_radius - len_v3(vec);
}
 
/* This function positions the tail of the bone so that it preserves the length of it.
 * The length of the bone can be seen as a sphere radius.
 */
static int position_tail_on_spline(bSplineIKConstraint *ik_data,
                                   const float head_pos[3],
                                   const float sphere_radius,
                                   int prev_seg_idx,
                                   float r_tail_pos[3],
                                   float *r_new_curve_pos,
                                   float *r_radius)
{
  /* This is using the tessellated curve data.
   * So we are working with piece-wise linear curve segments.
   * The same method is used in #BKE_where_on_path to get curve location data. */
  const CurveCache *cache = ik_data->tar->runtime->curve_cache;
  const float *seg_accum_len = cache->anim_path_accum_length;
 
  int max_seg_idx = BKE_anim_path_get_array_size(cache) - 1;
 
  /* Make an initial guess of where our intersection point will be.
   * If the curve was a straight line, then the fraction passed in r_new_curve_pos
   * would be the correct location.
   * So make it our first initial guess.
   */
  const float spline_len = BKE_anim_path_get_length(cache);
  const float guessed_len = *r_new_curve_pos * spline_len;
 
  BLI_assert(prev_seg_idx >= 0);
  int cur_seg_idx = prev_seg_idx;
  while (cur_seg_idx < max_seg_idx && guessed_len > seg_accum_len[cur_seg_idx]) {
    cur_seg_idx++;
  }
 
  /* Convert the segment to bev points.
   * For example, the segment with index 0 will have bev points 0 and 1.
   */
  int bp_idx = cur_seg_idx + 1;
 
  const BevList *bl = static_cast<const BevList *>(cache->bev.first);
  bool is_cyclic = bl->poly >= 0;
  BevPoint *bp = bl->bevpoints;
  BevPoint *prev_bp;
  bp = bp + bp_idx;
  prev_bp = bp - 1;
 
  /* Go to the next tessellated curve point until we cross to outside of the sphere. */
  while (len_v3v3(head_pos, bp->vec) < sphere_radius) {
    if (bp_idx > max_seg_idx) {
      /* We are outside the defined curve. We will now extrapolate the intersection point. */
      break;
    }
    prev_bp = bp;
    if (is_cyclic && bp_idx == max_seg_idx) {
      /* Wrap around to the start point.
       * Don't set the bp_idx to zero here as we use it to get the segment index later.
       */
      bp = bl->bevpoints;
    }
    else {
      bp++;
    }
    bp_idx++;
  }
 
  /* Calculate the intersection point using the secant root finding method */
  float x0 = 0.0f, x1 = 1.0f;
  float x0_point[3], x1_point[3], start_p[3];
  float epsilon = max_fff(1.0f, len_v3(head_pos), len_v3(bp->vec)) * FLT_EPSILON;
 
  if (prev_seg_idx == bp_idx - 1) {
    /* The intersection lies inside the same segment as the last point.
     * Set the last point to be the start search point so we minimize the risks of going backwards
     * on the curve.
     */
    copy_v3_v3(start_p, head_pos);
  }
  else {
    copy_v3_v3(start_p, prev_bp->vec);
  }
 
  for (int i = 0; i < 10; i++) {
    interp_v3_v3v3(x0_point, start_p, bp->vec, x0);
    interp_v3_v3v3(x1_point, start_p, bp->vec, x1);
 
    float f_x0 = dist_to_sphere_shell(head_pos, sphere_radius, x0_point);
    float f_x1 = dist_to_sphere_shell(head_pos, sphere_radius, x1_point);
 
    if (fabsf(f_x1) <= epsilon || f_x0 == f_x1) {
      break;
    }
 
    const float x2 = x1 - f_x1 * (x1 - x0) / (f_x1 - f_x0);
    x0 = x1;
    x1 = x2;
  }
  /* Found the bone tail position! */
  copy_v3_v3(r_tail_pos, x1_point);
 
  /* Because our intersection point lies inside the current segment,
   * Convert our bevpoint index back to the previous segment index (-2 instead of -1).
   * This is because our actual location is prev_seg_len + isect_seg_len.
   */
  prev_seg_idx = bp_idx - 2;
  float prev_seg_len = 0;
 
  if (prev_seg_idx < 0) {
    prev_seg_idx = 0;
    prev_seg_len = 0;
  }
  else {
    prev_seg_len = seg_accum_len[prev_seg_idx];
  }
 
  /* Convert the point back into the 0-1 interpolation range. */
  const float isect_seg_len = len_v3v3(prev_bp->vec, r_tail_pos);
  const float frac = isect_seg_len / len_v3v3(prev_bp->vec, bp->vec);
  *r_new_curve_pos = (prev_seg_len + isect_seg_len) / spline_len;
 
  if (*r_new_curve_pos > 1.0f) {
    *r_radius = bp->radius;
  }
  else {
    *r_radius = (1.0f - frac) * prev_bp->radius + frac * bp->radius;
  }
 
  /* Return the current segment. */
  return bp_idx - 1;
}
 
/* Evaluate spline IK for a given bone. */
static void splineik_evaluate_bone(
    tSplineIK_Tree *tree, Object *ob, bPoseChannel *pchan, int index, tSplineIk_EvalState *state)
{
  bSplineIKConstraint *ik_data = tree->ik_data;
 
  if (pchan->bone->length < FLT_EPSILON) {
    /* Only move the bone position with zero length bones. */
    float bone_pos[4], rad;
    BKE_where_on_path(
        ik_data->tar, state->curve_position, bone_pos, nullptr, nullptr, &rad, nullptr);
 
    apply_curve_transform(ik_data, ob, rad, bone_pos, &rad);
 
    copy_v3_v3(pchan->pose_mat[3], bone_pos);
    copy_v3_v3(pchan->pose_head, bone_pos);
    copy_v3_v3(pchan->pose_tail, bone_pos);
    pchan->flag |= POSE_DONE;
    return;
  }
 
  float orig_head[3], orig_tail[3], pose_head[3], pose_tail[3];
  float base_pose_mat[3][3], pose_mat[3][3];
  float spline_vec[3], scale_fac, radius = 1.0f;
  float tail_blend_fac = 0.0f;
 
  mul_v3_m4v3(pose_head, state->locrot_offset, pchan->pose_head);
  mul_v3_m4v3(pose_tail, state->locrot_offset, pchan->pose_tail);
 
  copy_v3_v3(orig_head, pose_head);
 
  /* First, adjust the point positions on the curve. */
  float curveLen = tree->points[index] - tree->points[index + 1];
  float bone_len = len_v3v3(pose_head, pose_tail);
  float point_start = state->curve_position;
  float pose_scale = bone_len / pchan->bone->length;
  float base_scale = 1.0f;
 
  if (ik_data->yScaleMode == CONSTRAINT_SPLINEIK_YS_ORIGINAL) {
    /* Carry over the bone Y scale to the curve range. */
    base_scale = pose_scale;
  }
 
  float point_end = point_start + curveLen * base_scale * state->curve_scale;
 
  state->curve_position = point_end;
 
  /* Step 1: determine the positions for the endpoints of the bone. */
  if (point_start < 1.0f) {
    float vec[4], rad;
    radius = 0.0f;
 
    /* Calculate head position. */
    if (point_start == 0.0f) {
      /* Start of the path. We have no previous tail position to copy. */
      BKE_where_on_path(ik_data->tar, point_start, vec, nullptr, nullptr, &rad, nullptr);
    }
    else {
      copy_v3_v3(vec, state->prev_tail_loc);
      rad = state->prev_tail_radius;
    }
 
    radius = rad;
    copy_v3_v3(pose_head, vec);
    apply_curve_transform(ik_data, ob, rad, pose_head, &radius);
 
    /* Calculate tail position. */
    if (ik_data->yScaleMode != CONSTRAINT_SPLINEIK_YS_FIT_CURVE) {
      float sphere_radius;
 
      if (ik_data->yScaleMode == CONSTRAINT_SPLINEIK_YS_ORIGINAL) {
        sphere_radius = bone_len;
      }
      else {
        /* Don't take bone scale into account. */
        sphere_radius = pchan->bone->length;
      }
 
      /* Calculate the tail position with sphere curve intersection. */
      state->prev_tail_seg_idx = position_tail_on_spline(
          ik_data, vec, sphere_radius, state->prev_tail_seg_idx, pose_tail, &point_end, &rad);
 
      state->prev_tail_radius = rad;
      copy_v3_v3(state->prev_tail_loc, pose_tail);
 
      apply_curve_transform(ik_data, ob, rad, pose_tail, &radius);
      state->curve_position = point_end;
    }
    else {
      /* Scale to fit curve end position. */
      if (BKE_where_on_path(ik_data->tar, point_end, vec, nullptr, nullptr, &rad, nullptr)) {
        state->prev_tail_radius = rad;
        copy_v3_v3(state->prev_tail_loc, vec);
        copy_v3_v3(pose_tail, vec);
        apply_curve_transform(ik_data, ob, rad, pose_tail, &radius);
      }
    }
 
    /* Determine if the bone should still be affected by SplineIK.
     * This makes it so that the bone slowly becomes poseable again the further it rolls off the
     * curve. When the whole bone has rolled off the curve, the IK constraint will not influence it
     * anymore.
     */
    if (point_end >= 1.0f) {
      /* Blending factor depends on the amount of the bone still left on the chain. */
      tail_blend_fac = (1.0f - point_start) / (point_end - point_start);
    }
    else {
      tail_blend_fac = 1.0f;
    }
  }
 
  /* Step 2: determine the implied transform from these endpoints.
   * - splineVec: the vector direction that the spline applies on the bone.
   * - scaleFac: the factor that the bone length is scaled by to get the desired amount.
   */
  sub_v3_v3v3(spline_vec, pose_tail, pose_head);
  scale_fac = len_v3(spline_vec) / pchan->bone->length;
 
  /* Step 3: compute the shortest rotation needed
   * to map from the bone rotation to the current axis.
   * - this uses the same method as is used for the Damped Track Constraint
   *   (see the code there for details).
   */
  {
    float dmat[3][3], rmat[3][3];
    float raxis[3], rangle;
 
    /* Compute the raw rotation matrix from the bone's current matrix by extracting only the
     * orientation-relevant axes, and normalizing them.
     */
    mul_m3_m4m4(base_pose_mat, state->locrot_offset, pchan->pose_mat);
    normalize_m3_m3(rmat, base_pose_mat);
 
    /* Also, normalize the orientation imposed by the bone,
     * now that we've extracted the scale factor. */
    normalize_v3(spline_vec);
 
    /* Calculate smallest axis-angle rotation necessary for getting from the
     * current orientation of the bone, to the spline-imposed direction.
     */
    cross_v3_v3v3(raxis, rmat[1], spline_vec);
 
    /* Check if the old and new bone direction is parallel to each other.
     * If they are, then 'raxis' should be near zero and we will have to get the rotation axis in
     * some other way.
     */
    float norm = normalize_v3(raxis);
 
    if (norm < FLT_EPSILON) {
      /* Can't use cross product! */
      int order[3] = {0, 1, 2};
      float tmp_axis[3];
      zero_v3(tmp_axis);
 
      axis_sort_v3(spline_vec, order);
 
      /* Use the second largest axis as the basis for the rotation axis. */
      tmp_axis[order[1]] = 1.0f;
      cross_v3_v3v3(raxis, tmp_axis, spline_vec);
    }
 
    rangle = dot_v3v3(rmat[1], spline_vec);
    CLAMP(rangle, -1.0f, 1.0f);
    rangle = acosf(rangle);
 
    /* Multiply the magnitude of the angle by the influence of the constraint to
     * control the influence of the SplineIK effect.
     */
    rangle *= tree->con->enforce * tail_blend_fac;
 
    /* Construct rotation matrix from the axis-angle rotation found above.
     * - This call takes care to make sure that the axis provided is a unit vector first.
     */
    axis_angle_to_mat3(dmat, raxis, rangle);
 
    /* Combine these rotations so that the y-axis of the bone is now aligned as the
     * spline dictates, while still maintaining roll control from the existing bone animation. */
    mul_m3_m3m3(pose_mat, dmat, rmat);
 
    /* Attempt to reduce shearing, though I doubt this will really help too much now. */
    normalize_m3(pose_mat);
 
    mul_m3_m3m3(base_pose_mat, dmat, base_pose_mat);
 
    /* Apply rotation to the accumulated parent transform. */
    mul_m4_m3m4(state->locrot_offset, dmat, state->locrot_offset);
  }
 
  /* Step 4: Set the scaling factors for the axes. */
 
  /* Always multiply the y-axis by the scaling factor to get the correct length. */
  mul_v3_fl(pose_mat[1], scale_fac);
 
  /* After that, apply x/z scaling modes. */
  if (ik_data->xzScaleMode != CONSTRAINT_SPLINEIK_XZS_NONE) {
    /* First, apply the original scale if enabled. */
    if (ik_data->xzScaleMode == CONSTRAINT_SPLINEIK_XZS_ORIGINAL ||
        (ik_data->flag & CONSTRAINT_SPLINEIK_USE_ORIGINAL_SCALE) != 0)
    {
      float scale;
 
      /* X-axis scale. */
      scale = len_v3(pchan->pose_mat[0]);
      mul_v3_fl(pose_mat[0], scale);
      /* Z-axis scale. */
      scale = len_v3(pchan->pose_mat[2]);
      mul_v3_fl(pose_mat[2], scale);
 
      /* Adjust the scale factor used for volume preservation
       * to consider the pre-IK scaling as the initial volume. */
      scale_fac /= pose_scale;
    }
 
    /* Apply volume preservation. */
    switch (ik_data->xzScaleMode) {
      case CONSTRAINT_SPLINEIK_XZS_INVERSE: {
        /* Old 'volume preservation' method using the inverse scale. */
        float scale;
 
        /* Calculate volume preservation factor which is
         * basically the inverse of the y-scaling factor.
         */
        if (fabsf(scale_fac) != 0.0f) {
          scale = 1.0f / fabsf(scale_fac);
 
          /* We need to clamp this within sensible values. */
          /* NOTE: these should be fine for now, but should get sanitized in future. */
          CLAMP(scale, 0.0001f, 100000.0f);
        }
        else {
          scale = 1.0f;
        }
 
        /* Apply the scaling. */
        mul_v3_fl(pose_mat[0], scale);
        mul_v3_fl(pose_mat[2], scale);
        break;
      }
      case CONSTRAINT_SPLINEIK_XZS_VOLUMETRIC: {
        /* Improved volume preservation based on the Stretch To constraint. */
        float final_scale;
 
        /* As the basis for volume preservation, we use the inverse scale factor... */
        if (fabsf(scale_fac) != 0.0f) {
          /* NOTE: The method here is taken wholesale from the Stretch To constraint. */
          float bulge = powf(1.0f / fabsf(scale_fac), ik_data->bulge);
 
          if (bulge > 1.0f) {
            if (ik_data->flag & CONSTRAINT_SPLINEIK_USE_BULGE_MAX) {
              float bulge_max = max_ff(ik_data->bulge_max, 1.0f);
              float hard = min_ff(bulge, bulge_max);
 
              float range = bulge_max - 1.0f;
              float scale = (range > 0.0f) ? 1.0f / range : 0.0f;
              float soft = 1.0f + range * atanf((bulge - 1.0f) * scale) / float(M_PI_2);
 
              bulge = interpf(soft, hard, ik_data->bulge_smooth);
            }
          }
          if (bulge < 1.0f) {
            if (ik_data->flag & CONSTRAINT_SPLINEIK_USE_BULGE_MIN) {
              float bulge_min = std::clamp(ik_data->bulge_min, 0.0f, 1.0f);
              float hard = max_ff(bulge, bulge_min);
 
              float range = 1.0f - bulge_min;
              float scale = (range > 0.0f) ? 1.0f / range : 0.0f;
              float soft = 1.0f - range * atanf((1.0f - bulge) * scale) / float(M_PI_2);
 
              bulge = interpf(soft, hard, ik_data->bulge_smooth);
            }
          }
 
          /* Compute scale factor for XZ axes from this value. */
          final_scale = sqrtf(bulge);
        }
        else {
          /* No scaling, so scale factor is simple. */
          final_scale = 1.0f;
        }
 
        /* Apply the scaling (assuming normalized scale). */
        mul_v3_fl(pose_mat[0], final_scale);
        mul_v3_fl(pose_mat[2], final_scale);
        break;
      }
    }
  }
 
  /* Finally, multiply the x and z scaling by the radius of the curve too,
   * to allow automatic scales to get tweaked still.
   */
  if ((ik_data->flag & CONSTRAINT_SPLINEIK_NO_CURVERAD) == 0) {
    mul_v3_fl(pose_mat[0], radius);
    mul_v3_fl(pose_mat[2], radius);
  }
 
  /* Blend the scaling of the matrix according to the influence. */
  sub_m3_m3m3(pose_mat, pose_mat, base_pose_mat);
  madd_m3_m3m3fl(pose_mat, base_pose_mat, pose_mat, tree->con->enforce * tail_blend_fac);
 
  /* Step 5: Set the location of the bone in the matrix. */
  if (ik_data->flag & CONSTRAINT_SPLINEIK_NO_ROOT) {
    /* When the 'no-root' option is affected, the chain can retain
     * the shape but be moved elsewhere.
     */
    copy_v3_v3(pose_head, orig_head);
  }
  else if (tree->con->enforce < 1.0f) {
    /* When the influence is too low:
     * - Blend the positions for the 'root' bone.
     * - Stick to the parent for any other.
     */
    if (index < tree->chainlen - 1) {
      copy_v3_v3(pose_head, orig_head);
    }
    else {
      interp_v3_v3v3(pose_head, orig_head, pose_head, tree->con->enforce);
    }
  }
 
  /* Finally, store the new transform. */
  copy_m4_m3(pchan->pose_mat, pose_mat);
  copy_v3_v3(pchan->pose_mat[3], pose_head);
  copy_v3_v3(pchan->pose_head, pose_head);
 
  mul_v3_mat3_m4v3(orig_tail, state->locrot_offset, pchan->pose_tail);
 
  /* Recalculate tail, as it's now outdated after the head gets adjusted above! */
  BKE_pose_where_is_bone_tail(pchan);
 
  /* Update the offset in the accumulated parent transform. */
  sub_v3_v3v3(state->locrot_offset[3], pchan->pose_tail, orig_tail);
 
  /* Done! */
  pchan->flag |= POSE_DONE;
}
 
/* Evaluate the chain starting from the nominated bone */
static void splineik_execute_tree(
    Depsgraph *depsgraph, Scene *scene, Object *ob, bPoseChannel *pchan_root, float ctime)
{
  tSplineIK_Tree *tree;
 
  /* for each pose-tree, execute it if it is spline, otherwise just free it */
  while ((tree = static_cast<tSplineIK_Tree *>(pchan_root->siktree.first)) != nullptr) {
    /* Firstly, calculate the bone matrix the standard way,
     * since this is needed for roll control. */
    for (int i = tree->chainlen - 1; i >= 0; i--) {
      BKE_pose_where_is_bone(depsgraph, scene, ob, tree->chain[i], ctime, true);
    }
 
    /* After that, evaluate the actual Spline IK, unless there are missing dependencies. */
    tSplineIk_EvalState state;
 
    if (splineik_evaluate_init(tree, &state)) {
      /* Walk over each bone in the chain, calculating the effects of spline IK
       * - the chain is traversed in the opposite order to storage order
       *   (i.e. parent to children) so that dependencies are correct
       */
      for (int i = tree->chainlen - 1; i >= 0; i--) {
        bPoseChannel *pchan = tree->chain[i];
        splineik_evaluate_bone(tree, ob, pchan, i, &state);
      }
    }
 
    /* free the tree info specific to SplineIK trees now */
    if (tree->chain) {
      MEM_freeN(tree->chain);
    }
 
    /* free this tree */
    BLI_freelinkN(&pchan_root->siktree, tree);
  }
}
 
void BKE_pose_splineik_init_tree(Scene *scene, Object *ob, float ctime)
{
  splineik_init_tree(scene, ob, ctime);
}
 
void BKE_splineik_execute_tree(
    Depsgraph *depsgraph, Scene *scene, Object *ob, bPoseChannel *pchan_root, float ctime)
{
  splineik_execute_tree(depsgraph, scene, ob, pchan_root, ctime);
}
 
/* *************** Depsgraph evaluation callbacks ************ */
 
void BKE_pose_pchan_index_rebuild(bPose *pose)
{
  MEM_SAFE_FREE(pose->chan_array);
  const int num_channels = BLI_listbase_count(&pose->chanbase);
  pose->chan_array = MEM_malloc_arrayN<bPoseChannel *>(size_t(num_channels), "pose->chan_array");
  int pchan_index = 0;
  for (bPoseChannel *pchan = static_cast<bPoseChannel *>(pose->chanbase.first); pchan != nullptr;
       pchan = pchan->next)
  {
    pose->chan_array[pchan_index++] = pchan;
  }
}
 
BLI_INLINE bPoseChannel *pose_pchan_get_indexed(Object *ob, int pchan_index)
{
  bPose *pose = ob->pose;
  BLI_assert(pose != nullptr);
  BLI_assert(pose->chan_array != nullptr);
  BLI_assert(pchan_index >= 0);
  BLI_assert(pchan_index < MEM_allocN_len(pose->chan_array) / sizeof(bPoseChannel *));
  return pose->chan_array[pchan_index];
}
 
void BKE_pose_eval_init(Depsgraph *depsgraph, Scene * /*scene*/, Object *object)
{
  bPose *pose = object->pose;
  BLI_assert(pose != nullptr);
 
  DEG_debug_print_eval(depsgraph, __func__, object->id.name, object);
 
  BLI_assert(object->type == OB_ARMATURE);
 
  /* We demand having proper pose. */
  BLI_assert(object->pose != nullptr);
  BLI_assert((object->pose->flag & POSE_RECALC) == 0);
 
  /* world_to_object is needed for solvers. */
  invert_m4_m4(object->runtime->world_to_object.ptr(), object->object_to_world().ptr());
 
  /* clear flags */
  for (bPoseChannel *pchan = static_cast<bPoseChannel *>(pose->chanbase.first); pchan != nullptr;
       pchan = pchan->next)
  {
    pchan->flag &= ~(POSE_DONE | POSE_CHAIN | POSE_IKTREE | POSE_IKSPLINE);
 
    /* Free B-Bone shape data cache if it's not a B-Bone. */
    if (pchan->bone == nullptr || pchan->bone->segments <= 1) {
      BKE_pose_channel_free_bbone_cache(&pchan->runtime);
    }
  }
 
  BLI_assert(pose->chan_array != nullptr || BLI_listbase_is_empty(&pose->chanbase));
}
 
void BKE_pose_eval_init_ik(Depsgraph *depsgraph, Scene *scene, Object *object)
{
  DEG_debug_print_eval(depsgraph, __func__, object->id.name, object);
  BLI_assert(object->type == OB_ARMATURE);
  const float ctime = BKE_scene_ctime_get(scene); /* not accurate... */
  bArmature *armature = (bArmature *)object->data;
  if (armature->flag & ARM_RESTPOS) {
    return;
  }
  /* construct the IK tree (standard IK) */
  BIK_init_tree(depsgraph, scene, object, ctime);
  /* construct the Spline IK trees
   * - this is not integrated as an IK plugin, since it should be able
   *   to function in conjunction with standard IK. */
  BKE_pose_splineik_init_tree(scene, object, ctime);
}
 
void BKE_pose_eval_bone(Depsgraph *depsgraph, Scene *scene, Object *object, int pchan_index)
{
  const bArmature *armature = (bArmature *)object->data;
  if (armature->edbo != nullptr) {
    return;
  }
  bPoseChannel *pchan = pose_pchan_get_indexed(object, pchan_index);
  DEG_debug_print_eval_subdata(
      depsgraph, __func__, object->id.name, object, "pchan", pchan->name, pchan);
  BLI_assert(object->type == OB_ARMATURE);
  if (armature->flag & ARM_RESTPOS) {
    Bone *bone = pchan->bone;
    if (bone) {
      copy_m4_m4(pchan->pose_mat, bone->arm_mat);
      copy_v3_v3(pchan->pose_head, bone->arm_head);
      copy_v3_v3(pchan->pose_tail, bone->arm_tail);
    }
  }
  else {
    /* TODO(sergey): Currently if there are constraints full transform is
     * being evaluated in BKE_pose_constraints_evaluate. */
    if (pchan->constraints.first == nullptr) {
      if (pchan->flag & POSE_IKTREE || pchan->flag & POSE_IKSPLINE) {
        /* pass */
      }
      else {
        if ((pchan->flag & POSE_DONE) == 0) {
          /* TODO(sergey): Use time source node for time. */
          float ctime = BKE_scene_ctime_get(scene); /* not accurate... */
          BKE_pose_where_is_bone(depsgraph, scene, object, pchan, ctime, true);
        }
      }
    }
  }
}
 
void BKE_pose_constraints_evaluate(Depsgraph *depsgraph,
                                   Scene *scene,
                                   Object *object,
                                   int pchan_index)
{
  const bArmature *armature = (bArmature *)object->data;
  if (armature->edbo != nullptr) {
    return;
  }
  bPoseChannel *pchan = pose_pchan_get_indexed(object, pchan_index);
  DEG_debug_print_eval_subdata(
      depsgraph, __func__, object->id.name, object, "pchan", pchan->name, pchan);
  if (armature->flag & ARM_RESTPOS) {
    return;
  }
  if (pchan->flag & POSE_IKTREE || pchan->flag & POSE_IKSPLINE) {
    /* IK are being solved separately/ */
  }
  else {
    if ((pchan->flag & POSE_DONE) == 0) {
      float ctime = BKE_scene_ctime_get(scene); /* not accurate... */
      BKE_pose_where_is_bone(depsgraph, scene, object, pchan, ctime, true);
    }
  }
}
 
static void pose_channel_flush_to_orig_if_needed(Depsgraph *depsgraph,
                                                 Object *object,
                                                 bPoseChannel *pchan)
{
  if (!DEG_is_active(depsgraph)) {
    return;
  }
  const bArmature *armature = (bArmature *)object->data;
  if (armature->edbo != nullptr) {
    return;
  }
  bPoseChannel *pchan_orig = pchan->orig_pchan;
  /* TODO(sergey): Using BKE_pose_copy_pchan_result() introduces #70901, but why? */
  copy_m4_m4(pchan_orig->pose_mat, pchan->pose_mat);
  copy_m4_m4(pchan_orig->chan_mat, pchan->chan_mat);
  copy_v3_v3(pchan_orig->pose_head, pchan->pose_mat[3]);
  copy_m4_m4(pchan_orig->constinv, pchan->constinv);
  copy_v3_v3(pchan_orig->pose_tail, pchan->pose_tail);
  pchan_orig->constflag = pchan->constflag;
}
 
void BKE_pose_bone_done(Depsgraph *depsgraph, Object *object, int pchan_index)
{
  /* Note: tests in `armature_deform_test.cc` update pose matrices locally to avoid creating a full
   * depsgraph. Keep these in sync if this function is changed! */
 
  const bArmature *armature = (bArmature *)object->data;
  if (armature->edbo != nullptr) {
    return;
  }
  bPoseChannel *pchan = pose_pchan_get_indexed(object, pchan_index);
  float imat[4][4];
  DEG_debug_print_eval_subdata(
      depsgraph, __func__, object->id.name, object, "pchan", pchan->name, pchan);
  if (pchan->bone) {
    invert_m4_m4(imat, pchan->bone->arm_mat);
    mul_m4_m4m4(pchan->chan_mat, pchan->pose_mat, imat);
    if (!(pchan->bone->flag & BONE_NO_DEFORM)) {
      mat4_to_dquat(&pchan->runtime.deform_dual_quat, pchan->bone->arm_mat, pchan->chan_mat);
    }
  }
  pose_channel_flush_to_orig_if_needed(depsgraph, object, pchan);
  if (DEG_is_active(depsgraph)) {
    bPoseChannel *pchan_orig = pchan->orig_pchan;
    if (pchan->bone == nullptr || pchan->bone->segments <= 1) {
      BKE_pose_channel_free_bbone_cache(&pchan_orig->runtime);
    }
  }
}
 
void BKE_pose_eval_bbone_segments(Depsgraph *depsgraph, Object *object, int pchan_index)
{
  const bArmature *armature = (bArmature *)object->data;
  if (armature->edbo != nullptr) {
    return;
  }
  bPoseChannel *pchan = pose_pchan_get_indexed(object, pchan_index);
  DEG_debug_print_eval_subdata(
      depsgraph, __func__, object->id.name, object, "pchan", pchan->name, pchan);
  if (pchan->bone != nullptr && pchan->bone->segments > 1) {
    BKE_pchan_bbone_segments_cache_compute(pchan);
    if (DEG_is_active(depsgraph)) {
      BKE_pchan_bbone_segments_cache_copy(pchan->orig_pchan, pchan);
    }
  }
}
 
void BKE_pose_iktree_evaluate(Depsgraph *depsgraph,
                              Scene *scene,
                              Object *object,
                              int rootchan_index)
{
  const bArmature *armature = (bArmature *)object->data;
  if (armature->edbo != nullptr) {
    return;
  }
  bPoseChannel *rootchan = pose_pchan_get_indexed(object, rootchan_index);
  DEG_debug_print_eval_subdata(
      depsgraph, __func__, object->id.name, object, "rootchan", rootchan->name, rootchan);
  BLI_assert(object->type == OB_ARMATURE);
  const float ctime = BKE_scene_ctime_get(scene); /* not accurate... */
  if (armature->flag & ARM_RESTPOS) {
    return;
  }
  BIK_execute_tree(depsgraph, scene, object, rootchan, ctime);
}
 
void BKE_pose_splineik_evaluate(Depsgraph *depsgraph,
                                Scene *scene,
                                Object *object,
                                int rootchan_index)
 
{
  const bArmature *armature = (bArmature *)object->data;
  if (armature->edbo != nullptr) {
    return;
  }
  bPoseChannel *rootchan = pose_pchan_get_indexed(object, rootchan_index);
  DEG_debug_print_eval_subdata(
      depsgraph, __func__, object->id.name, object, "rootchan", rootchan->name, rootchan);
  BLI_assert(object->type == OB_ARMATURE);
  const float ctime = BKE_scene_ctime_get(scene); /* not accurate... */
  if (armature->flag & ARM_RESTPOS) {
    return;
  }
  BKE_splineik_execute_tree(depsgraph, scene, object, rootchan, ctime);
}
 
static void pose_eval_cleanup_common(Object *object)
{
  bPose *pose = object->pose;
  BLI_assert(pose != nullptr);
  BLI_assert(pose->chan_array != nullptr || BLI_listbase_is_empty(&pose->chanbase));
  UNUSED_VARS_NDEBUG(pose);
}
 
void BKE_pose_eval_done(Depsgraph *depsgraph, Object *object)
{
  bPose *pose = object->pose;
  BLI_assert(pose != nullptr);
  UNUSED_VARS_NDEBUG(pose);
  DEG_debug_print_eval(depsgraph, __func__, object->id.name, object);
  BLI_assert(object->type == OB_ARMATURE);
}
 
void BKE_pose_eval_cleanup(Depsgraph *depsgraph, Scene *scene, Object *object)
{
  bPose *pose = object->pose;
  BLI_assert(pose != nullptr);
  UNUSED_VARS_NDEBUG(pose);
  const float ctime = BKE_scene_ctime_get(scene); /* not accurate... */
  DEG_debug_print_eval(depsgraph, __func__, object->id.name, object);
  BLI_assert(object->type == OB_ARMATURE);
  /* Release the IK tree. */
  BIK_release_tree(scene, object, ctime);
  pose_eval_cleanup_common(object);
}