| //##################################################################### |
| // Copyright 2002-2004, Ronald Fedkiw, Geoffrey Irving, Sergey Koltakov, Neil Molino, Igor Neverov, Joseph Teran. |
| // This file is part of PhysBAM whose distribution is governed by the license contained in the accompanying file PHYSBAM_COPYRIGHT.txt. |
| //##################################################################### |
| // Class FRAME |
| //##################################################################### |
| #ifndef __FRAME__ |
| #define __FRAME__ |
| |
| #include <iostream> |
| #include <math.h> |
| #include "VECTOR_3D.h" |
| #include "QUATERNION.h" |
| namespace PhysBAM |
| { |
| |
| template<class T> |
| class FRAME |
| { |
| public: |
| VECTOR_3D<T> t; // by default T is (0,0,0) |
| QUATERNION<T> r; // by default r is (1,0,0,0) |
| |
| FRAME() |
| {} |
| |
| FRAME (const VECTOR_3D<T>& t_input) |
| : t (t_input) |
| {} |
| |
| FRAME (const QUATERNION<T>& r_input) |
| : r (r_input) |
| {} |
| |
| FRAME (const VECTOR_3D<T>& t_input, const QUATERNION<T>& r_input) |
| : t (t_input), r (r_input) |
| {} |
| |
| bool operator== (const FRAME<T>& f) const |
| { |
| return t == f.t && r == f.r; |
| } |
| |
| bool operator!= (const FRAME<T>& f) const |
| { |
| return t != f.t || r != f.r; |
| } |
| |
| FRAME<T>& operator*= (const FRAME<T>& f) |
| { |
| t += r.Rotate (f.t); |
| r *= f.r; |
| return *this; |
| } |
| |
| FRAME<T> operator* (const FRAME<T>& f) const |
| { |
| return FRAME<T> (t + r.Rotate (f.t), r * f.r); |
| } |
| |
| FRAME<T>& operator+= (const FRAME<T>& f) |
| { |
| t += f.t; |
| r += f.r; |
| return *this; |
| } |
| |
| FRAME<T> operator+ (const FRAME<T>& f) const |
| { |
| return FRAME<T> (t + f.t, r + f.r); |
| } |
| |
| FRAME<T> operator- (const FRAME<T>& f) const |
| { |
| return FRAME<T> (t - f.t, r - f.r); |
| } |
| |
| VECTOR_3D<T> operator* (const VECTOR_3D<T>& v) const |
| { |
| return t + r.Rotate (v); |
| } |
| |
| VECTOR_3D<T> Local_Coordinate (const VECTOR_3D<T>& v) const |
| { |
| return (r.Inverse()).Rotate (v - t); |
| } |
| |
| void Invert() |
| { |
| *this = Inverse(); |
| } |
| |
| FRAME<T> Inverse() const |
| { |
| QUATERNION<T> r_inverse = r.Inverse(); |
| return FRAME<T> (-r_inverse.Rotate (t), r_inverse); |
| } |
| |
| FRAME<T> Left_Multiply_With_Position_And_Orientation (const VECTOR_3D<T>& position, const QUATERNION<T>& orientation) const |
| { |
| return FRAME<T> (position + t, orientation * r); |
| } |
| |
| void Print() const |
| { |
| std::cout << "FRAME:" << std::endl; |
| t.Print(); |
| r.Print(); |
| } |
| |
| template<class RW> |
| void Read (std::istream& input_stream) |
| { |
| t.template Read<RW> (input_stream); |
| r.template Read<RW> (input_stream); |
| } |
| |
| template<class RW> |
| void Write (std::ostream& output_stream) const |
| { |
| t.template Write<RW> (output_stream); |
| r.template Write<RW> (output_stream); |
| } |
| |
| //##################################################################### |
| }; |
| // global functions |
| template<class T> inline FRAME<T> operator* (const T a, const FRAME<T>& f) |
| { |
| return FRAME<T> (a * f.t, a * f.r); |
| } |
| |
| template<class T> inline std::istream& operator>> (std::istream& input_stream, FRAME<T>& f) |
| { |
| input_stream >> f.t >> f.r; |
| return input_stream; |
| } |
| |
| template<class T> inline std::ostream& operator<< (std::ostream& output_stream, const FRAME<T>& f) |
| { |
| output_stream << f.t << " " << f.r; |
| return output_stream; |
| } |
| } |
| #endif |
| |