BALL
1.4.1
|
00001 // -*- Mode: C++; tab-width: 2; -*- 00002 // vi: set ts=2: 00003 // 00004 00005 #ifndef BALL_MATHS_VECTOR3_H 00006 #define BALL_MATHS_VECTOR3_H 00007 00008 #ifndef BALL_COMMON_EXCEPTION_H 00009 # include <BALL/COMMON/exception.h> 00010 #endif 00011 00012 #ifndef BALL_CONCEPT_PERSISTENCEMANAGER_H 00013 # include <BALL/CONCEPT/persistenceManager.h> 00014 #endif 00015 00016 #ifndef BALL_MATHS_ANGLE_H 00017 # include <BALL/MATHS/angle.h> 00018 #endif 00019 00020 #ifndef BALL_MATHS_COMMON_H 00021 # include <BALL/MATHS/common.h> 00022 #endif 00023 00024 #ifdef BALL_HAS_IEEEFP_H 00025 # include <ieeefp.h> 00026 #endif 00027 00028 00029 namespace BALL 00030 { 00036 00037 template <typename T> 00038 class TVector3; 00039 00043 00047 template <typename T> 00048 BALL_INLINE 00049 TVector3<T> operator * (const T& a, const TVector3<T>& b); 00050 00055 template <typename T> 00056 std::istream& operator >> (std::istream& s, TVector3<T>& vector); 00057 00062 template <typename T> 00063 std::ostream& operator << (std::ostream& s, const TVector3<T>& vector); 00064 00066 00069 template <typename T> 00070 class TVector3 00071 { 00072 public: 00073 00077 00082 TVector3(); 00083 00090 explicit TVector3(const T* ptr); 00091 00097 explicit TVector3(const T& value); 00098 00102 template<typename T2> 00103 explicit TVector3(const TVector3<T2>& vec); 00104 00111 TVector3(const T& vx, const T& vy, const T& vz); 00112 00117 TVector3(const TVector3& vector); 00118 00128 TVector3(const T& r, const TAngle<T>& phi, const TAngle<T>& theta); 00129 00134 ~TVector3(); 00135 00139 void clear(); 00140 00142 00146 00153 void set(const T* ptr); 00154 00159 void set(const T& value); 00160 00166 void set(const T& vx, const T& vy, const T& vz); 00167 00171 void set(const TVector3& vector); 00172 00185 void set(const T& r, const TAngle<T>& phi, const TAngle<T>& theta); 00186 00191 TVector3& operator = (const TVector3& v); 00192 00197 TVector3& operator = (T value); 00198 00204 TVector3& operator = (const T* ptr); 00205 00212 void get(T* ptr) const; 00213 00219 void get(T& x, T& y, T& z) const; 00220 00225 void get(TVector3& vector) const; 00226 00234 void get(T& r, TAngle<T>& phi, TAngle<T>& theta) const; 00235 00239 void swap(TVector3& vector); 00240 00246 T getLength() const; 00247 00253 T getSquareLength() const; 00254 00261 TVector3& normalize(); 00262 00267 TVector3& negate(); 00268 00271 static const TVector3& getZero(); 00272 00276 static const TVector3& getUnit(); 00277 00281 T& operator [] (Position position); 00282 00286 const T& operator [] (Position position) const; 00288 00292 00295 const TVector3& operator + () const; 00296 00299 TVector3 operator - () const; 00300 00303 TVector3 operator + (const TVector3& b) const; 00304 00307 TVector3 operator - (const TVector3& b) const; 00308 00314 TVector3& operator += (const TVector3& vector); 00315 00320 TVector3& operator -= (const TVector3& vector); 00321 00327 TVector3 operator * (const T& scalar) const; 00328 00334 TVector3& operator *= (const T& scalar); 00335 00342 TVector3 operator / (const T& lambda) const; 00343 00349 TVector3& operator /= (const T& lambda); 00350 00354 T operator * (const TVector3& vector) const; 00355 00359 TVector3 operator % (const TVector3& vector) const; 00360 00364 TVector3& operator %= (const TVector3& vector); 00365 00367 00371 00374 T getDistance(const TVector3& vector) const; 00375 00378 T getSquareDistance(const TVector3& vector) const; 00379 00384 TAngle<T> getAngle(const TVector3& vector) const; 00385 00389 TVector3 getOrthogonalProjection(const TVector3& direction) const; 00390 00397 static TVector3 getPerpendicularNormalization 00398 (const TVector3& a, const TVector3& b, const TVector3& c); 00399 00407 static T getTripleProduct (const TVector3<T>& a, const TVector3<T>& b, const TVector3<T>& c); 00408 00410 00414 00420 bool operator == (const TVector3& vector) const; 00421 00427 bool operator != (const TVector3& vector) const; 00428 00430 bool operator < (const TVector3& vector) const; 00431 00432 00437 bool isZero() const; 00438 00441 bool isOrthogonalTo(const TVector3& vector) const; 00442 00444 00445 00449 00452 void write(PersistenceManager& pm) const; 00453 00456 bool read(PersistenceManager& pm); 00457 00459 00463 00470 void dump(std::ostream& s = std::cout, Size depth = 0) const; 00471 00476 bool isValid() const; 00477 00479 00480 00486 00489 T x; 00490 00493 T y; 00494 00497 T z; 00499 00500 private: 00501 00502 TAngle<T> getAngle_(const T& a, const T& b) const 00503 { 00504 TAngle<T> angle; 00505 00506 if (Maths::isNotZero(a)) 00507 { 00508 angle = atan(b / a); 00509 } 00510 else 00511 { 00512 angle = BALL_SGN(b) * Constants::PI / 2; 00513 } 00514 00515 if (Maths::isLess(a, 0)) 00516 { 00517 angle += Constants::PI; 00518 } 00519 00520 if (Maths::isLess(angle.value, 0)) 00521 { 00522 return (Angle)(angle.value += 2.0 * Constants::PI); 00523 } 00524 else 00525 { 00526 return angle; 00527 } 00528 } 00529 }; 00531 00532 template <typename T> 00533 BALL_INLINE 00534 TVector3<T>::TVector3() 00535 : x(0), 00536 y(0), 00537 z(0) 00538 { 00539 } 00540 00541 template <typename T> 00542 BALL_INLINE 00543 TVector3<T>::TVector3(const T* ptr) 00544 { 00545 if (ptr == 0) 00546 { 00547 throw Exception::NullPointer(__FILE__, __LINE__); 00548 } 00549 00550 x = *ptr++; 00551 y = *ptr++; 00552 z = *ptr; 00553 } 00554 00555 template <typename T> 00556 BALL_INLINE 00557 TVector3<T>::TVector3(const T& value) 00558 : x(value), 00559 y(value), 00560 z(value) 00561 { 00562 } 00563 00564 template <typename T> template <typename T2> 00565 BALL_INLINE 00566 TVector3<T>::TVector3(const TVector3<T2>& vec) 00567 : x((T)vec.x), 00568 y((T)vec.y), 00569 z((T)vec.z) 00570 { 00571 } 00572 00573 template <typename T> 00574 BALL_INLINE 00575 TVector3<T>::TVector3(const T& vx, const T& vy, const T& vz) 00576 : x(vx), 00577 y(vy), 00578 z(vz) 00579 { 00580 } 00581 00582 template <typename T> 00583 BALL_INLINE 00584 TVector3<T>::TVector3(const TVector3& vector) 00585 : x(vector.x), 00586 y(vector.y), 00587 z(vector.z) 00588 { 00589 } 00590 00591 template <typename T> 00592 BALL_INLINE 00593 TVector3<T>::TVector3(const T& r, const TAngle<T>& phi, const TAngle<T>& theta) 00594 : x(r * cos(phi) * sin(theta)), 00595 y(r * sin(phi) * sin(theta)), 00596 z(r * cos(theta)) 00597 { 00598 } 00599 00600 template <typename T> 00601 BALL_INLINE 00602 TVector3<T>::~TVector3() 00603 { 00604 } 00605 00606 template <typename T> 00607 BALL_INLINE 00608 void TVector3<T>::clear() 00609 { 00610 x = y = z = (T)0; 00611 } 00612 00613 template <typename T> 00614 BALL_INLINE 00615 void TVector3<T>::set(const T* ptr) 00616 { 00617 if (ptr == 0) 00618 throw Exception::NullPointer(__FILE__, __LINE__); 00619 00620 x = *ptr++; 00621 y = *ptr++; 00622 z = *ptr; 00623 } 00624 00625 template <typename T> 00626 BALL_INLINE 00627 void TVector3<T>::set(const T& value) 00628 { 00629 x = value; 00630 y = value; 00631 z = value; 00632 } 00633 00634 template <typename T> 00635 BALL_INLINE 00636 void TVector3<T>::set(const T& vx, const T& vy, const T& vz) 00637 { 00638 x = vx; 00639 y = vy; 00640 z = vz; 00641 } 00642 00643 template <typename T> 00644 BALL_INLINE 00645 void TVector3<T>::set(const TVector3<T>& vector) 00646 { 00647 x = vector.x; 00648 y = vector.y; 00649 z = vector.z; 00650 } 00651 00652 template <typename T> 00653 BALL_INLINE 00654 void TVector3<T>::set(const T& r, const TAngle<T> &phi, const TAngle<T> &theta) 00655 { 00656 x = r * cos(phi) * sin(theta); 00657 y = r * sin(phi) * sin(theta); 00658 z = r * cos(theta); 00659 } 00660 00661 template <typename T> 00662 BALL_INLINE 00663 TVector3<T>& TVector3<T>::operator = (const T* ptr) 00664 { 00665 if (ptr == 0) 00666 { 00667 throw Exception::NullPointer(__FILE__, __LINE__); 00668 } 00669 00670 x = *ptr++; 00671 y = *ptr++; 00672 z = *ptr; 00673 00674 return *this; 00675 } 00676 00677 template <typename T> 00678 BALL_INLINE 00679 TVector3<T>& TVector3<T>::operator = (const TVector3<T>& vector) 00680 { 00681 x = vector.x; 00682 y = vector.y; 00683 z = vector.z; 00684 00685 return *this; 00686 } 00687 00688 template <typename T> 00689 BALL_INLINE 00690 TVector3<T>& TVector3<T>::operator = (T value) 00691 { 00692 x = y = z = value; 00693 00694 return *this; 00695 } 00696 00697 template <typename T> 00698 BALL_INLINE 00699 void TVector3<T>::get(T* ptr) const 00700 { 00701 if (ptr == 0) 00702 { 00703 throw Exception::NullPointer(__FILE__, __LINE__); 00704 } 00705 00706 *ptr++ = x; 00707 *ptr++ = y; 00708 *ptr = z; 00709 } 00710 00711 template <typename T> 00712 BALL_INLINE 00713 void TVector3<T>::get(T& new_x, T& new_y, T& new_z) const 00714 { 00715 new_x = x; 00716 new_y = y; 00717 new_z = z; 00718 } 00719 00720 template <typename T> 00721 BALL_INLINE 00722 void TVector3<T>::get(TVector3<T>& vector) const 00723 { 00724 vector.x = x; 00725 vector.y = y; 00726 vector.z = z; 00727 } 00728 00729 template <typename T> 00730 BALL_INLINE 00731 void TVector3<T>::get(T& r, TAngle<T>& phi, TAngle<T>& theta) const 00732 { 00733 r = sqrt(x * x + y * y + z * z); 00734 phi = (Angle)getAngle_(x, y); 00735 theta = getAngle_(z, sqrt(x * x + y * y)); 00736 } 00737 00738 template <typename T> 00739 BALL_INLINE 00740 void TVector3<T>::swap(TVector3<T>& vector) 00741 { 00742 T temp = x; 00743 x = vector.x; 00744 vector.x = temp; 00745 00746 temp = y; 00747 y = vector.y; 00748 vector.y = temp; 00749 00750 temp = z; 00751 z = vector.z; 00752 vector.z = temp; 00753 } 00754 00755 template <typename T> 00756 BALL_INLINE 00757 T TVector3<T>::getLength() const 00758 { 00759 return (T)sqrt(x * x + y * y + z * z); 00760 } 00761 00762 template <typename T> 00763 BALL_INLINE 00764 T TVector3<T>::getSquareLength() const 00765 { 00766 return (x * x + y * y + z * z); 00767 } 00768 00769 template <typename T> 00770 TVector3<T>& TVector3<T>::normalize() 00771 { 00772 T len = sqrt(x * x + y * y + z * z); 00773 00774 if (Maths::isZero(len)) 00775 { 00776 throw Exception::DivisionByZero(__FILE__, __LINE__); 00777 } 00778 00779 x /= len; 00780 y /= len; 00781 z /= len; 00782 00783 return *this; 00784 } 00785 00786 template <typename T> 00787 BALL_INLINE 00788 TVector3<T>& TVector3<T>::negate() 00789 { 00790 x *= -1; 00791 y *= -1; 00792 z *= -1; 00793 return *this; 00794 } 00795 00796 template <typename T> 00797 BALL_INLINE 00798 const TVector3<T>& TVector3<T>::getZero() 00799 { 00800 static TVector3<T> null_vector(0, 0, 0); 00801 return null_vector; 00802 } 00803 00804 template <typename T> 00805 BALL_INLINE 00806 const TVector3<T>& TVector3<T>::getUnit() 00807 { 00808 static TVector3<T> unit_vector(1, 1, 1); 00809 return unit_vector; 00810 } 00811 00812 template <typename T> 00813 BALL_INLINE 00814 T& TVector3<T>::operator [] (Position position) 00815 { 00816 if (position > 2) 00817 { 00818 throw Exception::IndexOverflow(__FILE__, __LINE__, position); 00819 } 00820 switch (position) 00821 { 00822 case 0: return x; 00823 case 1: return y; 00824 case 2: 00825 default: 00826 return z; 00827 } 00828 } 00829 00830 template <typename T> 00831 BALL_INLINE 00832 const T& TVector3<T>::operator [] (Position position) const 00833 { 00834 if (position > 2) 00835 { 00836 throw Exception::IndexOverflow(__FILE__, __LINE__); 00837 } 00838 switch (position) 00839 { 00840 case 0: return x; 00841 case 1: return y; 00842 case 2: 00843 default: 00844 return z; 00845 } 00846 } 00847 00848 template <typename T> 00849 BALL_INLINE 00850 const TVector3<T>& TVector3<T>::operator + () const 00851 { 00852 return *this; 00853 } 00854 00855 template <typename T> 00856 BALL_INLINE 00857 TVector3<T> TVector3<T>::operator - () const 00858 { 00859 return TVector3<T>(-x, -y, -z); 00860 } 00861 00862 template <typename T> 00863 BALL_INLINE 00864 TVector3<T>& TVector3<T>::operator += (const TVector3<T>& vector) 00865 { 00866 x += vector.x; 00867 y += vector.y; 00868 z += vector.z; 00869 00870 return *this; 00871 } 00872 00873 template <typename T> 00874 BALL_INLINE 00875 TVector3<T>& TVector3<T>::operator -= (const TVector3<T>& vector) 00876 { 00877 x -= vector.x; 00878 y -= vector.y; 00879 z -= vector.z; 00880 00881 return *this; 00882 } 00883 00884 template <typename T> 00885 BALL_INLINE 00886 TVector3<T> TVector3<T>::operator * (const T& scalar) const 00887 { 00888 return TVector3<T>(x * scalar, y * scalar, z * scalar); 00889 } 00890 00891 template <typename T> 00892 BALL_INLINE 00893 TVector3<T>& TVector3<T>::operator *= (const T &scalar) 00894 { 00895 x *= scalar; 00896 y *= scalar; 00897 z *= scalar; 00898 00899 return *this; 00900 } 00901 00902 template <typename T> 00903 TVector3<T> TVector3<T>::operator / (const T& lambda) const 00904 { 00905 if (lambda == (T)0) 00906 { 00907 throw Exception::DivisionByZero(__FILE__, __LINE__); 00908 } 00909 return TVector3<T>(x / lambda, y / lambda, z / lambda); 00910 } 00911 00912 template <typename T> 00913 TVector3<T>& TVector3<T>::operator /= (const T& lambda) 00914 { 00915 if (lambda == (T)0) 00916 { 00917 throw Exception::DivisionByZero(__FILE__, __LINE__); 00918 } 00919 x /= lambda; 00920 y /= lambda; 00921 z /= lambda; 00922 00923 return *this; 00924 } 00925 00926 template <typename T> 00927 BALL_INLINE 00928 T TVector3<T>::operator * (const TVector3<T>& vector) const 00929 { 00930 return (x * vector.x + y * vector.y + z * vector.z); 00931 } 00932 00933 template <typename T> 00934 TVector3<T> TVector3<T>::operator % (const TVector3<T>& v) const 00935 { 00936 return TVector3(y * v.z - z * v.y, z * v.x - x * v.z, x * v.y - y * v.x); 00937 } 00938 00939 template <typename T> 00940 BALL_INLINE 00941 TVector3<T>& TVector3<T>::operator %= (const TVector3<T>& v) 00942 { 00943 set(y * v.z - z * v.y, z * v.x - x * v.z, x * v.y - y * v.x); 00944 return *this; 00945 } 00946 00947 template <typename T> 00948 BALL_INLINE 00949 T TVector3<T>::getDistance(const TVector3<T>& v) const 00950 { 00951 T dx = x - v.x; 00952 T dy = y - v.y; 00953 T dz = z - v.z; 00954 00955 return (T)sqrt(dx * dx + dy * dy + dz * dz); 00956 } 00957 00958 template <typename T> 00959 BALL_INLINE T 00960 TVector3<T>::getSquareDistance(const TVector3<T>& v) const 00961 { 00962 T dx = x - v.x; 00963 T dy = y - v.y; 00964 T dz = z - v.z; 00965 00966 return (dx * dx + dy * dy + dz * dz); 00967 } 00968 00969 template <typename T> 00970 BALL_INLINE 00971 TAngle<T> TVector3<T>::getAngle(const TVector3<T>& vector) const 00972 { 00973 T length_product = getSquareLength() * vector.getSquareLength(); 00974 00975 if (length_product == (T)0) 00976 { 00977 throw Exception::DivisionByZero(__FILE__, __LINE__); 00978 } 00979 00980 T acos_arg = ((*this) * vector) / sqrt(length_product); 00981 00982 // ensure that the argument of acos is in the correct range 00983 // (might happen if the angle between the two vectors is 00984 // very close to zero) 00985 if (fabs(acos_arg) > 1.0) 00986 { 00987 return (TAngle<T>)0.0; 00988 } 00989 00990 return (TAngle<T>)acos(acos_arg); 00991 } 00992 00993 template <typename T> 00994 BALL_INLINE 00995 TVector3<T> TVector3<T>::getOrthogonalProjection(const TVector3<T>& direction) const 00996 { 00997 return ((direction * (*this)) / (direction * direction) * direction); 00998 } 00999 01000 template <typename T> 01001 TVector3<T> TVector3<T>::getPerpendicularNormalization 01002 (const TVector3<T> &a, const TVector3<T> &b, const TVector3<T> &c) 01003 { 01004 TVector3 diff1(b.x - a.x, b.y - a.y, b.z - a.z); 01005 TVector3 diff2(b.x - c.x, b.y - c.y, b.z - c.z); 01006 01007 return TVector3 01008 (diff1.y * diff2.z - diff1.z * diff2.y, 01009 diff1.z * diff2.x - diff1.x * diff2.z, 01010 diff1.x * diff2.y - diff1.y * diff2.x); 01011 } 01012 01013 template <typename T> 01014 BALL_INLINE 01015 T TVector3<T>::getTripleProduct 01016 (const TVector3<T>& a, 01017 const TVector3<T>& b, 01018 const TVector3<T>& c) 01019 { 01020 return ( a.x * (b.y * c.z - b.z * c.y) 01021 + a.y * (b.z * c.x - b.x * c.z) 01022 + a.z * (b.x * c.y - b.y * c.x)); 01023 } 01024 01025 template <typename T> 01026 BALL_INLINE 01027 bool TVector3<T>::operator == (const TVector3<T>& v) const 01028 { 01029 return (Maths::isEqual(x, v.x) && Maths::isEqual(y, v.y) && Maths::isEqual(z, v.z)); 01030 } 01031 01032 template <typename T> 01033 BALL_INLINE 01034 bool TVector3<T>::operator < (const TVector3<T>& v) const 01035 { 01036 return (x < v.x || y < v.y || z < v.z); 01037 } 01038 01039 01040 template <typename T> 01041 BALL_INLINE 01042 bool TVector3<T>::operator != (const TVector3<T>& v) const 01043 { 01044 return (Maths::isNotEqual(x, v.x) || Maths::isNotEqual(y, v.y) || Maths::isNotEqual(z, v.z)); 01045 } 01046 01047 template <typename T> 01048 BALL_INLINE 01049 bool TVector3<T>::isOrthogonalTo(const TVector3<T>& v) const 01050 { 01051 return Maths::isZero((*this) * v); 01052 } 01053 01054 template <typename T> 01055 BALL_INLINE 01056 bool TVector3<T>::isValid() const 01057 { 01058 return true; 01059 } 01060 01061 template <typename T> 01062 BALL_INLINE 01063 bool TVector3<T>::isZero() const 01064 { 01065 return (Maths::isZero(x) && Maths::isZero(y) && Maths::isZero(z)); 01066 } 01067 01068 template <typename T> 01069 void TVector3<T>::dump(std::ostream& s, Size depth) const 01070 { 01071 BALL_DUMP_STREAM_PREFIX(s); 01072 01073 BALL_DUMP_HEADER(s, this, this); 01074 01075 BALL_DUMP_DEPTH(s, depth); 01076 s << " (x = " << x << ", y = " << y << ", z = " << z << ")" << std::endl; 01077 01078 BALL_DUMP_STREAM_SUFFIX(s); 01079 } 01080 01084 typedef TVector3<float> Vector3; 01085 01086 template <typename T> 01087 BALL_INLINE 01088 TVector3<T> TVector3<T>::operator + (const TVector3<T>& b) const 01089 { 01090 return TVector3<T>(x + b.x, y + b.y, z + b.z); 01091 } 01092 01093 template <typename T> 01094 BALL_INLINE 01095 TVector3<T> TVector3<T>::operator - (const TVector3<T>& b) const 01096 { 01097 return TVector3<T>(x - b.x, y - b.y, z - b.z); 01098 } 01099 01100 template <typename T> 01101 void TVector3<T>::write(PersistenceManager& pm) const 01102 { 01103 pm.writePrimitive(x, "x"); 01104 pm.writePrimitive(y, "y"); 01105 pm.writePrimitive(z, "z"); 01106 } 01107 01108 template <typename T> 01109 bool TVector3<T>::read(PersistenceManager& pm) 01110 { 01111 pm.readPrimitive(x, "x"); 01112 pm.readPrimitive(y, "y"); 01113 pm.readPrimitive(z, "z"); 01114 01115 return true; 01116 } 01117 01118 01119 template <typename T> 01120 BALL_INLINE 01121 TVector3<T> operator * (const T& scalar, const TVector3<T>& vector) 01122 { 01123 return TVector3<T>(scalar * vector.x, scalar * vector.y, scalar * vector.z); 01124 } 01125 01126 template <typename T> 01127 std::istream& operator >> (std::istream& s, TVector3<T>& v) 01128 { 01129 char c; 01130 s >> c >> v.x >> v.y >> v.z >> c; 01131 01132 return s; 01133 } 01134 01135 template <typename T> 01136 std::ostream& operator << (std::ostream& s, const TVector3<T>& v) 01137 { 01138 s << "(" << v.x << ' ' << v.y << ' ' << v.z << ')'; 01139 01140 return s; 01141 } 01142 // required for visual studio 01143 #ifdef BALL_COMPILER_MSVC 01144 #include <vector> 01145 #ifdef BALL_HAS_EXTERN_TEMPLATES 01146 extern template class BALL_EXPORT std::vector<Vector3>; 01147 #elif 01148 template class BALL_EXPORT std::vector<Vector3>; 01149 #endif 01150 #endif 01151 01152 #ifdef BALL_HAS_EXTERN_TEMPLATES 01153 extern template class BALL_EXPORT TVector3<float>; 01154 #endif 01155 01156 }// namespace BALL 01157 01158 #endif // BALL_MATHS_VECTOR3_H