diff options
Diffstat (limited to 'src/lib/mathlib/math/Vector3.hpp')
| -rw-r--r-- | src/lib/mathlib/math/Vector3.hpp | 287 |
1 files changed, 0 insertions, 287 deletions
diff --git a/src/lib/mathlib/math/Vector3.hpp b/src/lib/mathlib/math/Vector3.hpp deleted file mode 100644 index a0c3b9290..000000000 --- a/src/lib/mathlib/math/Vector3.hpp +++ /dev/null @@ -1,287 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Will Perone <will.perone@gmail.com> - * Anton Babushkin <anton.babushkin@me.com> - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file Vector3.hpp - * - * 3D Vector - */ - -#ifndef VECTOR3_HPP -#define VECTOR3_HPP - -#include <math.h> -#include "../CMSIS/Include/arm_math.h" - -namespace math -{ - -template <typename T> -class Vector3 { -public: - T x, y, z; - arm_matrix_instance_f32 arm_col; - - /** - * trivial ctor - */ - Vector3<T>() { - arm_col = {3, 1, &x}; - } - - /** - * setting ctor - */ - Vector3<T>(const T x0, const T y0, const T z0): x(x0), y(y0), z(z0) { - arm_col = {3, 1, &x}; - } - - /** - * setting ctor - */ - Vector3<T>(const T data[3]): x(data[0]), y(data[1]), z(data[2]) { - arm_col = {3, 1, &x}; - } - - /** - * setter - */ - void set(const T x0, const T y0, const T z0) { - x = x0; - y = y0; - z = z0; - } - - /** - * access to elements by index - */ - T operator ()(unsigned int i) { - return *(&x + i); - } - - /** - * access to elements by index - */ - const T operator ()(unsigned int i) const { - return *(&x + i); - } - - /** - * test for equality - */ - bool operator ==(const Vector3<T> &v) { - return (x == v.x && y == v.y && z == v.z); - } - - /** - * test for inequality - */ - bool operator !=(const Vector3<T> &v) { - return (x != v.x || y != v.y || z != v.z); - } - - /** - * set to value - */ - const Vector3<T> &operator =(const Vector3<T> &v) { - x = v.x; - y = v.y; - z = v.z; - return *this; - } - - /** - * negation - */ - const Vector3<T> operator -(void) const { - return Vector3<T>(-x, -y, -z); - } - - /** - * addition - */ - const Vector3<T> operator +(const Vector3<T> &v) const { - return Vector3<T>(x + v.x, y + v.y, z + v.z); - } - - /** - * subtraction - */ - const Vector3<T> operator -(const Vector3<T> &v) const { - return Vector3<T>(x - v.x, y - v.y, z - v.z); - } - - /** - * uniform scaling - */ - const Vector3<T> operator *(const T num) const { - Vector3<T> temp(*this); - return temp *= num; - } - - /** - * uniform scaling - */ - const Vector3<T> operator /(const T num) const { - Vector3<T> temp(*this); - return temp /= num; - } - - /** - * addition - */ - const Vector3<T> &operator +=(const Vector3<T> &v) { - x += v.x; - y += v.y; - z += v.z; - return *this; - } - - /** - * subtraction - */ - const Vector3<T> &operator -=(const Vector3<T> &v) { - x -= v.x; - y -= v.y; - z -= v.z; - return *this; - } - - /** - * uniform scaling - */ - const Vector3<T> &operator *=(const T num) { - x *= num; - y *= num; - z *= num; - return *this; - } - - /** - * uniform scaling - */ - const Vector3<T> &operator /=(const T num) { - x /= num; - y /= num; - z /= num; - return *this; - } - - /** - * dot product - */ - T operator *(const Vector3<T> &v) const { - return x * v.x + y * v.y + z * v.z; - } - - /** - * cross product - */ - const Vector3<T> operator %(const Vector3<T> &v) const { - Vector3<T> temp(y * v.z - z * v.y, z * v.x - x * v.z, x * v.y - y * v.x); - return temp; - } - - /** - * gets the length of this vector squared - */ - float length_squared() const { - return (*this * *this); - } - - /** - * gets the length of this vector - */ - float length() const { - return (T)sqrt(*this * *this); - } - - /** - * normalizes this vector - */ - void normalize() { - *this /= length(); - } - - /** - * returns the normalized version of this vector - */ - Vector3<T> normalized() const { - return *this / length(); - } - - /** - * reflects this vector about n - */ - void reflect(const Vector3<T> &n) - { - Vector3<T> orig(*this); - project(n); - *this = *this * 2 - orig; - } - - /** - * projects this vector onto 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) { - return v * (*this * v) / (v * v); - } - - /** - * computes the angle between 2 arbitrary vectors - */ - static inline float angle(const Vector3<T> &v1, const Vector3<T> &v2) { - return acosf((v1 * v2) / (v1.length() * v2.length())); - } - - /** - * computes the angle between 2 arbitrary normalized vectors - */ - static inline float angle_normalized(const Vector3<T> &v1, const Vector3<T> &v2) { - return acosf(v1 * v2); - } -}; - -typedef Vector3<float> Vector3f; -} - -#endif // VECTOR3_HPP |