Dogcows Code - chaz/yoink/blob - src/cml/mathlib/vector_angle.h

   1 /* -*- C++ -*- ------------------------------------------------------------
   2
   3 Copyright (c) 2007 Jesse Anders and Demian Nave http://cmldev.net/
   4
   5 The Configurable Math Library (CML) is distributed under the terms of the
   6 Boost Software License, v1.0 (see cml/LICENSE for details).
   7
   8  *-----------------------------------------------------------------------*/
   9 /** @file
  10  *  @brief
  11  */
  12
  13 #ifndef vector_angle_h
  14 #define vector_angle_h
  15
  16 #include <cml/mathlib/checking.h>
  17
  18 /* Functions for finding the signed and unsigned angles between vectors in
  19  * 3D and 2D.
  20  *
  21  * Note that the input vectors for these functions are not required to be
  22  * unit length.
  23  *
  24  * @todo: Clean up promotions, conversions, and return types.
  25  */
  26
  27 namespace cml {
  28
  29 /** Signed angle between two 3D vectors. */
  30 template< class VecT_1, class VecT_2, class VecT_3 >
  31 typename detail::DotPromote<
  32     typename detail::CrossPromote<VecT_1,VecT_2>::promoted_vector, VecT_3
  33 >::promoted_scalar
  34 signed_angle(const VecT_1& v1, const VecT_2& v2, const VecT_3& reference)
  35 {
  36     typedef typename detail::CrossPromote<VecT_1,VecT_2>::promoted_vector
  37         vector_type;
  38     typedef typename detail::DotPromote<vector_type,VecT_3>::promoted_scalar
  39         value_type;
  40
  41     vector_type c = cross(v1,v2);
  42     value_type angle = std::atan2(double(length(c)),double(dot(v1,v2)));
  43     return dot(c,reference) < value_type(0) ? -angle : angle;
  44 }
  45
  46 /** Unsigned angle between two 3D vectors. */
  47 template< class VecT_1, class VecT_2 >
  48 typename detail::DotPromote< VecT_1, VecT_2 >::promoted_scalar
  49 unsigned_angle(const VecT_1& v1, const VecT_2& v2) {
  50     return std::atan2(double(length(cross(v1,v2))),double(dot(v1,v2)));
  51 }
  52
  53 /** Signed angle between two 2D vectors. */
  54 template< class VecT_1, class VecT_2 >
  55 typename detail::DotPromote< VecT_1, VecT_2 >::promoted_scalar
  56 signed_angle_2D(const VecT_1& v1, const VecT_2& v2) {
  57     return std::atan2(double(perp_dot(v1,v2)),double(dot(v1,v2)));
  58 }
  59
  60 /** Unsigned angle between two 2D vectors. */
  61 template< class VecT_1, class VecT_2 >
  62 typename detail::DotPromote< VecT_1, VecT_2 >::promoted_scalar
  63 unsigned_angle_2D(const VecT_1& v1, const VecT_2& v2) {
  64     return std::fabs(signed_angle_2D(v1,v2));
  65 }
  66
  67 } // namespace cml
  68
  69 #endif