Fix T37500: implement Bendy bone segment deformation interpolation.

Previously B-Bone deformation mapped every vertex to just one
B-Bone segment. This results in abrupt transformation differences
between the sides of each threshold plane, reducing the quality
of B-Bone deformation and making the use of shape keys impractical.

This commit replaces this approach with a linear blend between
the two closest segment transformations, effectively representing
the B-Bone as two weight-blended plain bones for each vertex.

In order to distribute the interpolation more evenly along the
bone, segment matrices for deformation are now computed at points
between the segments and at the ends of the B-Bone. The computation
also uses the true tangents of the Bezier curve for the orientation.
The nodes at the end of the bone require some special handling to
deal with zero-length Bezier handles caused by a zero ease value.

The Copy Transforms constraint now also smoothly interpolates
rotation and scaling along the bone shape when enabled.

The initial version of the patch was submitted by @Sam200.

Differential Revision: https://developer.blender.org/D4635

10 files changed, 297 insertions, 259 deletions

diff --git a/source/blender/blenkernel/BKE_armature.h b/source/blender/blenkernel/BKE_armature.h
index 09d8cbf933c..7182561a038 100644
--- a/source/blender/blenkernel/BKE_armature.h
+++ b/source/blender/blenkernel/BKE_armature.h
@@ -184,7 +184,7 @@ void BKE_pchan_bbone_spline_params_get(
         struct bPoseChannel *pchan, const bool rest, struct BBoneSplineParameters *r_param);
 
 void BKE_pchan_bbone_spline_setup(
-        struct bPoseChannel *pchan, const bool rest, Mat4 result_array[MAX_BBONE_SUBDIV]);
+        struct bPoseChannel *pchan, const bool rest, const bool for_deform, Mat4 *result_array);
 
 void BKE_pchan_bbone_handles_compute(
         const BBoneSplineParameters *param,
@@ -192,13 +192,15 @@ void BKE_pchan_bbone_handles_compute(
         float h2[3], float *r_roll2,
         bool ease, bool offsets);
 int  BKE_pchan_bbone_spline_compute(
-        struct BBoneSplineParameters *param, Mat4 result_array[MAX_BBONE_SUBDIV]);
+        struct BBoneSplineParameters *param, const bool for_deform, Mat4 *result_array);
 
 void BKE_pchan_bbone_segments_cache_compute(
         struct bPoseChannel *pchan);
 void BKE_pchan_bbone_segments_cache_copy(
         struct bPoseChannel *pchan, struct bPoseChannel *pchan_from);
 
+void BKE_pchan_bbone_deform_segment_index(const struct bPoseChannel *pchan, float pos, int *r_index, float *r_blend_next);
+
 /* like EBONE_VISIBLE */
 #define PBONE_VISIBLE(arm, bone) ( \
 	CHECK_TYPE_INLINE(arm, bArmature *), \
diff --git a/source/blender/blenkernel/intern/armature.c b/source/blender/blenkernel/intern/armature.c
index 6e0767836d7..5123b159440 100644
--- a/source/blender/blenkernel/intern/armature.c
+++ b/source/blender/blenkernel/intern/armature.c
@@ -36,6 +36,7 @@
 #include "BLI_ghash.h"
 #include "BLI_task.h"
 #include "BLI_utildefines.h"
+#include "BLI_alloca.h"
 
 #include "DNA_anim_types.h"
 #include "DNA_armature_types.h"
