boost/geometry
2
0
Fork 0
mirror of https://github.com/boostorg/geometry.git synced 2026-02-21 15:12:17 +00:00
Code Issues Projects Releases Wiki Activity

geometry extensions: quaternion_rotate moved to detail::algebra

Browse Source 
[SVN r84585]
This commit is contained in:
Adam Wulkiewicz
2013-05-31 19:43:38 +00:00
parent 8336d7f387
commit 11ddd4b117
2 changed files with 24 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files

23
include/boost/geometry/extensions/algebra/algorithms/detail.hpp
View File

@@ -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

18
include/boost/geometry/extensions/algebra/algorithms/transform_geometrically.hpp
View File

@@ -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);
}
};