From 43c3c60de269e7b8940f8c177bb1e5b9e640c812 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Fri, 12 Apr 2013 10:04:18 +1000
Subject: [PATCH] AP_Math: moved a lot of vector templates to cpp from .h

this reduces the code size quite a lot on AVR
---
 libraries/AP_Math/vector2.cpp | 112 +++++++++++++++++++++++++++++
 libraries/AP_Math/vector2.h   | 106 +++++++--------------------
 libraries/AP_Math/vector3.cpp | 104 ++++++++++++++++++++++++++-
 libraries/AP_Math/vector3.h   | 131 ++++++++++------------------------
 4 files changed, 278 insertions(+), 175 deletions(-)

diff --git a/libraries/AP_Math/vector2.cpp b/libraries/AP_Math/vector2.cpp
index 03c8d9ce67..da858b0903 100644
--- a/libraries/AP_Math/vector2.cpp
+++ b/libraries/AP_Math/vector2.cpp
@@ -25,4 +25,116 @@ float Vector2<T>::length(void) const
 	return pythagorous2(x, y);
 }
 
+
+// dot product
+template <typename T>
+T Vector2<T>::operator *(const Vector2<T> &v) const
+{
+    return x*v.x + y*v.y;
+}
+
+template <typename T>
+Vector2<T> &Vector2<T>::operator *=(const T num)
+{
+    x*=num; y*=num;
+    return *this;
+}
+
+template <typename T>
+Vector2<T> &Vector2<T>::operator /=(const T num)
+{
+    x /= num; y /= num;
+    return *this;
+}
+
+template <typename T>
+Vector2<T> &Vector2<T>::operator -=(const Vector2<T> &v)
+{
+    x -= v.x; y -= v.y;
+    return *this;
+}
+
+template <typename T>
+bool Vector2<T>::is_nan(void) const
+{
+    return isnan(x) || isnan(y);
+}
+
+template <typename T>
+bool Vector2<T>::is_inf(void) const
+{
+    return isinf(x) || isinf(y);
+}
+
+template <typename T>
+Vector2<T> &Vector2<T>::operator +=(const Vector2<T> &v)
+{
+    x+=v.x; y+=v.y;
+    return *this;
+}
+
+template <typename T>
+Vector2<T> Vector2<T>::operator /(const T num) const
+{
+    return Vector2<T>(x/num, y/num);
+}
+
+template <typename T>
+Vector2<T> Vector2<T>::operator *(const T num) const
+{
+    return Vector2<T>(x*num, y*num);
+}
+
+template <typename T>
+Vector2<T> Vector2<T>::operator -(const Vector2<T> &v) const
+{
+    return Vector2<T>(x-v.x, y-v.y);
+}
+
+template <typename T>
+Vector2<T> Vector2<T>::operator +(const Vector2<T> &v) const
+{
+    return Vector2<T>(x+v.x, y+v.y);
+}
+
+template <typename T>
+Vector2<T> Vector2<T>::operator -(void) const
+{
+    return Vector2<T>(-x,-y);
+}
+
+template <typename T>
+bool Vector2<T>::operator ==(const Vector2<T> &v) const
+{
+    return (x==v.x && y==v.y);
+}
+
+template <typename T>
+bool Vector2<T>::operator !=(const Vector2<T> &v) const
+{
+    return (x!=v.x && y!=v.y);
+}
+
+template <typename T>
+float Vector2<T>::angle(const Vector2<T> &v2) const
+{
+    return acosf(((*this)*v2) / (this->length()*v2.length()));
+}
+
+// only define for float
 template float Vector2<float>::length(void) const;
+template float Vector2<float>::operator *(const Vector2<float> &v) const;
+template Vector2<float> &Vector2<float>::operator *=(const float num);
+template Vector2<float> &Vector2<float>::operator /=(const float num);
+template Vector2<float> &Vector2<float>::operator -=(const Vector2<float> &v);
+template Vector2<float> &Vector2<float>::operator +=(const Vector2<float> &v);
+template Vector2<float> Vector2<float>::operator /(const float num) const;
+template Vector2<float> Vector2<float>::operator *(const float num) const;
+template Vector2<float> Vector2<float>::operator +(const Vector2<float> &v) const;
+template Vector2<float> Vector2<float>::operator -(const Vector2<float> &v) const;
+template Vector2<float> Vector2<float>::operator -(void) const;
+template bool Vector2<float>::operator ==(const Vector2<float> &v) const;
+template bool Vector2<float>::operator !=(const Vector2<float> &v) const;
+template bool Vector2<float>::is_nan(void) const;
+template bool Vector2<float>::is_inf(void) const;
+template float Vector2<float>::angle(const Vector2<float> &v) const;
diff --git a/libraries/AP_Math/vector2.h b/libraries/AP_Math/vector2.h
index 68d4f98c84..6f08e19474 100644
--- a/libraries/AP_Math/vector2.h
+++ b/libraries/AP_Math/vector2.h
@@ -46,82 +46,55 @@ struct Vector2
     }
 
     // test for equality
