Initial commit

Lib/Include/CML/mathlib/vector_angle.h

@@ -0,0 +1,69 @@
+/* -*- C++ -*- ------------------------------------------------------------
+ 
+Copyright (c) 2007 Jesse Anders and Demian Nave http://cmldev.net/
+
+The Configurable Math Library (CML) is distributed under the terms of the
+Boost Software License, v1.0 (see cml/LICENSE for details).
+
+ *-----------------------------------------------------------------------*/
+/** @file
+ *  @brief
+ */
+
+#ifndef vector_angle_h
+#define vector_angle_h
+
+#include <cml/mathlib/checking.h>
+
+/* Functions for finding the signed and unsigned angles between vectors in
+ * 3D and 2D.
+ *
+ * Note that the input vectors for these functions are not required to be
+ * unit length.
+ *
+ * @todo: Clean up promotions, conversions, and return types.
+ */
+
+namespace cml {
+
+/** Signed angle between two 3D vectors. */
+template< class VecT_1, class VecT_2, class VecT_3 >
+typename detail::DotPromote<
+    typename detail::CrossPromote<VecT_1,VecT_2>::promoted_vector, VecT_3
+>::promoted_scalar
+signed_angle(const VecT_1& v1, const VecT_2& v2, const VecT_3& reference)
+{
+    typedef typename detail::CrossPromote<VecT_1,VecT_2>::promoted_vector
+        vector_type;
+    typedef typename detail::DotPromote<vector_type,VecT_3>::promoted_scalar
+        value_type;
+
+    vector_type c = cross(v1,v2);
+    value_type angle = std::atan2(double(length(c)),double(dot(v1,v2)));
+    return dot(c,reference) < value_type(0) ? -angle : angle;
+}
+
+/** Unsigned angle between two 3D vectors. */
+template< class VecT_1, class VecT_2 >
+typename detail::DotPromote< VecT_1, VecT_2 >::promoted_scalar
+unsigned_angle(const VecT_1& v1, const VecT_2& v2) {
+    return std::atan2(double(length(cross(v1,v2))),double(dot(v1,v2)));
+}
+
+/** Signed angle between two 2D vectors. */
+template< class VecT_1, class VecT_2 >
+typename detail::DotPromote< VecT_1, VecT_2 >::promoted_scalar
+signed_angle_2D(const VecT_1& v1, const VecT_2& v2) {
+    return std::atan2(double(perp_dot(v1,v2)),double(dot(v1,v2)));
+}
+
+/** Unsigned angle between two 2D vectors. */
+template< class VecT_1, class VecT_2 >
+typename detail::DotPromote< VecT_1, VecT_2 >::promoted_scalar
+unsigned_angle_2D(const VecT_1& v1, const VecT_2& v2) {
+    return std::fabs(signed_angle_2D(v1,v2));
+}
+
+} // namespace cml
+
+#endif