From 11ddd4b117aad96f671faa907df14d85f74af39a Mon Sep 17 00:00:00 2001
From: Adam Wulkiewicz <adam.wulkiewicz@gmail.com>
Date: Fri, 31 May 2013 19:43:38 +0000
Subject: [PATCH] geometry extensions: quaternion_rotate moved to
 detail::algebra

[SVN r84585]
---
 .../extensions/algebra/algorithms/detail.hpp  | 23 ++++++++++++++++++-
 .../algorithms/transform_geometrically.hpp    | 18 ++-------------
 2 files changed, 24 insertions(+), 17 deletions(-)

diff --git a/include/boost/geometry/extensions/algebra/algorithms/detail.hpp b/include/boost/geometry/extensions/algebra/algorithms/detail.hpp
index ac8f2d258..94ee8a742 100644
--- a/include/boost/geometry/extensions/algebra/algorithms/detail.hpp
+++ b/include/boost/geometry/extensions/algebra/algorithms/detail.hpp
@@ -151,13 +151,34 @@ struct matrix_mul_impl<M, V, VD, N, N>
 };
 
 template <typename M, typename V, typename VD>
-inline static void matrix_mul(M const& m, V const& v, VD & vd)
+inline static void matrix_rotate(M const& m, V const& v, VD & vd)
 {
     static const std::size_t dimension = traits::dimension<M>::value;
 
     matrix_mul_impl<M, V, VD, 0, dimension>::apply(m, v, vd);
 }
 
+template <typename V, typename Q>
+inline static void quaternion_rotate(V & v, Q const& q)
+{
+    // TODO - choose more precise type?
+
+    typedef typename select_most_precise<
+        typename traits::coordinate_type<V>::type,
+        typename traits::coordinate_type<Q>::type
+    >::type T;
+
+    // Hamilton product T=Q*V
+    T a = /*get<0>(r) * 0 */- get<1>(r) * get<0>(v) - get<2>(r) * get<1>(v) - get<3>(r) * get<2>(v);
+    T b = get<0>(r) * get<0>(v)/* + get<1>(r) * 0*/ + get<2>(r) * get<2>(v) - get<3>(r) * get<1>(v);
+    T c = get<0>(r) * get<1>(v) - get<1>(r) * get<2>(v)/* + get<2>(r) * 0*/ + get<3>(r) * get<0>(v);
+    T d = get<0>(r) * get<2>(v) + get<1>(r) * get<1>(v) - get<2>(r) * get<0>(v)/* + get<3>(r) * 0*/;
+    // Hamilton product V=T*inv(Q)
+    set<0>(v, - a * get<1>(r) + b * get<0>(r) - c * get<3>(r) + d * get<2>(r));
+    set<1>(v, - a * get<2>(r) + b * get<3>(r) + c * get<0>(r) - d * get<1>(r));
+    set<2>(v, - a * get<3>(r) - b * get<2>(r) + c * get<1>(r) + d * get<0>(r));
+}
+
 }} // namespace detail::algebra
 
 }} // namespace boost::geometry
diff --git a/include/boost/geometry/extensions/algebra/algorithms/transform_geometrically.hpp b/include/boost/geometry/extensions/algebra/algorithms/transform_geometrically.hpp
index 7f9e5cadd..501cdde79 100644
--- a/include/boost/geometry/extensions/algebra/algorithms/transform_geometrically.hpp
+++ b/include/boost/geometry/extensions/algebra/algorithms/transform_geometrically.hpp
@@ -126,21 +126,7 @@ struct transform_geometrically<Vector, RotationQuaternion, vector_tag, rotation_
     {
         concept::check_concepts_and_equal_dimensions<Vector, RotationQuaternion const>();
 
-        // TODO - choose more precise type?
-
-        typedef typename select_most_precise<
-            typename traits::coordinate_type<Vector>::type,
-            typename traits::coordinate_type<RotationQuaternion>::type
-        >::type T;
-
-        T a = /*get<0>(r) * 0 */- get<1>(r) * get<0>(v) - get<2>(r) * get<1>(v) - get<3>(r) * get<2>(v);
-        T b = get<0>(r) * get<0>(v)/* + get<1>(r) * 0*/ + get<2>(r) * get<2>(v) - get<3>(r) * get<1>(v);
-        T c = get<0>(r) * get<1>(v) - get<1>(r) * get<2>(v)/* + get<2>(r) * 0*/ + get<3>(r) * get<0>(v);
-        T d = get<0>(r) * get<2>(v) + get<1>(r) * get<1>(v) - get<2>(r) * get<0>(v)/* + get<3>(r) * 0*/;
-
-        set<0>(v, - a * get<1>(r) + b * get<0>(r) - c * get<3>(r) + d * get<2>(r));
-        set<1>(v, - a * get<2>(r) + b * get<3>(r) + c * get<0>(r) - d * get<1>(r));
-        set<2>(v, - a * get<3>(r) - b * get<2>(r) + c * get<1>(r) + d * get<0>(r));
+        detail::algebra::quaternion_rotate(v, r);
     }
 };
 
@@ -154,7 +140,7 @@ struct transform_geometrically<Vector, RotationMatrix, vector_tag, rotation_matr
 
         // TODO vector_type and convert from Vector
         Vector tmp(v);
-        detail::algebra::matrix_mul(r, tmp, v);
+        detail::algebra::matrix_rotate(r, tmp, v);
     }
 };