-    bool operator==(const Vector2<T> &v)
-    {
-        return (x==v.x && y==v.y);
-    }
+    bool operator ==(const Vector2<T> &v) const;
 
     // test for inequality
-    bool operator!=(const Vector2<T> &v)
-    {
-        return (x!=v.x || y!=v.y);
-    }
+    bool operator !=(const Vector2<T> &v) const;
 
     // negation
-    Vector2<T> operator -(void) const
-    {
-        return Vector2<T>(-x, -y);
-    }
+    Vector2<T> operator -(void) const;
 
     // addition
-    Vector2<T> operator +(const Vector2<T> &v) const
-    {
-        return Vector2<T>(x+v.x, y+v.y);
-    }
+    Vector2<T> operator +(const Vector2<T> &v) const;
 
     // subtraction
-    Vector2<T> operator -(const Vector2<T> &v) const
-    {
-        return Vector2<T>(x-v.x, y-v.y);
-    }
+    Vector2<T> operator -(const Vector2<T> &v) const;
 
     // uniform scaling
-    Vector2<T> operator *(const T num) const
-    {
-        Vector2<T>        temp(*this);
-        return temp*=num;
-    }
+    Vector2<T> operator *(const T num) const;
 
     // uniform scaling
-    Vector2<T> operator /(const T num) const
-    {
-        Vector2<T>        temp(*this);
-        return temp/=num;
-    }
+    Vector2<T> operator  /(const T num) const;
 
     // addition
-    Vector2<T> &operator +=(const Vector2<T> &v)
-    {
-        x+=v.x; y+=v.y;
-        return *this;
-    }
+    Vector2<T> &operator +=(const Vector2<T> &v);
 
     // subtraction
-    Vector2<T> &operator -=(const Vector2<T> &v)
-    {
-        x-=v.x; y-=v.y;
-        return *this;
-    }
+    Vector2<T> &operator -=(const Vector2<T> &v);
 
     // uniform scaling
-    Vector2<T> &operator *=(const T num)
-    {
-        x*=num; y*=num;
-        return *this;
-    }
+    Vector2<T> &operator *=(const T num);
 
     // uniform scaling
-    Vector2<T> &operator /=(const T num)
-    {
-        x/=num; y/=num;
-        return *this;
-    }
+    Vector2<T> &operator /=(const T num);
 
     // dot product
-    T operator *(const Vector2<T> &v) const
-    {
-        return x*v.x + y*v.y;
-    }
+    T operator *(const Vector2<T> &v) const;
+
+    // cross product
+    Vector2<T> operator %(const Vector2<T> &v) const;
+
+    // computes the angle between this vector and another vector
+    float angle(const Vector2<T> &v2) const;
+
+    // computes the angle in radians between the origin and this vector
+    T angle(void) const;
+
+    // check if any elements are NAN
+    bool is_nan(void) const;
+
+    // check if any elements are infinity
+    bool is_inf(void) const;
 
     // gets the length of this vector squared
     T   length_squared() const
@@ -163,31 +136,6 @@ struct Vector2
     {
         return v * (*this * v)/(v*v);
     }
-
-    // computes the angle between 2 arbitrary vectors
-    T   angle(const Vector2<T> &v1, const Vector2<T> &v2)
-    {
-        return (T)acosf((v1*v2) / (v1.length()*v2.length()));
-    }
-
-    // computes the angle in radians between the origin and this vector
-    T   angle(void)
-    {
-        return (T)atan2f(y, x);
-    }
-
-    // computes the angle between this vector and another vector
-    T   angle(const Vector2<T> &v2)
-    {
-        return (T)acosf(((*this)*v2) / (this->length()*v2.length()));
-    }
-
-    // computes the angle between 2 normalized arbitrary vectors
-    T   angle_normalized(const Vector2<T> &v1, const Vector2<T> &v2)
-    {
-        return (T)acosf(v1*v2);
-    }
-
 };
 
 typedef Vector2<int16_t>        Vector2i;
diff --git a/libraries/AP_Math/vector3.cpp b/libraries/AP_Math/vector3.cpp
index c6ce8ac850..631667e586 100644
--- a/libraries/AP_Math/vector3.cpp
+++ b/libraries/AP_Math/vector3.cpp
@@ -184,8 +184,110 @@ float Vector3<T>::length(void) const
     return pythagorous3(x, y, z);
 }
 
