Py API: Vector.slerp(). also added interp_v3_v3v3_slerp(_safe) functions

4 files changed, 155 insertions, 15 deletions

diff --git a/source/blender/blenlib/BLI_math_rotation.h b/source/blender/blenlib/BLI_math_rotation.h
index eb4579887f7..25c9b5e468f 100644
--- a/source/blender/blenlib/BLI_math_rotation.h
+++ b/source/blender/blenlib/BLI_math_rotation.h
@@ -69,6 +69,7 @@ float normalize_qt_qt(float q1[4], const float q2[4]);
 bool is_zero_qt(const float q[4]);
 
 /* interpolation */
+void interp_dot_slerp(const float t, const float cosom, float w[2]);
 void interp_qt_qtqt(float q[4], const float a[4], const float b[4], const float t);
 void add_qt_qtqt(float q[4], const float a[4], const float b[4], const float t);
 
diff --git a/source/blender/blenlib/BLI_math_vector.h b/source/blender/blenlib/BLI_math_vector.h
index 9ef2dc781de..1f4ccf880fb 100644
--- a/source/blender/blenlib/BLI_math_vector.h
+++ b/source/blender/blenlib/BLI_math_vector.h
@@ -190,6 +190,12 @@ void interp_v4_v4v4v4(float p[4], const float v1[4], const float v2[4], const fl
 void interp_v4_v4v4v4v4(float p[4], const float v1[4], const float v2[4], const float v3[4], const float v4[4], const float w[4]);
 void interp_v3_v3v3v3_uv(float p[3], const float v1[3], const float v2[3], const float v3[3], const float uv[2]);
 
+bool interp_v3_v3v3_slerp(float target[3], const float a[3], const float b[3], const float t)  ATTR_WARN_UNUSED_RESULT;
+bool interp_v2_v2v2_slerp(float target[2], const float a[2], const float b[2], const float t)  ATTR_WARN_UNUSED_RESULT;
+
+void interp_v3_v3v3_slerp_safe(float target[3], const float a[3], const float b[3], const float t);
+void interp_v2_v2v2_slerp_safe(float target[2], const float a[2], const float b[2], const float t);
+
 void interp_v3_v3v3_char(char target[3], const char a[3], const char b[3], const float t);
 void interp_v3_v3v3_uchar(unsigned char target[3], const unsigned char a[3], const unsigned char b[3], const float t);
 void interp_v4_v4v4_char(char target[4], const char a[4], const char b[4], const float t);
@@ -255,6 +261,7 @@ void project_v3_plane(float v[3], const float n[3], const float p[3]);
 void reflect_v3_v3v3(float r[3], const float v[3], const float n[3]);
 void ortho_basis_v3v3_v3(float r1[3], float r2[3], const float a[3]);
 void ortho_v3_v3(float p[3], const float v[3]);
+void ortho_v2_v2(float p[3], const float v[3]);
 void bisect_v3_v3v3v3(float r[3], const float a[3], const float b[3], const float c[3]);
 void rotate_v3_v3v3fl(float v[3], const float p[3], const float axis[3], const float angle);
 void rotate_normalized_v3_v3v3fl(float v[3], const float p[3], const float axis[3], const float angle);
diff --git a/source/blender/blenlib/intern/math_rotation.c b/source/blender/blenlib/intern/math_rotation.c
index 1136020a4f7..64016de1718 100644
--- a/source/blender/blenlib/intern/math_rotation.c
+++ b/source/blender/blenlib/intern/math_rotation.c
@@ -623,9 +623,42 @@ void QuatInterpolW(float *result, float quat1[4], float quat2[4], float t)
 }
 #endif
 
+/**
+ * Generic function for implementing slerp
+ * (quaternions and spherical vector coords).
+ *
+ * \param t: factor in [0..1]
+ * \param cosom: dot product from normalized vectors/quats.
+ * \param r_w: calculated weights.
+ */
+void interp_dot_slerp(const float t, const float cosom, float r_w[2])
+{
+	const float eps = 0.0001f;
+
+	BLI_assert(IN_RANGE_INCL(cosom, -1.0f, 1.0f));
+
+	/* within [-1..1] range, avoid aligned axis */
+	if (LIKELY(fabsf(cosom) < (1.0f - eps))) {
+		float omega, sinom;
+
+		omega = acosf(cosom);
+		sinom = sinf(omega);
+		r_w[0] = sinf((1.0f - t) * omega) / sinom;
+		r_w[1] = sinf(t * omega) / sinom;
+	}
+	else {
+		/* fallback to lerp */
+		r_w[0] = 1.0f - t;
+		r_w[1] = t;
+	}
+}
+
 void interp_qt_qtqt(float result[4], const float quat1[4], const float quat2[4], const float t)
 {
-	float quat[4], omega, cosom, sinom, sc1, sc2;
+	float quat[4], cosom, w[2];
+
+	BLI_ASSERT_UNIT_QUAT(quat1);
+	BLI_ASSERT_UNIT_QUAT(quat2);
 
 	cosom = dot_qtqt(quat1, quat2);
 
@@ -638,21 +671,12 @@ void interp_qt_qtqt(float result[4], const float quat1[4], const float quat2[4],
 		copy_qt_qt(quat, quat1);
 	}
 
-	if ((1.0f - cosom) > 0.0001f) {
-		omega = acosf(cosom);
-		sinom = sinf(omega);
-		sc1 = sinf((1.0f - t) * omega) / sinom;
-		sc2 = sinf(t * omega) / sinom;
-	}
-	else {
-		sc1 = 1.0f - t;
-		sc2 = t;
-	}
+	interp_dot_slerp(t, cosom, w);
 
-	result[0] = sc1 * quat[0] + sc2 * quat2[0];
-	result[1] = sc1 * quat[1] + sc2 * quat2[1];
-	result[2] = sc1 * quat[2] + sc2 * quat2[2];
-	result[3] = sc1 * quat[3] + sc2 * quat2[3];
+	result[0] = w[0] * quat[0] + w[1] * quat2[0];
+	result[1] = w[0] * quat[1] + w[1] * quat2[1];
+	result[2] = w[0] * quat[2] + w[1] * quat2[2];
+	result[3] = w[0] * quat[3] + w[1] * quat2[3];
 }
 
 void add_qt_qtqt(float result[4], const float quat1[4], const float quat2[4], const float t)
