From fabc46b41f43aa93c5118fceb4f482419e75ed6f Mon Sep 17 00:00:00 2001
From: Campbell Barton <ideasman42@gmail.com>
Date: Sat, 19 Apr 2014 15:36:47 +1000
Subject: Math Lib: add rotation_between_vecs_to_mat3

- behaves like rotation_between_vecs_to_quat
- avoids calling sin,cos calls (approx 1.6x faster).
---
 source/blender/blenlib/BLI_math_base.h        |  8 ++++-
 source/blender/blenlib/BLI_math_rotation.h    |  1 +
 source/blender/blenlib/intern/math_rotation.c | 49 +++++++++++++++++++++++++++
 3 files changed, 57 insertions(+), 1 deletion(-)

(limited to 'source')

diff --git a/source/blender/blenlib/BLI_math_base.h b/source/blender/blenlib/BLI_math_base.h
index de35ccd58f4..68ffb4b4893 100644
--- a/source/blender/blenlib/BLI_math_base.h
+++ b/source/blender/blenlib/BLI_math_base.h
@@ -279,18 +279,24 @@ double double_round(double x, int ndigits);
 } (void)0
 
 #  define BLI_ASSERT_ZERO_M3(m)  {                                            \
-	BLI_assert(dot_vn_vn((const float *)m, (const float *)m, 9) != 0.0);     \
+	BLI_assert(dot_vn_vn((const float *)m, (const float *)m, 9) != 0.0);      \
 } (void)0
 
 #  define BLI_ASSERT_ZERO_M4(m)  {                                            \
 	BLI_assert(dot_vn_vn((const float *)m, (const float *)m, 16) != 0.0);     \
 } (void)0
+#  define BLI_ASSERT_UNIT_M3(m)  {                                            \
+	BLI_ASSERT_UNIT_V3((m)[0]);                                               \
+	BLI_ASSERT_UNIT_V3((m)[1]);                                               \
+	BLI_ASSERT_UNIT_V3((m)[2]);                                               \
+} (void)0
 #else
 #  define BLI_ASSERT_UNIT_V2(v) (void)(v)
 #  define BLI_ASSERT_UNIT_V3(v) (void)(v)
 #  define BLI_ASSERT_UNIT_QUAT(v) (void)(v)
 #  define BLI_ASSERT_ZERO_M3(m) (void)(m)
 #  define BLI_ASSERT_ZERO_M4(m) (void)(m)
+#  define BLI_ASSERT_UNIT_M3(m) (void)(m)
 #endif
 
 #endif /* __BLI_MATH_BASE_H__ */
diff --git a/source/blender/blenlib/BLI_math_rotation.h b/source/blender/blenlib/BLI_math_rotation.h
index 25c9b5e468f..6c63217e0ba 100644
--- a/source/blender/blenlib/BLI_math_rotation.h
+++ b/source/blender/blenlib/BLI_math_rotation.h
@@ -84,6 +84,7 @@ void tri_to_quat_ex(float quat[4], const float v1[3], const float v2[3], const f
 float tri_to_quat(float q[4], const float a[3], const float b[3], const float c[3]);
 void  vec_to_quat(float q[4], const float vec[3], short axis, const short upflag);
 /* note: v1 and v2 must be normalized */
+void rotation_between_vecs_to_mat3(float m[3][3], const float v1[3], const float v2[3]);
 void rotation_between_vecs_to_quat(float q[4], const float v1[3], const float v2[3]);
 void rotation_between_quats_to_quat(float q[4], const float q1[4], const float q2[4]);
 
diff --git a/source/blender/blenlib/intern/math_rotation.c b/source/blender/blenlib/intern/math_rotation.c
index 46f508b12bc..00fdf445209 100644
--- a/source/blender/blenlib/intern/math_rotation.c
+++ b/source/blender/blenlib/intern/math_rotation.c
@@ -400,6 +400,55 @@ float normalize_qt_qt(float r[4], const float q[4])
 	return normalize_qt(r);
 }
 
+/**
+ * Calculate a rotation matrix from 2 normalized vectors.
+ *
+ * \note faster then using axis/angle functions.
+ */
+void rotation_between_vecs_to_mat3(float m[3][3], const float v1[3], const float v2[3])
+{
+	float axis[3];
+
+	BLI_ASSERT_UNIT_V3(v1);
+	BLI_ASSERT_UNIT_V3(v2);
+
+	cross_v3_v3v3(axis, v1, v2);
+
+	if (normalize_v3(axis) > FLT_EPSILON) {
+		float m1[3][3], m2[3][3];
+
+axis_calc:
+		BLI_ASSERT_UNIT_V3(axis);
+
+		copy_v3_v3(m1[0], v1);
+		copy_v3_v3(m2[0], v2);
+
+		copy_v3_v3(m1[1], axis);
+		copy_v3_v3(m2[1], axis);
+
+		cross_v3_v3v3(m1[2], m1[1], m1[0]);
+		cross_v3_v3v3(m2[2], m2[1], m2[0]);
+
+		transpose_m3(m2);
+		mul_m3_m3m3(m, m1, m2);
+		transpose_m3(m);
+
+		BLI_ASSERT_UNIT_M3(m);
+	}
+	else {
+		if (dot_v3v3(v1, v2) > 0.0f) {
+			/* Same vectors, zero rotation... */
+			unit_m3(m);
+		}
+		else {
+			/* Colinear but opposed vectors, 180 rotation... */
+			ortho_v3_v3(axis, v1);
+			normalize_v3(axis);
+			goto axis_calc;
+		}
+	}
+}
+
 /* note: expects vectors to be normalized */
 void rotation_between_vecs_to_quat(float q[4], const float v1[3], const float v2[3])
 {
-- 
cgit v1.2.3