@@ -389,45 +390,60 @@ int bone_autoside_name(char name[MAXBONENAME], int UNUSED(strip_number), short a
 
 /* ************* B-Bone support ******************* */
 
-/* data has MAX_BBONE_SUBDIV+1 interpolated points, will become desired amount with equal distances */
-static void equalize_bbone_bezier(float *data, int desired)
+/* Compute a set of bezier parameter values that produce approximately equally spaced points. */
+static void equalize_cubic_bezier(const float control[4][3], int temp_segments, int final_segments, float *r_t_points)
 {
-	float *fp, totdist, ddist, dist, fac1, fac2;
-	float pdist[MAX_BBONE_SUBDIV + 1];
-	float temp[MAX_BBONE_SUBDIV + 1][4];
-	int a, nr;
+	float (*coords)[3] = BLI_array_alloca(coords, temp_segments + 1);
+	float *pdist = BLI_array_alloca(pdist, temp_segments + 1);
 
+	/* Compute the first pass of bezier point coordinates. */
+	for (int i = 0; i < 3; i++)	{
+		BKE_curve_forward_diff_bezier(
+			control[0][i], control[1][i], control[2][i], control[3][i],
+		    &coords[0][i], temp_segments, sizeof(*coords)
+		);
+	}
+
+	/* Calculate the length of the polyline at each point. */
 	pdist[0] = 0.0f;
-	for (a = 0, fp = data; a < MAX_BBONE_SUBDIV; a++, fp += 4) {
-		copy_qt_qt(temp[a], fp);
-		pdist[a + 1] = pdist[a] + len_v3v3(fp, fp + 4);
-	}
-	/* do last point */
-	copy_qt_qt(temp[a], fp);
-	totdist = pdist[a];
-
-	/* go over distances and calculate new points */
-	ddist = totdist / ((float)desired);
-	nr = 1;
-	for (a = 1, fp = data + 4; a < desired; a++, fp += 4) {
-		dist = ((float)a) * ddist;
-
-		/* we're looking for location (distance) 'dist' in the array */
-		while ((nr < MAX_BBONE_SUBDIV) && (dist >= pdist[nr]))
+
+	for (int i = 0; i < temp_segments; i++)
+		pdist[i + 1] = pdist[i] + len_v3v3(coords[i], coords[i + 1]);
+
+	/* Go over distances and calculate new parameter values. */
+	float dist_step = pdist[temp_segments] / final_segments;
+
+	r_t_points[0] = 0.0f;
+
+	for (int i = 1, nr = 1; i <= final_segments; i++) {
+		float dist = i * dist_step;
+
+		/* We're looking for location (distance) 'dist' in the array. */
+		while ((nr < temp_segments) && (dist >= pdist[nr]))
 			nr++;
 
-		fac1 = pdist[nr] - pdist[nr - 1];
-		fac2 = pdist[nr] - dist;
-		fac1 = fac2 / fac1;
-		fac2 = 1.0f - fac1;
+		float fac = (pdist[nr] - dist) / (pdist[nr] - pdist[nr - 1]);
 
-		fp[0] = fac1 * temp[nr - 1][0] + fac2 * temp[nr][0];
-		fp[1] = fac1 * temp[nr - 1][1] + fac2 * temp[nr][1];
-		fp[2] = fac1 * temp[nr - 1][2] + fac2 * temp[nr][2];
-		fp[3] = fac1 * temp[nr - 1][3] + fac2 * temp[nr][3];
+		r_t_points[i] = (nr - fac) / temp_segments;
 	}
-	/* set last point, needed for orientation calculus */
-	copy_qt_qt(fp, temp[MAX_BBONE_SUBDIV]);
+
+	r_t_points[final_segments] = 1.0f;
+}
+
+/* Evaluate bezier position and tangent at a specific parameter value using the De Casteljau algorithm. */
+static void evaluate_cubic_bezier(const float control[4][3], float t, float r_pos[3], float r_tangent[3])
+{
+	float layer1[3][3];
+	interp_v3_v3v3(layer1[0], control[0], control[1], t);
+	interp_v3_v3v3(layer1[1], control[1], control[2], t);
+	interp_v3_v3v3(layer1[2], control[2], control[3], t);
+
+	float layer2[2][3];
+	interp_v3_v3v3(layer2[0], layer1[0], layer1[1], t);
+	interp_v3_v3v3(layer2[1], layer1[1], layer1[2], t);
+
+	sub_v3_v3v3(r_tangent, layer2[1], layer2[0]);
+	madd_v3_v3v3fl(r_pos, layer2[0], r_tangent, t);
 }
 
 /* Get "next" and "prev" bones - these are used for handle calculations. */
@@ -640,13 +656,13 @@ void BKE_pchan_bbone_spline_params_get(struct bPoseChannel *pchan, const bool re
 
 /* Fills the array with the desired amount of bone->segments elements.
  * This calculation is done within unit bone space. */
-void BKE_pchan_bbone_spline_setup(bPoseChannel *pchan, const bool rest, Mat4 result_array[MAX_BBONE_SUBDIV])
+void BKE_pchan_bbone_spline_setup(bPoseChannel *pchan, const bool rest, const bool for_deform, Mat4 *result_array)
 {
 	BBoneSplineParameters param;
 
 	BKE_pchan_bbone_spline_params_get(pchan, rest, &param);
 
-	pchan->bone->segments = BKE_pchan_bbone_spline_compute(&param, result_array);
+	pchan->bone->segments = BKE_pchan_bbone_spline_compute(&param, for_deform, result_array);
 }
 
 /* Computes the bezier handle vectors and rolls coming from custom handles. */
@@ -654,6 +670,7 @@ void BKE_pchan_bbone_handles_compute(const BBoneSplineParameters *param, float h
 {
 	float mat3[3][3];
 	float length = param->length;
+	float epsilon = 1e-5 * length;
 
 	if (param->do_scale) {
 		length *= param->scale[1];
@@ -669,7 +686,9 @@ void BKE_pchan_bbone_handles_compute(const BBoneSplineParameters *param, float h
 			h1[1] -= length;
 		}
 
-		normalize_v3(h1);
+		if (normalize_v3(h1) < epsilon)
+			copy_v3_fl3(h1, 0.0f, -1.0f, 0.0f);
+
 		negate_v3(h1);
 
 		if (!param->prev_bbone) {
@@ -693,7 +712,8 @@ void BKE_pchan_bbone_handles_compute(const BBoneSplineParameters *param, float h
 			h2[1] -= length;
 		}
 
-		normalize_v3(h2);
+		if (normalize_v3(h2) < epsilon)
+			copy_v3_fl3(h2, 0.0f, 1.0f, 0.0f);
 
 		/* Find the next roll to interpolate as well. */
 		copy_m3_m4(mat3, param->next_mat);
@@ -749,20 +769,55 @@ void BKE_pchan_bbone_handles_compute(const BBoneSplineParameters *param, float h
 	}
 }
 
+static void make_bbone_spline_matrix(
+        BBoneSplineParameters *param, float scalemats[2][4][4],
+		float pos[3], float axis[3], float roll, float scalefac,
+		float result[4][4]
+) {
+	float mat3[3][3];
+
+	vec_roll_to_mat3(axis, roll, mat3);
+
+	copy_m4_m3(result, mat3);
+	copy_v3_v3(result[3], pos);
+
+	if (param->do_scale) {
+		/* Correct for scaling when this matrix is used in scaled space. */
+		mul_m4_series(result, scalemats[0], result, scalemats[1]);
+	}
+
+	/* BBone scale... */
+	mul_v3_fl(result[0], scalefac);
+	mul_v3_fl(result[2], scalefac);
+}
+
+/* Fade from first to second derivative when the handle is very short. */
+static void ease_handle_axis(const float deriv1[3], const float deriv2[3], float r_axis[3])
+{
+	const float gap = 0.1f;
+
+	copy_v3_v3(r_axis, deriv1);
+
+	float len1 = len_squared_v3(deriv1), len2 = len_squared_v3(deriv2);
+	float ratio = len1 / len2;
+
+	if (ratio < gap * gap) {
+		madd_v3_v3fl(r_axis, deriv2, gap - sqrtf(ratio));
+	}
+}
+
 /* Fills the array with the desired amount of bone->segments elements.
  * This calculation is done within unit bone space. */
-int BKE_pchan_bbone_spline_compute(BBoneSplineParameters *param, Mat4 result_array[MAX_BBONE_SUBDIV])
+int BKE_pchan_bbone_spline_compute(BBoneSplineParameters *param, const bool for_deform, Mat4 *result_array)
 {
-	float scalemat[4][4], iscalemat[4][4];
-	float mat3[3][3];
-	float h1[3], roll1, h2[3], roll2;
-	float data[MAX_BBONE_SUBDIV + 1][4], *fp;
+	float scalemats[2][4][4];
+	float bezt_controls[4][3];
+	float h1[3], roll1, h2[3], roll2, prev[3], cur[3], axis[3];
 	float length = param->length;
-	int a;
 
 	if (param->do_scale) {
-		size_to_mat4(scalemat, param->scale);
-		invert_m4_m4(iscalemat, scalemat);
+		size_to_mat4(scalemats[1], param->scale);
+		invert_m4_m4(scalemats[0], scalemats[1]);
 
 		length *= param->scale[1];
 	}
@@ -772,48 +827,59 @@ int BKE_pchan_bbone_spline_compute(BBoneSplineParameters *param, Mat4 result_arr
 	/* Make curve. */
 	CLAMP_MAX(param->segments, MAX_BBONE_SUBDIV);
 
-	BKE_curve_forward_diff_bezier(0.0f,  h1[0],                               h2[0],                               0.0f,   data[0],     MAX_BBONE_SUBDIV, 4 * sizeof(float));
-	BKE_curve_forward_diff_bezier(0.0f,  h1[1],                               length + h2[1],                      length, data[0] + 1, MAX_BBONE_SUBDIV, 4 * sizeof(float));
-	BKE_curve_forward_diff_bezier(0.0f,  h1[2],                               h2[2],                               0.0f,   data[0] + 2, MAX_BBONE_SUBDIV, 4 * sizeof(float));
-	BKE_curve_forward_diff_bezier(roll1, roll1 + 0.390464f * (roll2 - roll1), roll2 - 0.390464f * (roll2 - roll1), roll2,  data[0] + 3, MAX_BBONE_SUBDIV, 4 * sizeof(float));
+	copy_v3_fl3(bezt_controls[3], 0.0f, length, 0.0f);
+	add_v3_v3v3(bezt_controls[2], bezt_controls[3], h2);
+	copy_v3_v3(bezt_controls[1], h1);
+	zero_v3(bezt_controls[0]);
 
-	equalize_bbone_bezier(data[0], param->segments); /* note: does stride 4! */
+	float bezt_points[MAX_BBONE_SUBDIV + 1];
 
-	/* Make transformation matrices for the segments for drawing. */
-	for (a = 0, fp = data[0]; a < param->segments; a++, fp += 4) {
-		sub_v3_v3v3(h1, fp + 4, fp);
-		vec_roll_to_mat3(h1, fp[3], mat3); /* fp[3] is roll */
+	equalize_cubic_bezier(bezt_controls, MAX_BBONE_SUBDIV, param->segments, bezt_points);
 
-		copy_m4_m3(result_array[a].mat, mat3);
-		copy_v3_v3(result_array[a].mat[3], fp);
+	/* Deformation uses N+1 matrices computed at points between the segments. */
+	if (for_deform) {
+		/* Bezier derivatives. */
+		float bezt_deriv1[3][3], bezt_deriv2[2][3];
 
-		if (param->do_scale) {
-			/* Correct for scaling when this matrix is used in scaled space. */
-			mul_m4_series(result_array[a].mat, iscalemat, result_array[a].mat, scalemat);
+		for (int i = 0; i < 3; i++) {
+			sub_v3_v3v3(bezt_deriv1[i], bezt_controls[i + 1], bezt_controls[i]);
+		}
+		for (int i = 0; i < 2; i++) {
+			sub_v3_v3v3(bezt_deriv2[i], bezt_deriv1[i + 1], bezt_deriv1[i]);
 		}
 
-		/* BBone scale... */
-		{
-			const int num_segments = param->segments;
+		/* End points require special handling to fix zero length handles. */
+		ease_handle_axis(bezt_deriv1[0], bezt_deriv2[0], axis);
+		make_bbone_spline_matrix(param, scalemats, bezt_controls[0], axis, roll1, param->scaleIn, result_array[0].mat);
+
+		for (int a = 1; a < param->segments; a++) {
+			evaluate_cubic_bezier(bezt_controls, bezt_points[a], cur, axis);
 
-			const float scaleIn = param->scaleIn;
-			const float scaleFactorIn  = 1.0f + (scaleIn  - 1.0f) * ((float)(num_segments - a) / (float)num_segments);
+			float fac = ((float)a) / param->segments;
+			float roll = interpf(roll2, roll1, fac);
+			float scalefac = interpf(param->scaleOut, param->scaleIn, fac);
+
+			make_bbone_spline_matrix(param, scalemats, cur, axis, roll, scalefac, result_array[a].mat);
+		}
 
-			const float scaleOut = param->scaleOut;
-			const float scaleFactorOut = 1.0f + (scaleOut - 1.0f) * ((float)(a + 1)            / (float)num_segments);
+		ease_handle_axis(bezt_deriv1[2], bezt_deriv2[1], axis);
+		make_bbone_spline_matrix(param, scalemats, bezt_controls[3], axis, roll2, param->scaleOut, result_array[param->segments].mat);
+	}
+	/* Other code (e.g. display) uses matrices for the segments themselves. */
+	else {
+		zero_v3(prev);
 
-			const float scalefac = scaleFactorIn * scaleFactorOut;
-			float bscalemat[4][4], bscale[3];
+		for (int a = 0; a < param->segments; a++) {
+			evaluate_cubic_bezier(bezt_controls, bezt_points[a + 1], cur, axis);
 
-			bscale[0] = scalefac;
-			bscale[1] = 1.0f;
-			bscale[2] = scalefac;
+			sub_v3_v3v3(axis, cur, prev);
 
-			size_to_mat4(bscalemat, bscale);
+			float fac = (a + 0.5f) / param->segments;
+			float roll = interpf(roll2, roll1, fac);
+			float scalefac = interpf(param->scaleOut, param->scaleIn, fac);
 
-			/* Note: don't multiply by inverse scale mat here, as it causes problems with scaling shearing and breaking segment chains */
-			/*mul_m4_series(result_array[a].mat, ibscalemat, result_array[a].mat, bscalemat);*/
-			mul_m4_series(result_array[a].mat, result_array[a].mat, bscalemat);
+			make_bbone_spline_matrix(param, scalemats, prev, axis, roll, scalefac, result_array[a].mat);
+			copy_v3_v3(prev, cur);
 		}
 	}
 
@@ -843,10 +909,10 @@ static void allocate_bbone_cache(bPoseChannel *pchan, int segments)
 		}
 
 		runtime->bbone_segments = segments;
-		runtime->bbone_rest_mats = MEM_malloc_arrayN(sizeof(Mat4), (uint)segments, "bPoseChannel_Runtime::bbone_rest_mats");
-		runtime->bbone_pose_mats = MEM_malloc_arrayN(sizeof(Mat4), (uint)segments, "bPoseChannel_Runtime::bbone_pose_mats");
-		runtime->bbone_deform_mats = MEM_malloc_arrayN(sizeof(Mat4), 1 + (uint)segments, "bPoseChannel_Runtime::bbone_deform_mats");
-		runtime->bbone_dual_quats = MEM_malloc_arrayN(sizeof(DualQuat), (uint)segments, "bPoseChannel_Runtime::bbone_dual_quats");
+		runtime->bbone_rest_mats = MEM_malloc_arrayN(sizeof(Mat4), 1 + (uint)segments, "bPoseChannel_Runtime::bbone_rest_mats");
+		runtime->bbone_pose_mats = MEM_malloc_arrayN(sizeof(Mat4), 1 + (uint)segments, "bPoseChannel_Runtime::bbone_pose_mats");
+		runtime->bbone_deform_mats = MEM_malloc_arrayN(sizeof(Mat4), 2 + (uint)segments, "bPoseChannel_Runtime::bbone_deform_mats");
+		runtime->bbone_dual_quats = MEM_malloc_arrayN(sizeof(DualQuat), 1 + (uint)segments, "bPoseChannel_Runtime::bbone_dual_quats");
 	}
 }
 
@@ -869,8 +935,8 @@ void BKE_pchan_bbone_segments_cache_compute(bPoseChannel *pchan)
 	DualQuat *b_bone_dual_quats = runtime->bbone_dual_quats;
 	int a;
 
-	BKE_pchan_bbone_spline_setup(pchan, false, b_bone);
-	BKE_pchan_bbone_spline_setup(pchan, true, b_bone_rest);
+	BKE_pchan_bbone_spline_setup(pchan, false, true, b_bone);
+	BKE_pchan_bbone_spline_setup(pchan, true, true, b_bone_rest);
 
 	/* Compute deform matrices. */
 	/* first matrix is the inverse arm_mat, to bring points in local bone space
@@ -883,7 +949,7 @@ void BKE_pchan_bbone_segments_cache_compute(bPoseChannel *pchan)
 	 * - transform with b_bone matrix
 	 * - transform back into global space */
 
-	for (a = 0; a < bone->segments; a++) {
+	for (a = 0; a <= bone->segments; a++) {
 		float tmat[4][4];
 
 		invert_m4_m4(tmat, b_bone_rest[a].mat);
@@ -906,44 +972,83 @@ void BKE_pchan_bbone_segments_cache_copy(bPoseChannel *pchan, bPoseChannel *pcha
 	else {
 		allocate_bbone_cache(pchan, segments);
 
-		memcpy(runtime->bbone_rest_mats, runtime_from->bbone_rest_mats, sizeof(Mat4) * segments);
-		memcpy(runtime->bbone_pose_mats, runtime_from->bbone_pose_mats, sizeof(Mat4) * segments);
-		memcpy(runtime->bbone_deform_mats, runtime_from->bbone_deform_mats, sizeof(Mat4) * (1 + segments));
-		memcpy(runtime->bbone_dual_quats, runtime_from->bbone_dual_quats, sizeof(DualQuat) * segments);
+		memcpy(runtime->bbone_rest_mats, runtime_from->bbone_rest_mats, sizeof(Mat4) * (1 + segments));
+		memcpy(runtime->bbone_pose_mats, runtime_from->bbone_pose_mats, sizeof(Mat4) * (1 + segments));
+		memcpy(runtime->bbone_deform_mats, runtime_from->bbone_deform_mats, sizeof(Mat4) * (2 + segments));
+		memcpy(runtime->bbone_dual_quats, runtime_from->bbone_dual_quats, sizeof(DualQuat) * (1 + segments));
 	}
 }
 
-static void b_bone_deform(const bPoseChannel *pchan, float co[3], DualQuat *dq, float defmat[3][3])
+/** Calculate index and blend factor for the two B-Bone segment nodes affecting the point at 0 <= pos <= 1. */
+void BKE_pchan_bbone_deform_segment_index(const bPoseChannel *pchan, float pos, int *r_index, float *r_blend_next)
 {
-	Bone *bone = pchan->bone;
-	const Mat4 *b_bone = pchan->runtime.bbone_deform_mats;
-	const float (*mat)[4] = b_bone[0].mat;
-	float segment, y;
-	int a;
+	int segments = pchan->bone->segments;
 
-	/* need to transform co back to bonespace, only need y */
-	y = mat[0][1] * co[0] + mat[1][1] * co[1] + mat[2][1] * co[2] + mat[3][1];
+	CLAMP(pos, 0.0f, 1.0f);
 
-	/* now calculate which of the b_bones are deforming this */
-	segment = bone->length / ((float)bone->segments);
-	a = (int)(y / segment);
+	/* Calculate the indices of the 2 affecting b_bone segments.
+	 * Integer part is the first segment's index.
+	 * Integer part plus 1 is the second segment's index.
+	 * Fractional part is the blend factor. */
+	float pre_blend = pos * (float)segments;
 
-	/* note; by clamping it extends deform at endpoints, goes best with
-	 * straight joints in restpos. */
-	CLAMP(a, 0, bone->segments - 1);
+	int index = (int)floorf(pre_blend);
+	float blend = pre_blend - index;
 
-	if (dq) {
-		copy_dq_dq(dq, &(pchan->runtime.bbone_dual_quats)[a]);
+	CLAMP(index, 0, segments);
+	CLAMP(blend, 0.0f, 1.0f);
+
+	*r_index = index;
+	*r_blend_next = blend;
+}
+
+/* Add the effect of one bone or B-Bone segment to the accumulated result. */
+static void pchan_deform_accumulate(
+    const DualQuat *deform_dq, const float deform_mat[4][4], const float co_in[3], float weight,
+	float co_accum[3], DualQuat *dq_accum, float mat_accum[3][3]
+) {
+	if (weight == 0.0f)
+		return;
+
+	if (dq_accum) {
+		BLI_assert(!co_accum);
+
+		add_weighted_dq_dq(dq_accum, deform_dq, weight);
 	}
 	else {
-		mul_m4_v3(b_bone[a + 1].mat, co);
+		float tmp[3];
+		mul_v3_m4v3(tmp, deform_mat, co_in);
 
-		if (defmat) {
-			copy_m3_m4(defmat, b_bone[a + 1].mat);
+		sub_v3_v3(tmp, co_in);
+		madd_v3_v3fl(co_accum, tmp, weight);
+
+		if (mat_accum) {
+			float tmpmat[3][3];
+			copy_m3_m4(tmpmat, deform_mat);
+
+			madd_m3_m3m3fl(mat_accum, mat_accum, tmpmat, weight);
 		}
 	}
 }
 
+static void b_bone_deform(const bPoseChannel *pchan, const float co[3], float weight, float vec[3], DualQuat *dq, float defmat[3][3])
+{
+	const DualQuat *quats = pchan->runtime.bbone_dual_quats;
+	const Mat4 *mats = pchan->runtime.bbone_deform_mats;
+	const float (*mat)[4] = mats[0].mat;
+	float blend, y;
+	int index;
+
+	/* Transform co to bone space and get its y component. */
+	y = mat[0][1] * co[0] + mat[1][1] * co[1] + mat[2][1] * co[2] + mat[3][1];
+
+	/* Calculate the indices of the 2 affecting b_bone segments. */
+	BKE_pchan_bbone_deform_segment_index(pchan, y / pchan->bone->length, &index, &blend);
+
+	pchan_deform_accumulate(&quats[index], mats[index + 1].mat, co, weight * (1.0f - blend), vec, dq, defmat);
+	pchan_deform_accumulate(&quats[index + 1], mats[index + 2].mat, co, weight * blend, vec, dq, defmat);
+}
+
 /* using vec with dist to bone b1 - b2 */
 float distfactor_to_bone(const float vec[3], const float b1[3], const float b2[3], float rad1, float rad2, float rdist)
 {
@@ -996,60 +1101,25 @@ float distfactor_to_bone(const float vec[3], const float b1[3], const float b2[3
 	}
 }
 
-static void pchan_deform_mat_add(bPoseChannel *pchan, float weight, float bbonemat[3][3], float mat[3][3])
-{
-	float wmat[3][3];
-
-	if (pchan->bone->segments > 1)
-		copy_m3_m3(wmat, bbonemat);
-	else
-		copy_m3_m4(wmat, pchan->chan_mat);
-
-	mul_m3_fl(wmat, weight);
-	add_m3_m3m3(mat, mat, wmat);
-}
-
 static float dist_bone_deform(bPoseChannel *pchan, const bPoseChanDeform *pdef_info, float vec[3], DualQuat *dq,
                               float mat[3][3], const float co[3])
 {
 	Bone *bone = pchan->bone;
 	float fac, contrib = 0.0;
-	float cop[3], bbonemat[3][3];
-	DualQuat bbonedq;
 
 	if (bone == NULL)
 		return 0.0f;
 
-	copy_v3_v3(cop, co);
-
-	fac = distfactor_to_bone(cop, bone->arm_head, bone->arm_tail, bone->rad_head, bone->rad_tail, bone->dist);
+	fac = distfactor_to_bone(co, bone->arm_head, bone->arm_tail, bone->rad_head, bone->rad_tail, bone->dist);
 
 	if (fac > 0.0f) {
 		fac *= bone->weight;
 		contrib = fac;
 		if (contrib > 0.0f) {
-			if (vec) {
-				if (bone->segments > 1 && pchan->runtime.bbone_segments == bone->segments)
-					/* applies on cop and bbonemat */
-					b_bone_deform(pchan, cop, NULL, (mat) ? bbonemat : NULL);
-				else
-					mul_m4_v3(pchan->chan_mat, cop);
-
-				/* Make this a delta from the base position */
-				sub_v3_v3(cop, co);
-				madd_v3_v3fl(vec, cop, fac);
-
-				if (mat)
-					pchan_deform_mat_add(pchan, fac, bbonemat, mat);
-			}
-			else {
-				if (bone->segments > 1 && pchan->runtime.bbone_segments == bone->segments) {
-					b_bone_deform(pchan, cop, &bbonedq, NULL);
-					add_weighted_dq_dq(dq, &bbonedq, fac);
-				}
-				else
-					add_weighted_dq_dq(dq, pdef_info->dual_quat, fac);
-			}
+			if (bone->segments > 1 && pchan->runtime.bbone_segments == bone->segments)
+				b_bone_deform(pchan, co, fac, vec, dq, mat);
+			else
+				pchan_deform_accumulate(pdef_info->dual_quat, pchan->chan_mat, co, fac, vec, dq, mat);
 		}
 	}
 
@@ -1061,36 +1131,14 @@ static void pchan_bone_deform(bPoseChannel *pchan, const bPoseChanDeform *pdef_i
                               float mat[3][3], const float co[3], float *contrib)
 {
 	Bone *bone = pchan->bone;
-	float cop[3], bbonemat[3][3];
-	DualQuat bbonedq;
 
 	if (!weight)
 		return;
 
-	copy_v3_v3(cop, co);
-
-	if (vec) {
-		if (bone->segments > 1 && bone->segments == pchan->runtime.bbone_segments)
-			/* applies on cop and bbonemat */
-			b_bone_deform(pchan, cop, NULL, (mat) ? bbonemat : NULL);
-		else
-			mul_m4_v3(pchan->chan_mat, cop);
-
-		vec[0] += (cop[0] - co[0]) * weight;
-		vec[1] += (cop[1] - co[1]) * weight;
-		vec[2] += (cop[2] - co[2]) * weight;
-
-		if (mat)
-			pchan_deform_mat_add(pchan, weight, bbonemat, mat);
-	}
-	else {
-		if (bone->segments > 1 && bone->segments == pchan->runtime.bbone_segments) {
-			b_bone_deform(pchan, cop, &bbonedq, NULL);
-			add_weighted_dq_dq(dq, &bbonedq, weight);
-		}
-		else
-			add_weighted_dq_dq(dq, pdef_info->dual_quat, weight);
-	}
+	if (bone->segments > 1 && pchan->runtime.bbone_segments == bone->segments)
+		b_bone_deform(pchan, co, weight, vec, dq, mat);
+	else
+		pchan_deform_accumulate(pdef_info->dual_quat, pchan->chan_mat, co, weight, vec, dq, mat);
 
 	(*contrib) += weight;
 }
diff --git a/source/blender/blenkernel/intern/constraint.c b/source/blender/blenkernel/intern/constraint.c
index 6d7765d4314..7d9a6c234a0 100644
--- a/source/blender/blenkernel/intern/constraint.c
+++ b/source/blender/blenkernel/intern/constraint.c
@@ -592,58 +592,25 @@ static void constraint_target_to_mat4(Object *ob, const char *substring, float m
 				Mat4 *bbone = pchan->runtime.bbone_pose_mats;
 				float tempmat[4][4];
 				float loc[3], fac;
+				int index;
 
 				/* figure out which segment(s) the headtail value falls in */
-				fac = (float)pchan->bone->segments * headtail;
-
-				if (fac >= pchan->bone->segments - 1) {
-					/* special case: end segment doesn't get created properly... */
-					float pt[3], sfac;
-					int index;
-
-					/* bbone points are in bonespace, so need to move to posespace first */
-					index = pchan->bone->segments - 1;
-					mul_v3_m4v3(pt, pchan->pose_mat, bbone[index].mat[3]);
-
-					/* interpolate between last segment point and the endpoint */
-					sfac = fac - (float)(pchan->bone->segments - 1); /* fac is just the "leftover" between penultimate and last points */
-					interp_v3_v3v3(loc, pt, pchan->pose_tail, sfac);
-				}
-				else {
-					/* get indices for finding interpolating between points along the bbone */
-					float pt_a[3], pt_b[3], pt[3];
-					int   index_a, index_b;
-
-					index_a = floorf(fac);
-					CLAMP(index_a, 0, MAX_BBONE_SUBDIV - 1);
-
-					index_b = ceilf(fac);
-					CLAMP(index_b, 0, MAX_BBONE_SUBDIV - 1);
-
-					/* interpolate between these points */
-					copy_v3_v3(pt_a, bbone[index_a].mat[3]);
-					copy_v3_v3(pt_b, bbone[index_b].mat[3]);
-
-					interp_v3_v3v3(pt, pt_a, pt_b, fac - floorf(fac));
-
-					/* move the point from bone local space to pose space... */
-					mul_v3_m4v3(loc, pchan->pose_mat, pt);
-				}
+				BKE_pchan_bbone_deform_segment_index(pchan, headtail, &index, &fac);
 
 				/* apply full transformation of the segment if requested */
 				if (full_bbone) {
-					int index = floorf(fac);
-					CLAMP(index, 0, pchan->bone->segments - 1);
+					interp_m4_m4m4(tempmat, bbone[index].mat, bbone[index + 1].mat, fac);
 
-					mul_m4_m4m4(tempmat, pchan->pose_mat, bbone[index].mat);
+					mul_m4_m4m4(tempmat, pchan->pose_mat, tempmat);
 				}
+				/* only interpolate location */
 				else {
+					interp_v3_v3v3(loc, bbone[index].mat[3], bbone[index + 1].mat[3], fac);
+
 					copy_m4_m4(tempmat, pchan->pose_mat);
+					mul_v3_m4v3(tempmat[3], pchan->pose_mat, loc);
 				}
 
-				/* use interpolated distance for subtarget */
-				copy_v3_v3(tempmat[3], loc);
-
 				mul_m4_m4m4(mat, ob->obmat, tempmat);
 			}
 			else {
@@ -2155,10 +2122,31 @@ static void armdef_get_tarmat(struct Depsgraph *UNUSED(depsgraph),
 	}
 }
 
+static void armdef_accumulate_matrix(float obmat[4][4], float iobmat[4][4], float basemat[4][4], float bonemat[4][4], float weight, float r_sum_mat[4][4], DualQuat *r_sum_dq)
+{
+	if (weight == 0.0f)
+		return;
+
+	/* Convert the selected matrix into object space. */
+	float mat[4][4];
+	mul_m4_series(mat, obmat, bonemat, iobmat);
+
+	/* Accumulate the transformation. */
+	if (r_sum_dq != NULL) {
+		DualQuat tmpdq;
+
+		mat4_to_dquat(&tmpdq, basemat, mat);
+		add_weighted_dq_dq(r_sum_dq, &tmpdq, weight);
+	}
+	else {
+		madd_m4_m4m4fl(r_sum_mat, r_sum_mat, mat, weight);
+	}
+}
+
 /* Compute and accumulate transformation for a single target bone. */
 static void armdef_accumulate_bone(bConstraintTarget *ct, bPoseChannel *pchan, const float wco[3], bool force_envelope, float *r_totweight, float r_sum_mat[4][4], DualQuat *r_sum_dq)
 {
-	float mat[4][4], iobmat[4][4], basemat[4][4], co[3];
+	float iobmat[4][4], basemat[4][4], co[3];
 	Bone *bone = pchan->bone;
 	float weight = ct->weight;
 
@@ -2172,6 +2160,11 @@ static void armdef_accumulate_bone(bConstraintTarget *ct, bPoseChannel *pchan, c
 		                             bone->rad_head, bone->rad_tail, bone->dist);
 	}
 
+	/* Compute the quaternion base matrix. */
+	if (r_sum_dq != NULL) {
+		mul_m4_series(basemat, ct->tar->obmat, bone->arm_mat, iobmat);
+	}
+
 	/* Find the correct bone transform matrix in world space. */
 	if (bone->segments > 1 && bone->segments == pchan->runtime.bbone_segments) {
 		Mat4 *b_bone_mats = pchan->runtime.bbone_deform_mats;
@@ -2182,34 +2175,21 @@ static void armdef_accumulate_bone(bConstraintTarget *ct, bPoseChannel *pchan, c
 		 * Need to transform co back to bonespace, only need y. */
 		float y = iamat[0][1] * co[0] + iamat[1][1] * co[1] + iamat[2][1] * co[2] + iamat[3][1];
 
-		float segment = bone->length / ((float)bone->segments);
-		int a = (int)(y / segment);
-
-		CLAMP(a, 0, bone->segments - 1);
+		/* Blend the matrix. */
+		int index;
+		float blend;
+		BKE_pchan_bbone_deform_segment_index(pchan, y / bone->length, &index, &blend);
 
-		/* Convert the selected matrix into object space. */
-		mul_m4_series(mat, ct->tar->obmat, b_bone_mats[a + 1].mat, iobmat);
+		armdef_accumulate_matrix(ct->tar->obmat, iobmat, basemat, b_bone_mats[index + 1].mat, weight * (1.0f - blend), r_sum_mat, r_sum_dq);
+		armdef_accumulate_matrix(ct->tar->obmat, iobmat, basemat, b_bone_mats[index + 2].mat, weight * blend, r_sum_mat, r_sum_dq);
 	}
 	else {
 		/* Simple bone. This requires DEG_OPCODE_BONE_DONE dependency due to chan_mat. */
-		mul_m4_series(mat, ct->tar->obmat, pchan->chan_mat, iobmat);
+		armdef_accumulate_matrix(ct->tar->obmat, iobmat, basemat, pchan->chan_mat, weight, r_sum_mat, r_sum_dq);
 	}
 
-	/* Accumulate the transformation. */
+	/* Accumulate the weight. */
 	*r_totweight += weight;
-
-	if (r_sum_dq != NULL) {
-		DualQuat tmpdq;
-
-		mul_m4_series(basemat, ct->tar->obmat, bone->arm_mat, iobmat);
-
-		mat4_to_dquat(&tmpdq, basemat, mat);
-		add_weighted_dq_dq(r_sum_dq, &tmpdq, weight);
-	}
-	else {
-		mul_m4_fl(mat, weight);
-		add_m4_m4m4(r_sum_mat, r_sum_mat, mat);
-	}
 }
 
 static void armdef_evaluate(bConstraint *con, bConstraintOb *cob, ListBase *targets)
diff --git a/source/blender/blenlib/BLI_math_matrix.h b/source/blender/blenlib/BLI_math_matrix.h
index 8e35b197e14..49e05f5401e 100644
--- a/source/blender/blenlib/BLI_math_matrix.h
+++ b/source/blender/blenlib/BLI_math_matrix.h
@@ -59,6 +59,9 @@ void swap_m4m4(float A[4][4], float B[4][4]);
 void add_m3_m3m3(float R[3][3], const float A[3][3], const float B[3][3]);
 void add_m4_m4m4(float R[4][4], const float A[4][4], const float B[4][4]);
 
+void madd_m3_m3m3fl(float R[3][3], const float A[3][3], const float B[3][3], const float f);
+void madd_m4_m4m4fl(float R[4][4], const float A[4][4], const float B[4][4], const float f);
+
 void sub_m3_m3m3(float R[3][3], const float A[3][3], const float B[3][3]);
 void sub_m4_m4m4(float R[4][4], const float A[4][4], const float B[4][4]);
 
diff --git a/source/blender/blenlib/intern/math_matrix.c b/source/blender/blenlib/intern/math_matrix.c
index a98b65d0330..f21d883252c 100644
--- a/source/blender/blenlib/intern/math_matrix.c
+++ b/source/blender/blenlib/intern/math_matrix.c
@@ -813,6 +813,28 @@ void add_m4_m4m4(float m1[4][4], const float m2[4][4], const float m3[4][4])
 	}
 }
 
+void madd_m3_m3m3fl(float m1[3][3], const float m2[3][3], const float m3[3][3], const float f)
+{
+	int i, j;
+
+	for (i = 0; i < 3; i++) {
+		for (j = 0; j < 3; j++) {
+			m1[i][j] = m2[i][j] + m3[i][j] * f;
+		}
+	}
+}
+
+void madd_m4_m4m4fl(float m1[4][4], const float m2[4][4], const float m3[4][4], const float f)
+{
+	int i, j;
+
+	for (i = 0; i < 4; i++) {
+		for (j = 0; j < 4; j++) {
+			m1[i][j] = m2[i][j] + m3[i][j] * f;
+		}
+	}
+}
+
 void sub_m3_m3m3(float m1[3][3], const float m2[3][3], const float m3[3][3])
 {
 	int i, j;
diff --git a/source/blender/draw/intern/draw_armature.c b/source/blender/draw/intern/draw_armature.c
index 786ecbb1451..06013bad7c0 100644
--- a/source/blender/draw/intern/draw_armature.c
+++ b/source/blender/draw/intern/draw_armature.c
@@ -1079,7 +1079,7 @@ static void ebone_spline_preview(EditBone *ebone, float result_array[MAX_BBONE_S
 	param.curveOutX = ebone->curveOutX;
 	param.curveOutY = ebone->curveOutY;
 
-	ebone->segments = BKE_pchan_bbone_spline_compute(&param, (Mat4 *)result_array);
+	ebone->segments = BKE_pchan_bbone_spline_compute(&param, false, (Mat4 *)result_array);
 }
 
 static void draw_bone_update_disp_matrix_bbone(EditBone *eBone, bPoseChannel *pchan)
@@ -1118,12 +1118,7 @@ static void draw_bone_update_disp_matrix_bbone(EditBone *eBone, bPoseChannel *pc
 	if (pchan) {
 		Mat4 *bbones_mat = (Mat4 *)pchan->draw_data->bbone_matrix;
 		if (bbone_segments > 1) {
-			if (bbone_segments == pchan->runtime.bbone_segments) {
-				memcpy(bbones_mat, pchan->runtime.bbone_pose_mats, sizeof(Mat4) * bbone_segments);
-			}
-			else {
-				BKE_pchan_bbone_spline_setup(pchan, false, bbones_mat);
-			}
+			BKE_pchan_bbone_spline_setup(pchan, false, false, bbones_mat);
 
 			for (int i = bbone_segments; i--; bbones_mat++) {
 				mul_m4_m4m4(bbones_mat->mat, bbones_mat->mat, s);
diff --git a/source/blender/editors/armature/armature_skinning.c b/source/blender/editors/armature/armature_skinning.c
index 5608479905a..5aec55bd1ed 100644
--- a/source/blender/editors/armature/armature_skinning.c
+++ b/source/blender/editors/armature/armature_skinning.c
@@ -318,7 +318,7 @@ static void add_verts_to_dgroups(
 				if ((par->pose) && (pchan = BKE_pose_channel_find_name(par->pose, bone->name))) {
 					if (bone->segments > 1) {
 						segments = bone->segments;
-						BKE_pchan_bbone_spline_setup(pchan, true, bbone_array);
+						BKE_pchan_bbone_spline_setup(pchan, true, false, bbone_array);
 						bbone = bbone_array;
 					}
 				}
diff --git a/source/blender/editors/gpencil/gpencil_armature.c b/source/blender/editors/gpencil/gpencil_armature.c
index a482eb837e7..8fff6b207ba 100644
--- a/source/blender/editors/gpencil/gpencil_armature.c
+++ b/source/blender/editors/gpencil/gpencil_armature.c
@@ -314,7 +314,7 @@ static void gpencil_add_verts_to_dgroups(
 			{
 				if (bone->segments > 1) {
 					segments = bone->segments;
-					BKE_pchan_bbone_spline_setup(pchan, true, bbone_array);
+					BKE_pchan_bbone_spline_setup(pchan, true, false, bbone_array);
 					bbone = bbone_array;
 				}
 			}
diff --git a/source/blender/makesrna/intern/rna_pose_api.c b/source/blender/makesrna/intern/rna_pose_api.c
index 2658cd552bc..4366b18c049 100644
--- a/source/blender/makesrna/intern/rna_pose_api.c
+++ b/source/blender/makesrna/intern/rna_pose_api.c
@@ -63,7 +63,7 @@ static void rna_PoseBone_bbone_segment_matrix(bPoseChannel *pchan, ReportList *r
 		BKE_reportf(reports, RPT_ERROR, "Bone '%s' has out of date B-Bone segment data!", pchan->name);
 		return;
 	}
-	if (index < 0 || index >= pchan->runtime.bbone_segments) {
+	if (index < 0 || index > pchan->runtime.bbone_segments) {
 		BKE_reportf(reports, RPT_ERROR, "Invalid index %d for B-Bone segments of '%s'!", index, pchan->name);
 		return;
 	}
@@ -115,13 +115,13 @@ void RNA_api_pose_channel(StructRNA *srna)
 
 	/* B-Bone segment matrices */
 	func = RNA_def_function(srna, "bbone_segment_matrix", "rna_PoseBone_bbone_segment_matrix");
-	RNA_def_function_ui_description(func, "Retrieve the matrix of the B-Bone segment if available");
+	RNA_def_function_ui_description(func, "Retrieve the matrix of the joint between B-Bone segments if available");
 	RNA_def_function_flag(func, FUNC_USE_REPORTS);
 	parm = RNA_def_property(func, "matrix_return", PROP_FLOAT, PROP_MATRIX);
 	RNA_def_property_multi_array(parm, 2, rna_matrix_dimsize_4x4);
 	RNA_def_property_ui_text(parm, "", "The resulting matrix in bone local space");
 	RNA_def_function_output(func, parm);
-	parm = RNA_def_int(func, "index", 0, 0, INT_MAX, "", "Index of the segment", 0, 10000);
+	parm = RNA_def_int(func, "index", 0, 0, INT_MAX, "", "Index of the segment endpoint", 0, 10000);
 	RNA_def_parameter_flags(parm, 0, PARM_REQUIRED);
 	parm = RNA_def_boolean(func, "rest", false, "", "Return the rest pose matrix");
 
diff --git a/source/blender/physics/intern/implicit_blender.c b/source/blender/physics/intern/implicit_blender.c
index cc8caca9588..83f586c4078 100644
--- a/source/blender/physics/intern/implicit_blender.c
+++ b/source/blender/physics/intern/implicit_blender.c
@@ -493,18 +493,6 @@ BLI_INLINE void madd_m3_m3fl(float r[3][3], float m[3][3], float f)
 	r[2][2] += m[2][2] * f;
 }
 
-BLI_INLINE void madd_m3_m3m3fl(float r[3][3], float a[3][3], float b[3][3], float f)
-{
-	r[0][0] = a[0][0] + b[0][0] * f;
-	r[0][1] = a[0][1] + b[0][1] * f;
-	r[0][2] = a[0][2] + b[0][2] * f;
-	r[1][0] = a[1][0] + b[1][0] * f;
-	r[1][1] = a[1][1] + b[1][1] * f;
-	r[1][2] = a[1][2] + b[1][2] * f;
-	r[2][0] = a[2][0] + b[2][0] * f;
-	r[2][1] = a[2][1] + b[2][1] * f;
-	r[2][2] = a[2][2] + b[2][2] * f;
-}
 /////////////////////////////////////////////////////////////////
 
 ///////////////////////////