diff --git a/source/blender/blenlib/intern/math_vector.c b/source/blender/blenlib/intern/math_vector.c
index 3fbddacaea2..8455bf7550f 100644
--- a/source/blender/blenlib/intern/math_vector.c
+++ b/source/blender/blenlib/intern/math_vector.c
@@ -68,6 +68,103 @@ void interp_v4_v4v4(float target[4], const float a[4], const float b[4], const f
 	target[3] = s * a[3] + t * b[3];
 }
 
+/**
+ * slerp, treat vectors as spherical coordinates
+ * \see #interp_qt_qtqt
+ *
+ * \return success
+ */
+bool interp_v3_v3v3_slerp(float target[3], const float a[3], const float b[3], const float t)
+{
+	float cosom, w[2];
+
+	BLI_ASSERT_UNIT_V3(a);
+	BLI_ASSERT_UNIT_V3(b);
+
+	cosom = dot_v3v3(a, b);
+
+	/* direct opposites */
+	if (UNLIKELY(cosom < (-1.0f + FLT_EPSILON))) {
+		return false;
+	}
+
+	interp_dot_slerp(t, cosom, w);
+
+	target[0] = w[0] * a[0] + w[1] * b[0];
+	target[1] = w[0] * a[1] + w[1] * b[1];
+	target[2] = w[0] * a[2] + w[1] * b[2];
+
+	return true;
+}
+bool interp_v2_v2v2_slerp(float target[2], const float a[2], const float b[2], const float t)
+{
+	float cosom, w[2];
+
+	BLI_ASSERT_UNIT_V2(a);
+	BLI_ASSERT_UNIT_V2(b);
+
+	cosom = dot_v2v2(a, b);
+
+	/* direct opposites */
+	if (UNLIKELY(cosom < (1.0f + FLT_EPSILON))) {
+		return false;
+	}
+
+	interp_dot_slerp(t, cosom, w);
+
+	target[0] = w[0] * a[0] + w[1] * b[0];
+	target[1] = w[0] * a[1] + w[1] * b[1];
+
+	return true;
+}
+
+/**
+ * Same as #interp_v3_v3v3_slerp buy uses fallback values
+ * for opposite vectors.
+ */
+void interp_v3_v3v3_slerp_safe(float target[3], const float a[3], const float b[3], const float t)
+{
+	if (UNLIKELY(!interp_v3_v3v3_slerp(target, a, b, t))) {
+		/* axis are aligned so any otho vector is acceptable */
+		float ab_ortho[3];
+		ortho_v3_v3(ab_ortho, a);
+		normalize_v3(ab_ortho);
+		if (t < 0.5f) {
+			if (UNLIKELY(!interp_v3_v3v3_slerp(target, a, ab_ortho, t * 2.0f))) {
+				BLI_assert(0);
+				copy_v3_v3(target, a);
+			}
+		}
+		else {
+			if (UNLIKELY(!interp_v3_v3v3_slerp(target, ab_ortho, b, (t - 0.5f) * 2.0f))) {
+				BLI_assert(0);
+				copy_v3_v3(target, b);
+			}
+		}
+	}
+}
+void interp_v2_v2v2_slerp_safe(float target[2], const float a[2], const float b[2], const float t)
+{
+	if (UNLIKELY(!interp_v2_v2v2_slerp(target, a, b, t))) {
+		/* axis are aligned so any otho vector is acceptable */
+		float ab_ortho[2];
+		ortho_v2_v2(ab_ortho, a);
+		// normalize_v2(ab_ortho);
+		if (t < 0.5f) {
+			if (UNLIKELY(!interp_v2_v2v2_slerp(target, a, ab_ortho, t * 2.0f))) {
+				BLI_assert(0);
+				copy_v2_v2(target, a);
+			}
+		}
+		else {
+			if (UNLIKELY(!interp_v2_v2v2_slerp(target, ab_ortho, b, (t - 0.5f) * 2.0f))) {
+				BLI_assert(0);
+				copy_v2_v2(target, b);
+			}
+		}
+	}
+}
+
 /* weight 3 vectors,
  * 'w' must be unit length but is not a vector, just 3 weights */
 void interp_v3_v3v3v3(float p[3], const float v1[3], const float v2[3], const float v3[3], const float w[3])
@@ -538,6 +635,17 @@ void ortho_v3_v3(float p[3], const float v[3])
 	}
 }
 
+/**
+ * no brainer compared to v3, just have for consistency.
+ */
+void ortho_v2_v2(float p[3], const float v[3])
+{
+	BLI_assert(p != v);
+
+	p[0] = -v[1];
+	p[1] =  v[0];
+}
+
 /* Rotate a point p by angle theta around an arbitrary axis r
  * http://local.wasp.uwa.edu.au/~pbourke/geometry/
  */