diff options
Diffstat (limited to 'source/blender')
| -rw-r--r-- | source/blender/blenlib/BLI_math_base.h | 8 | ||||
| -rw-r--r-- | source/blender/blenlib/BLI_math_rotation.h | 1 | ||||
| -rw-r--r-- | source/blender/blenlib/intern/math_rotation.c | 49 |
3 files changed, 57 insertions, 1 deletions
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]) { |