watchrobot/libraries/AP_Math/vector4.cpp

/*
 * vector3.cpp
 * Copyright (C) Andrew Tridgell 2012
 *
 * This file is free software: you can redistribute it and/or modify it
 * under the terms of the GNU General Public License as published by the
 * Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This file is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
 * See the GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License along
 * with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

#pragma GCC optimize("O2")

#include "AP_Math.h"

#define HALF_SQRT_2 0.70710678118654757f


template <typename T>
float Vector4<T>::length(void) const
{
    return norm(M0, M1, M2, M3);
}



// dot product
template <typename T>
T Vector4<T>::operator *(const Vector4<T> &v) const
{
    return M0*v.M0 + M1*v.M1 + M2*v.M2 +M3*v.M3;
}

template <typename T>
Vector4<T> Vector4<T>::operator *(const T num) const
{
    return Vector4<T>(M0*num, M1*num, M2*num,M3*num);
}


template <typename T>
Vector4<T> &Vector4<T>::operator *=(const T num)
{
    M0*=num; M1*=num; M2*=num;M3*=num;
    return *this;
}

template <typename T>
Vector4<T> &Vector4<T>::operator /=(const T num)
{
    M0 /= num; M1 /= num; M2 /= num;M3 /= num;
    return *this;
}
template <typename T>
Vector4<T> Vector4<T>::operator /(const T num) const
{
    return Vector4<T>(M0/num, M1/num, M2/num, M3/num);
}

template <typename T>
Vector4<T> &Vector4<T>::operator -=(const Vector4<T> &v)
{
    M0 -= v.M0; M1 -= v.M1; M2 -= v.M2;M3 -= v.M3;
    return *this;
}
template <typename T>
Vector4<T> Vector4<T>::operator -(const Vector4<T> &v) const
{
    return Vector4<T>(M0-v.M0, M1-v.M1, M2-v.M2, M3-v.M3);
}
template <typename T>
Vector4<T> Vector4<T>::operator -(void) const
{
    return Vector4<T>(-M0,-M1,-M2,-M3);
}

template <typename T>
bool Vector4<T>::is_nan(void) const
{
    return isnan(M0) || isnan(M1) || isnan(M2)|| isnan(M3);
}

template <typename T>
bool Vector4<T>::is_inf(void) const
{
    return isinf(M0) || isinf(M1) || isinf(M2)|| isinf(M3);
}

template <typename T>
Vector4<T> &Vector4<T>::operator +=(const Vector4<T> &v)
{
    M0+=v.M0; M1+=v.M1; M2+=v.M2;M3+=v.M3;
    return *this;
}

template <typename T>
Vector4<T> Vector4<T>::operator +(const Vector4<T> &v) const
{
    return Vector4<T>(M0+v.M0, M1+v.M1, M2+v.M2, M3+v.M3);
}



template <typename T>
bool Vector4<T>::operator ==(const Vector4<T> &v) const
{
    return (is_equal(M0,v.M0) && is_equal(M1,v.M1) && is_equal(M2,v.M2) && is_equal(M3,v.M3));
}

template <typename T>
bool Vector4<T>::operator !=(const Vector4<T> &v) const
{
    return (!is_equal(M0,v.M0) || !is_equal(M1,v.M1) || !is_equal(M2,v.M2)||!is_equal(M3,v.M3));
}



// define for float

template float Vector4<float>::length(void) const;
template float Vector4<float>::operator *(const Vector4<float> &v) const;
template Vector4<float> Vector4<float>::operator *(const float num) const;
template Vector4<float> &Vector4<float>::operator *=(const float num);
template Vector4<float> &Vector4<float>::operator /=(const float num);
template Vector4<float> Vector4<float>::operator /(const float num) const;
template Vector4<float> &Vector4<float>::operator -=(const Vector4<float> &v);
template Vector4<float> Vector4<float>::operator -(const Vector4<float> &v) const;
template Vector4<float> Vector4<float>::operator -(void) const;
template bool Vector4<float>::is_nan(void) const;
template bool Vector4<float>::is_inf(void) const;
template Vector4<float> &Vector4<float>::operator +=(const Vector4<float> &v);
template Vector4<float> Vector4<float>::operator +(const Vector4<float> &v) const;

template bool Vector4<float>::operator ==(const Vector4<float> &v) const;
template bool Vector4<float>::operator !=(const Vector4<float> &v) const;