N3_sub/libraries/AP_Math/vector4.h

/*
   This program 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 program 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/>.
 */

// Copyright 2010 Michael Smith, all rights reserved.

// Derived closely from:
/****************************************
* 3D Vector Classes
* By Bill Perone (billperone@yahoo.com)
* Original: 9-16-2002
* Revised: 19-11-2003
*          11-12-2003
*          18-12-2003
*          06-06-2004
*
* Copyright 2003, This code is provided "as is" and you can use it freely as long as
* credit is given to Bill Perone in the application it is used in
*
* Notes:
* if a*b = 0 then a & b are orthogonal
* a%b = -b%a
* a*(b%c) = (a%b)*c
* a%b = a(cast to matrix)*b
* (a%b).length() = area of parallelogram formed by a & b
* (a%b).length() = a.length()*b.length() * sin(angle between a & b)
* (a%b).length() = 0 if angle between a & b = 0 or a.length() = 0 or b.length() = 0
* a * (b%c) = volume of parallelpiped formed by a, b, c
* vector triple product: a%(b%c) = b*(a*c) - c*(a*b)
* scalar triple product: a*(b%c) = c*(a%b) = b*(c%a)
* vector quadruple product: (a%b)*(c%d) = (a*c)*(b*d) - (a*d)*(b*c)
* if a is unit vector along b then a%b = -b%a = -b(cast to matrix)*a = 0
* vectors a1...an are linearly dependent if there exists a vector of scalars (b) where a1*b1 + ... + an*bn = 0
*           or if the matrix (A) * b = 0
*
****************************************/
#pragma once

#include <cmath>
#include <float.h>
#include <string.h>
#if MATH_CHECK_INDEXES
#include <assert.h>
#endif

#include "rotations.h"

template <typename T>
class Matrix4;

template <typename T>
class Vector4
{

public:
    T         M0, M1, M2, M3;

    // trivial ctor
    constexpr Vector4<T>()
        : M0(0)
        , M1(0)
        , M2(0)
		, M3(0){}

    // setting ctor
    constexpr Vector4<T>(const T x0, const T x1, const T x2,const T x3)
			: M0(x0)
			, M1(x1)
			, M2(x2)
			, M3(x3){}


    // function call operator
    void operator ()(const T x0, const T x1, const T x2,const T x3)
    {
        M0= x0; M1= x1; M2= x2;M3= x3;
    }
    // gets the length of this vector
    float length(void) const;
    // dot product
    T operator *(const Vector4<T> &v) const;
    // uniform scaling
    Vector4<T> operator *(const T num) const;
    // uniform scaling
    Vector4<T> &operator *=(const T num);
    // uniform scaling
    Vector4<T> &operator /=(const T num);
    // uniform scaling
    Vector4<T> operator  /(const T num) const;
    // subtraction
    Vector4<T> &operator -=(const Vector4<T> &v);
    // subtraction
    Vector4<T> operator -(const Vector4<T> &v) const;
    // negation
    Vector4<T> operator -(void) const;
    // addition
    Vector4<T> &operator +=(const Vector4<T> &v);
    // addition
    Vector4<T> operator +(const Vector4<T> &v) const;
    // test for equality
    bool operator ==(const Vector4<T> &v) const;

    // test for inequality
    bool operator !=(const Vector4<T> &v) const;

    // non-uniform scaling
    Vector4<T> &operator *=(const Vector4<T> &v) {
        M0 *= v.M0; M1 *= v.M1; M2 *= v.M2;M3 *= v.M3;
        return *this;
    }


    // check if any elements are NAN
    bool is_nan(void) const WARN_IF_UNUSED;

    // check if any elements are infinity
    bool is_inf(void) const WARN_IF_UNUSED;

    // check if all elements are zero
    bool is_zero(void) const WARN_IF_UNUSED {
        return (fabsf(M0) < FLT_EPSILON) &&
               (fabsf(M1) < FLT_EPSILON) &&
               (fabsf(M2) < FLT_EPSILON) &&
               (fabsf(M3) < FLT_EPSILON);
    }



    // gets the length of this vector squared
    T  length_squared() const
    {
        return (T)(*this * *this);
    }



    // normalizes this vector
    void normalize()
    {
        *this /= length();
    }

    // zero the vector
    void zero()
    {
        M0 = M1 = M2 = M3 = 0;
    }

    // returns the normalized version of this vector
    Vector4<T> normalized() const
    {
        return *this/length();
    }

 


};

typedef Vector4<int16_t>                Vector4i;
typedef Vector4<uint16_t>               Vector4ui;
typedef Vector4<int32_t>                Vector4l;
typedef Vector4<uint32_t>               Vector4ul;
typedef Vector4<float>                  Vector4f;