| //##################################################################### |
| // Copyright 2002-2004, Robert Bridson, Doug Enright, Ronald Fedkiw, Eran Guendelman, Geoffrey Irving, Sergey Koltakov, Igor Neverov, Duc Nguyen, Joseph Teran, Rachel Weinstein. |
| // This file is part of PhysBAM whose distribution is governed by the license contained in the accompanying file PHYSBAM_COPYRIGHT.txt. |
| //##################################################################### |
| // Class VECTOR_3D |
| //##################################################################### |
| #ifndef __VECTOR_3D__ |
| #define __VECTOR_3D__ |
| |
| #include <iostream> |
| #include <assert.h> |
| #include <math.h> |
| #include "../Math_Tools/sqr.h" |
| #include "../Math_Tools/max.h" |
| #include "../Math_Tools/min.h" |
| #include "../Math_Tools/constants.h" |
| #include "VECTOR_2D.h" |
| #include "VECTOR_ND.h" |
| #include "../Read_Write/READ_WRITE_FUNCTIONS.h" |
| namespace PhysBAM |
| { |
| |
| template<class T> |
| class VECTOR_3D |
| { |
| public: |
| T x, y, z; |
| |
| VECTOR_3D() |
| : x (T()), y (T()), z (T()) |
| {} |
| |
| VECTOR_3D (const T x_input, const T y_input, const T z_input) |
| : x (x_input), y (y_input), z (z_input) |
| {} |
| |
| VECTOR_3D (const VECTOR_3D<T>& vector_input) |
| : x (vector_input.x), y (vector_input.y), z (vector_input.z) |
| {} |
| |
| VECTOR_3D (const VECTOR_ND<T>& vector_input) |
| : x (vector_input (1)), y (vector_input (2)), z (vector_input (3)) |
| { |
| assert (vector_input.n == 3); |
| } |
| |
| template<class T2> VECTOR_3D (const VECTOR_3D<T2>& vector_input) |
| : x ( (T) vector_input.x), y ( (T) vector_input.y), z ( (T) vector_input.z) |
| {} |
| |
| explicit VECTOR_3D (const VECTOR_2D<T>& vector_input) |
| : x (vector_input.x), y (vector_input.y), z (0) |
| {} |
| |
| VECTOR_2D<T> Vector_2D() const |
| { |
| return VECTOR_2D<T> (x, y); |
| } |
| |
| T operator[] (const int i) const |
| { |
| assert (1 <= i && i <= 3); |
| return * ( (const T*) (this) + i - 1); |
| } |
| |
| T& operator[] (const int i) |
| { |
| assert (1 <= i && i <= 3); |
| return * ( (T*) (this) + i - 1); |
| } |
| |
| bool operator== (const VECTOR_3D<T>& v) const |
| { |
| return x == v.x && y == v.y && z == v.z; |
| } |
| |
| bool operator!= (const VECTOR_3D<T>& v) const |
| { |
| return x != v.x || y != v.y || z != v.z; |
| } |
| |
| VECTOR_3D<T> operator-() const |
| { |
| return VECTOR_3D<T> (-x, -y, -z); |
| } |
| |
| VECTOR_3D<T>& operator+= (const VECTOR_3D<T>& v) |
| { |
| x += v.x; |
| y += v.y; |
| z += v.z; |
| return *this; |
| } |
| |
| VECTOR_3D<T>& operator-= (const VECTOR_3D<T>& v) |
| { |
| x -= v.x; |
| y -= v.y; |
| z -= v.z; |
| return *this; |
| } |
| |
| VECTOR_3D<T>& operator*= (const VECTOR_3D<T>& v) |
| { |
| x *= v.x; |
| y *= v.y; |
| z *= v.z; |
| return *this; |
| } |
| |
| VECTOR_3D<T>& operator*= (const T a) |
| { |
| x *= a; |
| y *= a; |
| z *= a; |
| return *this; |
| } |
| |
| VECTOR_3D<T>& operator/= (const T a) |
| { |
| assert (a != 0); |
| T one_over_a = 1 / a; |
| x *= one_over_a; |
| y *= one_over_a; |
| z *= one_over_a; |
| return *this; |
| } |
| |
| VECTOR_3D<T>& operator/= (const VECTOR_3D<T>& v) |
| { |
| x /= v.x; |
| y /= v.y; |
| z /= v.z; |
| return *this; |
| } |
| |
| VECTOR_3D<T> operator+ (const VECTOR_3D<T>& v) const |
| { |
| return VECTOR_3D<T> (x + v.x, y + v.y, z + v.z); |
| } |
| |
| VECTOR_3D<T> operator- (const VECTOR_3D<T>& v) const |
| { |
| return VECTOR_3D<T> (x - v.x, y - v.y, z - v.z); |
| } |
| |
| VECTOR_3D<T> operator* (const VECTOR_3D<T>& v) const |
| { |
| return VECTOR_3D<T> (x * v.x, y * v.y, z * v.z); |
| } |
| |
| VECTOR_3D<T> operator/ (const VECTOR_3D<T>& v) const |
| { |
| return VECTOR_3D<T> (x / v.x, y / v.y, z / v.z); |
| } |
| |
| VECTOR_3D<T> operator* (const T a) const |
| { |
| return VECTOR_3D<T> (a * x, a * y, a * z); |
| } |
| |
| VECTOR_3D<T> operator/ (const T a) const |
| { |
| assert (a != 0); |
| T s = 1 / a; |
| return VECTOR_3D<T> (s * x, s * y, s * z); |
| } |
| |
| VECTOR_3D<T> fabs() const |
| { |
| return VECTOR_3D<T> (fabs (x), fabs (y), fabs (z)); |
| } |
| |
| T Magnitude_Squared() const |
| { |
| return x * x + y * y + z * z; |
| } |
| |
| T Magnitude() const |
| { |
| return sqrt (x * x + y * y + z * z); |
| } |
| |
| T Lp_Norm (const T p) const |
| { |
| return pow (pow (::fabs (x), p) + pow (::fabs (y), p) + pow (::fabs (z), p), 1 / p); |
| } |
| |
| T Normalize() |
| { |
| T magnitude = Magnitude(); |
| assert (magnitude != 0); |
| T s = 1 / magnitude; |
| x *= s; |
| y *= s; |
| z *= s; |
| return magnitude; |
| } |
| |
| T Robust_Normalize (T tolerance = (T) 1e-8, const VECTOR_3D<T>& fallback = VECTOR_3D<T> (1, 0, 0)) |
| { |
| T magnitude = Magnitude(); |
| |
| if (magnitude > tolerance) |
| { |
| T s = 1 / magnitude; |
| x *= s; |
| y *= s; |
| z *= s; |
| } |
| else |
| { |
| *this = fallback; |
| } |
| |
| return magnitude; |
| } |
| |
| VECTOR_3D<T> Normalized() const |
| { |
| T magnitude = Magnitude(); |
| assert (magnitude != 0); |
| T s = 1 / magnitude; |
| return VECTOR_3D (x * s, y * s, z * s); |
| } |
| |
| VECTOR_3D<T> Robust_Normalized (T tolerance = (T) 1e-8, const VECTOR_3D<T>& fallback = VECTOR_3D<T> (1, 0, 0)) const |
| { |
| T magnitude = Magnitude(); |
| |
| if (magnitude > tolerance) return *this / magnitude; |
| else return fallback; |
| } |
| |
| VECTOR_3D<T> Orthogonal_Vector() const |
| { |
| T abs_x =::fabs ( (T) x), abs_y =::fabs ( (T) y), abs_z =::fabs ( (T) z); |
| |
| if (abs_x < abs_y) return abs_x < abs_z ? VECTOR_3D<T> (0, z, -y) : VECTOR_3D<T> (y, -x, 0); |
| else return abs_y < abs_z ? VECTOR_3D<T> (-z, 0, x) : VECTOR_3D<T> (y, -x, 0); |
| } |
| |
| T Min_Element() const |
| { |
| return min (x, y, z); |
| } |
| |
| T Max_Element() const |
| { |
| return max (x, y, z); |
| } |
| |
| T Max_Abs_Element() const |
| { |
| return maxabs (x, y, z); |
| } |
| |
| int Dominant_Axis() const |
| { |
| T abs_x =::fabs (x), abs_y =::fabs (y), abs_z =::fabs (z); |
| return (abs_x > abs_y && abs_x > abs_z) ? 1 : ( (abs_y > abs_z) ? 2 : 3); |
| } |
| |
| static T Dot_Product (const VECTOR_3D<T>& v1, const VECTOR_3D<T>& v2) |
| { |
| return v1.x * v2.x + v1.y * v2.y + v1.z * v2.z; |
| } |
| |
| VECTOR_3D<T> Projected_On_Unit_Direction (const VECTOR_3D<T>& direction) const |
| { |
| return Dot_Product (*this, direction) * direction; |
| } |
| |
| VECTOR_3D<T> Projected (const VECTOR_3D<T>& direction) const // un-normalized direction |
| { |
| return Dot_Product (*this, direction) / Dot_Product (direction, direction) * direction; |
| } |
| |
| void Project_On_Unit_Direction (const VECTOR_3D<T>& direction) |
| { |
| *this = Dot_Product (*this, direction) * direction; |
| } |
| |
| void Project (const VECTOR_3D<T>& direction) // un-normalized direction |
| { |
| *this = Dot_Product (*this, direction) / Dot_Product (direction, direction) * direction; |
| } |
| |
| VECTOR_3D<T> Projected_Orthogonal_To_Unit_Direction (const VECTOR_3D<T>& direction) const |
| { |
| T dot_product = Dot_Product (*this, direction); |
| return VECTOR_3D<T> (x - dot_product * direction.x, y - dot_product * direction.y, z - dot_product * direction.z); |
| } |
| |
| static VECTOR_3D<T> Cross_Product (const VECTOR_3D<T>& v1, const VECTOR_3D<T>& v2) |
| { |
| return VECTOR_3D<T> (v1.y * v2.z - v2.y * v1.z, -v1.x * v2.z + v2.x * v1.z, v1.x * v2.y - v2.x * v1.y); |
| } |
| |
| static T Angle_Between (const VECTOR_3D<T>& v1, const VECTOR_3D<T>& v2) |
| { |
| return acos (max (T (-1), min (T (1), Dot_Product (v1, v2) / (v1.Magnitude() * v2.Magnitude())))); |
| } |
| |
| static T Triple_Product (const VECTOR_3D<T>& u, const VECTOR_3D<T>& v, const VECTOR_3D<T>& w) |
| { |
| return Dot_Product (u, Cross_Product (v, w)); |
| } |
| |
| static T Element_Average (const VECTOR_3D<T>& a) |
| { |
| return (T) one_third * (a.x + a.y + a.z); |
| } |
| |
| static VECTOR_3D<T> Exponent (const VECTOR_3D<T>& a) |
| { |
| return VECTOR_3D<T> (exp (a.x), exp (a.y), exp (a.z)); |
| } |
| |
| static VECTOR_3D<T> Square_Root (const VECTOR_3D<T>& a) |
| { |
| return VECTOR_3D<T> (sqrt (a.x), sqrt (a.y), sqrt (a.z)); |
| } |
| |
| static VECTOR_3D<T> Square (const VECTOR_3D<T>& a) |
| { |
| return VECTOR_3D<T> (sqr (a.x), sqr (a.y), sqr (a.z)); |
| } |
| |
| static VECTOR_3D<T> Cube (const VECTOR_3D<T>& a) |
| { |
| return VECTOR_3D<T> (cube (a.x), cube (a.y), cube (a.z)); |
| } |
| |
| static VECTOR_3D<T> Reciprocal (const VECTOR_3D<T>& a) |
| { |
| return VECTOR_3D<T> (1 / a.x, 1 / a.y, 1 / a.z); |
| } |
| |
| void Print() const |
| { |
| std::cout << "VECTOR_3D: " << x << " " << y << " " << z << std::endl; |
| } |
| |
| template<class RW> |
| void Read (std::istream &input_stream) |
| { |
| Read_Binary<RW> (input_stream, x, y, z); |
| } |
| |
| template<class RW> |
| void Write (std::ostream &output_stream) const |
| { |
| Write_Binary<RW> (output_stream, x, y, z); |
| } |
| |
| //##################################################################### |
| }; |
| template<> |
| inline VECTOR_3D<int> VECTOR_3D<int>::operator/ (const int a) const |
| { |
| return VECTOR_3D<int> (x / a, y / a, z / a); |
| } |
| |
| template<> |
| inline VECTOR_3D<int>& VECTOR_3D<int>::operator/= (const int a) |
| { |
| x /= a; |
| y /= a; |
| z /= a; |
| return *this; |
| } |
| |
| template<class T> |
| inline VECTOR_3D<T> operator* (const T a, const VECTOR_3D<T>& v) |
| { |
| return VECTOR_3D<T> (a * v.x, a * v.y, a * v.z); |
| } |
| |
| template<class T> |
| inline std::istream& operator>> (std::istream& input_stream, VECTOR_3D<T>& v) |
| { |
| input_stream >> v.x >> v.y >> v.z; |
| return input_stream; |
| } |
| |
| template<class T> |
| inline std::ostream& operator<< (std::ostream& output_stream, const VECTOR_3D<T>& v) |
| { |
| output_stream << v.x << " " << v.y << " " << v.z; |
| return output_stream; |
| } |
| } |
| #endif |