-// only define for signed numbers
+template <typename T>
+Vector3<T> &Vector3<T>::operator *=(const T num)
+{
+    x*=num; y*=num; z*=num;
+    return *this;
+}
+
+template <typename T>
+Vector3<T> &Vector3<T>::operator /=(const T num)
+{
+    x /= num; y /= num; z /= num;
+    return *this;
+}
+
+template <typename T>
+Vector3<T> &Vector3<T>::operator -=(const Vector3<T> &v)
+{
+    x -= v.x; y -= v.y; z -= v.z;
+    return *this;
+}
+
+template <typename T>
+bool Vector3<T>::is_nan(void) const
+{
+    return isnan(x) || isnan(y) || isnan(z);
+}
+
+template <typename T>
+bool Vector3<T>::is_inf(void) const
+{
+    return isinf(x) || isinf(y) || isinf(z);
+}
+
+template <typename T>
+Vector3<T> &Vector3<T>::operator +=(const Vector3<T> &v)
+{
+    x+=v.x; y+=v.y; z+=v.z;
+    return *this;
+}
+
+template <typename T>
+Vector3<T> Vector3<T>::operator /(const T num) const
+{
+    return Vector3<T>(x/num, y/num, z/num);
+}
+
+template <typename T>
+Vector3<T> Vector3<T>::operator *(const T num) const
+{
+    return Vector3<T>(x*num, y*num, z*num);
+}
+
+template <typename T>
+Vector3<T> Vector3<T>::operator -(const Vector3<T> &v) const
+{
+    return Vector3<T>(x-v.x, y-v.y, z-v.z);
+}
+
+template <typename T>
+Vector3<T> Vector3<T>::operator +(const Vector3<T> &v) const
+{
+    return Vector3<T>(x+v.x, y+v.y, z+v.z);
+}
+
+template <typename T>
+Vector3<T> Vector3<T>::operator -(void) const
+{
+    return Vector3<T>(-x,-y,-z);
+}
+
+template <typename T>
+bool Vector3<T>::operator ==(const Vector3<T> &v) const
+{
+    return (x==v.x && y==v.y && z==v.z);
+}
+
+template <typename T>
+bool Vector3<T>::operator !=(const Vector3<T> &v) const
+{
+    return (x!=v.x && y!=v.y && z!=v.z);
+}
+
+template <typename T>
+float Vector3<T>::angle(const Vector3<T> &v2) const
+{
+    return acosf(((*this)*v2) / (this->length()*v2.length()));
+}
+
+// only define for float
 template void Vector3<float>::rotate(enum Rotation);
 template float Vector3<float>::length(void) const;
 template Vector3<float> Vector3<float>::operator %(const Vector3<float> &v) const;
 template float Vector3<float>::operator *(const Vector3<float> &v) const;
+template Vector3<float> &Vector3<float>::operator *=(const float num);
+template Vector3<float> &Vector3<float>::operator /=(const float num);
+template Vector3<float> &Vector3<float>::operator -=(const Vector3<float> &v);
+template Vector3<float> &Vector3<float>::operator +=(const Vector3<float> &v);
+template Vector3<float> Vector3<float>::operator /(const float num) const;
+template Vector3<float> Vector3<float>::operator *(const float num) const;
+template Vector3<float> Vector3<float>::operator +(const Vector3<float> &v) const;
+template Vector3<float> Vector3<float>::operator -(const Vector3<float> &v) const;
+template Vector3<float> Vector3<float>::operator -(void) const;
+template bool Vector3<float>::operator ==(const Vector3<float> &v) const;
+template bool Vector3<float>::operator !=(const Vector3<float> &v) const;
+template bool Vector3<float>::is_nan(void) const;
+template bool Vector3<float>::is_inf(void) const;
+template float Vector3<float>::angle(const Vector3<float> &v) const;
diff --git a/libraries/AP_Math/vector3.h b/libraries/AP_Math/vector3.h
index 15f051093c..ce673aa87c 100644
--- a/libraries/AP_Math/vector3.h
+++ b/libraries/AP_Math/vector3.h
@@ -60,168 +60,109 @@ public:
     }
 
     // function call operator
-    void operator        ()(const T x0, const T y0, const T z0)
+    void operator ()(const T x0, const T y0, const T z0)
     {
         x= x0; y= y0; z= z0;
     }
 
     // test for equality
-    bool operator        ==(const Vector3<T> &v)
-    {
-        return (x==v.x && y==v.y && z==v.z);
-    }
+    bool operator ==(const Vector3<T> &v) const;
 
     // test for inequality
-    bool operator        !=(const Vector3<T> &v)
-    {
-        return (x!=v.x || y!=v.y || z!=v.z);
-    }
+    bool operator !=(const Vector3<T> &v) const;
 
     // negation
-    Vector3<T> operator        -(void) const
-    {
-        return Vector3<T>(-x,-y,-z);
-    }
+    Vector3<T> operator -(void) const;
 
     // addition
