diff options
Diffstat (limited to 'src/lib/mathlib/math/Vector2.hpp')
-rw-r--r-- | src/lib/mathlib/math/Vector2.hpp | 269 |
1 files changed, 269 insertions, 0 deletions
diff --git a/src/lib/mathlib/math/Vector2.hpp b/src/lib/mathlib/math/Vector2.hpp new file mode 100644 index 000000000..501740614 --- /dev/null +++ b/src/lib/mathlib/math/Vector2.hpp @@ -0,0 +1,269 @@ +/**************************************************************************** + * + * 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 Vector2.hpp + * + * 2D Vector + */ + +#ifndef VECTOR2_HPP +#define VECTOR2_HPP + +#include <math.h> + +namespace math +{ + +template <typename T> +class Vector2 { +public: + T x, y; + + /** + * trivial ctor + */ + Vector2<T>() { + } + + /** + * setting ctor + */ + Vector2<T>(const T x0, const T y0): x(x0), y(y0) { + } + + /** + * setter + */ + void set(const T x0, const T y0) { + x = x0; + y = y0; + } + + /** + * 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 Vector2<T> &v) { + return (x == v.x && y == v.y); + } + + /** + * test for inequality + */ + bool operator !=(const Vector2<T> &v) { + return (x != v.x || y != v.y); + } + + /** + * set to value + */ + const Vector2<T> &operator =(const Vector2<T> &v) { + x = v.x; + y = v.y; + return *this; + } + + /** + * negation + */ + const Vector2<T> operator -(void) const { + return Vector2<T>(-x, -y); + } + + /** + * addition + */ + const Vector2<T> operator +(const Vector2<T> &v) const { + return Vector2<T>(x + v.x, y + v.y); + } + + /** + * subtraction + */ + const Vector2<T> operator -(const Vector2<T> &v) const { + return Vector2<T>(x - v.x, y - v.y); + } + + /** + * uniform scaling + */ + const Vector2<T> operator *(const T num) const { + Vector2<T> temp(*this); + return temp *= num; + } + + /** + * uniform scaling + */ + const Vector2<T> operator /(const T num) const { + Vector2<T> temp(*this); + return temp /= num; + } + + /** + * addition + */ + const Vector2<T> &operator +=(const Vector2<T> &v) { + x += v.x; + y += v.y; + return *this; + } + + /** + * subtraction + */ + const Vector2<T> &operator -=(const Vector2<T> &v) { + x -= v.x; + y -= v.y; + return *this; + } + + /** + * uniform scaling + */ + const Vector2<T> &operator *=(const T num) { + x *= num; + y *= num; + return *this; + } + + /** + * uniform scaling + */ + const Vector2<T> &operator /=(const T num) { + x /= num; + y /= num; + return *this; + } + + /** + * dot product + */ + T operator *(const Vector2<T> &v) const { + return x * v.x + y * v.y; + } + + /** + * cross product + */ + const float operator %(const Vector2<T> &v) const { + return x * v.y - y * v.x; + } + + /** + * 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 + */ + Vector2<T> normalized() const { + return *this / length(); + } + + /** + * reflects this vector about n + */ + void reflect(const Vector2<T> &n) + { + Vector2<T> orig(*this); + project(n); + *this = *this * 2 - orig; + } + + /** + * projects this vector onto v + */ + void project(const Vector2<T> &v) { + *this = v * (*this * v) / (v * v); + } + + /** + * returns this vector projected onto v + */ + Vector2<T> projected(const Vector2<T> &v) { + return v * (*this * v) / (v * v); + } + + /** + * computes the angle between 2 arbitrary vectors + */ + static inline float angle(const Vector2<T> &v1, const Vector2<T> &v2) { + return acosf((v1 * v2) / (v1.length() * v2.length())); + } + + /** + * computes the angle between 2 arbitrary normalized vectors + */ + static inline float angle_normalized(const Vector2<T> &v1, const Vector2<T> &v2) { + return acosf(v1 * v2); + } +}; + +typedef Vector2<float> Vector2f; +} + +#endif // VECTOR2_HPP |