-    Vector3<T> operator        +(const Vector3<T> &v) const
-    {
-        return Vector3<T>(x+v.x, y+v.y, z+v.z);
-    }
+    Vector3<T> operator +(const Vector3<T> &v) const;
 
     // subtraction
-    Vector3<T> operator        -(const Vector3<T> &v) const
-    {
-        return Vector3<T>(x-v.x, y-v.y, z-v.z);
-    }
+    Vector3<T> operator -(const Vector3<T> &v) const;
 
     // uniform scaling
-    Vector3<T> operator        *(const T num) const
-    {
-        Vector3<T>        temp(*this);
-        return temp*=num;
-    }
+    Vector3<T> operator *(const T num) const;
 
     // uniform scaling
-    Vector3<T> operator        /(const T num) const
-    {
-        Vector3<T>        temp(*this);
-        return temp/=num;
-    }
+    Vector3<T> operator  /(const T num) const;
 
     // addition
-    Vector3<T> &operator        +=(const Vector3<T> &v)
-    {
-        x+=v.x; y+=v.y; z+=v.z;
-        return *this;
-    }
+    Vector3<T> &operator +=(const Vector3<T> &v);
 
     // subtraction
-    Vector3<T> &operator        -=(const Vector3<T> &v)
-    {
-        x-=v.x; y-=v.y; z-=v.z;
-        return *this;
-    }
+    Vector3<T> &operator -=(const Vector3<T> &v);
 
     // uniform scaling
-    Vector3<T> &operator        *=(const T num)
-    {
-        x*=num; y*=num; z*=num;
-        return *this;
-    }
+    Vector3<T> &operator *=(const T num);
 
     // uniform scaling
-    Vector3<T> &operator        /=(const T num)
-    {
-        x/=num; y/=num; z/=num;
-        return *this;
-    }
+    Vector3<T> &operator /=(const T num);
 
     // dot product
-    T operator                  *(const Vector3<T> &v) const;
+    T operator *(const Vector3<T> &v) const;
 
     // cross product
-    Vector3<T> operator         %(const Vector3<T> &v) const;
+    Vector3<T> operator %(const Vector3<T> &v) const;
+
+    // computes the angle between this vector and another vector
+    float angle(const Vector3<T> &v2) const;
+
+    // check if any elements are NAN
+    bool is_nan(void) const;
+
+    // check if any elements are infinity
+    bool is_inf(void) const;
+
+    // rotate by a standard rotation
+    void rotate(enum Rotation rotation);
 
     // gets the length of this vector squared
-    T                           length_squared() const
+    T  length_squared() const
     {
         return (T)(*this * *this);
     }
 
     // gets the length of this vector
-    float           length(void) const;
+    float length(void) const;
 
     // normalizes this vector
-    void            normalize()
+    void normalize()
     {
-        *this/=length();
+        *this /= length();
     }
 
     // zero the vector
-    void        zero()
+    void zero()
     {
         x = y = z = 0.0;
     }
 
     // returns the normalized version of this vector
-    Vector3<T>        normalized() const
+    Vector3<T> normalized() const
     {
         return *this/length();
     }
 
     // reflects this vector about n
-    void        reflect(const Vector3<T> &n)
+    void  reflect(const Vector3<T> &n)
     {
         Vector3<T>        orig(*this);
         project(n);
-        *this= *this*2 - orig;
+        *this = *this*2 - orig;
     }
 
     // projects this vector onto v
-    void        project(const Vector3<T> &v)
+    void project(const Vector3<T> &v)
     {
         *this= v * (*this * v)/(v*v);
     }
 
     // returns this vector projected onto v
-    Vector3<T>        projected(const Vector3<T> &v)
+    Vector3<T> projected(const Vector3<T> &v) const
     {
         return v * (*this * v)/(v*v);
     }
 
-    // computes the angle between 2 arbitrary vectors
-    T        angle(const Vector3<T> &v1, const Vector3<T> &v2)
-    {
-        return (T)acosf((v1*v2) / (v1.length()*v2.length()));
-    }
-
-    // computes the angle between this vector and another vector
-    T        angle(const Vector3<T> &v2)
-    {
-        return (T)acosf(((*this)*v2) / (this->length()*v2.length()));
-    }
-
-    // computes the angle between 2 arbitrary normalized vectors
-    T        angle_normalized(const Vector3<T> &v1, const Vector3<T> &v2)
-    {
-        return (T)acosf(v1*v2);
-    }
-
-    // check if any elements are NAN
-    bool        is_nan(void)
-    {
-        return isnan(x) || isnan(y) || isnan(z);
-    }
-
-    // check if any elements are infinity
-    bool        is_inf(void)
-    {
-        return isinf(x) || isinf(y) || isinf(z);
-    }
-
-    // rotate by a standard rotation
-    void        rotate(enum Rotation rotation);
 
 };