00001
00002
00003
00004
00005
00006
00007 #ifndef BALL_MATHS_VECTOR3_H
00008 #define BALL_MATHS_VECTOR3_H
00009
00010 #ifndef BALL_COMMON_EXCEPTION_H
00011 # include <BALL/COMMON/exception.h>
00012 #endif
00013
00014 #ifndef BALL_CONCEPT_PERSISTENCEMANAGER_H
00015 # include <BALL/CONCEPT/persistenceManager.h>
00016 #endif
00017
00018 #ifndef BALL_MATHS_ANGLE_H
00019 # include <BALL/MATHS/angle.h>
00020 #endif
00021
00022 #ifndef BALL_MATHS_COMMON_H
00023 # include <BALL/MATHS/common.h>
00024 #endif
00025
00026 #ifdef BALL_HAS_IEEEFP_H
00027 # include <ieeefp.h>
00028 #endif
00029
00030
00031 namespace BALL
00032 {
00038
00039 template <typename T>
00040 class TVector3;
00041
00045
00049 template <typename T>
00050 BALL_INLINE
00051 TVector3<T> operator * (const T& a, const TVector3<T>& b);
00052
00057 template <typename T>
00058 std::istream& operator >> (std::istream& s, TVector3<T>& vector);
00059
00064 template <typename T>
00065 std::ostream& operator << (std::ostream& s, const TVector3<T>& vector);
00066
00068
00071 template <typename T>
00072 class TVector3
00073 {
00074 public:
00075
00079
00084 TVector3();
00085
00092 explicit TVector3(const T* ptr)
00093 throw(Exception::NullPointer);
00094
00100 explicit TVector3(const T& value);
00101
00105 template<typename T2>
00106 explicit TVector3(const TVector3<T2>& vec);
00107
00114 TVector3(const T& vx, const T& vy, const T& vz);
00115
00120 TVector3(const TVector3& vector);
00121
00131 TVector3(const T& r, const TAngle<T>& phi, const TAngle<T>& theta);
00132
00137 ~TVector3();
00138
00142 void clear();
00143
00145
00149
00156 void set(const T* ptr)
00157 throw(Exception::NullPointer);
00158
00163 void set(const T& value);
00164
00170 void set(const T& vx, const T& vy, const T& vz);
00171
00175 void set(const TVector3& vector);
00176
00189 void set(const T& r, const TAngle<T>& phi, const TAngle<T>& theta);
00190
00195 TVector3& operator = (const TVector3& v);
00196
00201 TVector3& operator = (T value);
00202
00208 TVector3& operator = (const T* ptr)
00209 throw(Exception::NullPointer);
00210
00217 void get(T* ptr) const
00218 throw(Exception::NullPointer);
00219
00225 void get(T& x, T& y, T& z) const;
00226
00231 void get(TVector3& vector) const;
00232
00240 void get(T& r, TAngle<T>& phi, TAngle<T>& theta) const;
00241
00245 void swap(TVector3& vector);
00246
00252 T getLength() const;
00253
00259 T getSquareLength() const;
00260
00267 TVector3& normalize()
00268 throw(Exception::DivisionByZero);
00269
00274 TVector3& negate();
00275
00278 static const TVector3& getZero();
00279
00283 static const TVector3& getUnit();
00284
00288 T& operator [] (Position position)
00289 throw(Exception::IndexOverflow);
00290
00294 const T& operator [] (Position position) const
00295 throw(Exception::IndexOverflow);
00297
00301
00304 const TVector3& operator + () const;
00305
00308 TVector3 operator - () const;
00309
00312 TVector3 operator + (const TVector3& b) const;
00313
00316 TVector3 operator - (const TVector3& b) const;
00317
00323 TVector3& operator += (const TVector3& vector);
00324
00329 TVector3& operator -= (const TVector3& vector);
00330
00336 TVector3 operator * (const T& scalar) const;
00337
00343 TVector3& operator *= (const T& scalar);
00344
00351 TVector3 operator / (const T& lambda) const
00352 throw(Exception::DivisionByZero);
00353
00359 TVector3& operator /= (const T& lambda)
00360 throw(Exception::DivisionByZero);
00361
00365 T operator * (const TVector3& vector) const;
00366
00370 TVector3 operator % (const TVector3& vector) const;
00371
00375 TVector3& operator %= (const TVector3& vector);
00376
00378
00382
00385 T getDistance(const TVector3& vector) const;
00386
00389 T getSquareDistance(const TVector3& vector) const;
00390
00395 TAngle<T> getAngle(const TVector3& vector) const
00396 throw(Exception::DivisionByZero);
00397
00401 TVector3 getOrthogonalProjection(const TVector3& direction) const;
00402
00409 static TVector3 getPerpendicularNormalization
00410 (const TVector3& a, const TVector3& b, const TVector3& c);
00411
00419 static T getTripleProduct (const TVector3<T>& a, const TVector3<T>& b, const TVector3<T>& c);
00420
00422
00426
00432 bool operator == (const TVector3& vector) const;
00433
00439 bool operator != (const TVector3& vector) const;
00440
00442 bool operator < (const TVector3& vector) const;
00443
00444
00449 bool isZero() const;
00450
00453 bool isOrthogonalTo(const TVector3& vector) const;
00454
00456
00457
00461
00464 void write(PersistenceManager& pm) const;
00465
00468 bool read(PersistenceManager& pm);
00469
00471
00475
00482 void dump(std::ostream& s = std::cout, Size depth = 0) const;
00483
00488 bool isValid() const;
00489
00491
00492
00498
00501 T x;
00502
00505 T y;
00506
00509 T z;
00511
00512 private:
00513
00514 TAngle<T> getAngle_(const T& a, const T& b) const
00515 {
00516 TAngle<T> angle;
00517
00518 if (Maths::isNotZero(a))
00519 {
00520 angle = atan(b / a);
00521 }
00522 else
00523 {
00524 angle = BALL_SGN(b) * Constants::PI / 2;
00525 }
00526
00527 if (Maths::isLess(a, 0))
00528 {
00529 angle += Constants::PI;
00530 }
00531
00532 if (Maths::isLess(angle.value, 0))
00533 {
00534 return (Angle)(angle.value += 2.0 * Constants::PI);
00535 }
00536 else
00537 {
00538 return angle;
00539 }
00540 }
00541 };
00543
00544 template <typename T>
00545 BALL_INLINE
00546 TVector3<T>::TVector3()
00547 : x(0),
00548 y(0),
00549 z(0)
00550 {
00551 }
00552
00553 template <typename T>
00554 BALL_INLINE
00555 TVector3<T>::TVector3(const T* ptr)
00556 throw(Exception::NullPointer)
00557 {
00558 if (ptr == 0)
00559 {
00560 throw Exception::NullPointer(__FILE__, __LINE__);
00561 }
00562
00563 x = *ptr++;
00564 y = *ptr++;
00565 z = *ptr;
00566 }
00567
00568 template <typename T>
00569 BALL_INLINE
00570 TVector3<T>::TVector3(const T& value)
00571 : x(value),
00572 y(value),
00573 z(value)
00574 {
00575 }
00576
00577 template <typename T> template <typename T2>
00578 BALL_INLINE
00579 TVector3<T>::TVector3(const TVector3<T2>& vec)
00580 : x((T)vec.x),
00581 y((T)vec.y),
00582 z((T)vec.z)
00583 {
00584 }
00585
00586 template <typename T>
00587 BALL_INLINE
00588 TVector3<T>::TVector3(const T& vx, const T& vy, const T& vz)
00589 : x(vx),
00590 y(vy),
00591 z(vz)
00592 {
00593 }
00594
00595 template <typename T>
00596 BALL_INLINE
00597 TVector3<T>::TVector3(const TVector3& vector)
00598 : x(vector.x),
00599 y(vector.y),
00600 z(vector.z)
00601 {
00602 }
00603
00604 template <typename T>
00605 BALL_INLINE
00606 TVector3<T>::TVector3(const T& r, const TAngle<T>& phi, const TAngle<T>& theta)
00607 : x(r * cos(phi) * sin(theta)),
00608 y(r * sin(phi) * sin(theta)),
00609 z(r * cos(theta))
00610 {
00611 }
00612
00613 template <typename T>
00614 BALL_INLINE
00615 TVector3<T>::~TVector3()
00616 {
00617 }
00618
00619 template <typename T>
00620 BALL_INLINE
00621 void TVector3<T>::clear()
00622 {
00623 x = y = z = (T)0;
00624 }
00625
00626 template <typename T>
00627 BALL_INLINE
00628 void TVector3<T>::set(const T* ptr)
00629 throw(Exception::NullPointer)
00630 {
00631 if (ptr == 0)
00632 throw Exception::NullPointer(__FILE__, __LINE__);
00633
00634 x = *ptr++;
00635 y = *ptr++;
00636 z = *ptr;
00637 }
00638
00639 template <typename T>
00640 BALL_INLINE
00641 void TVector3<T>::set(const T& value)
00642 {
00643 x = value;
00644 y = value;
00645 z = value;
00646 }
00647
00648 template <typename T>
00649 BALL_INLINE
00650 void TVector3<T>::set(const T& vx, const T& vy, const T& vz)
00651 {
00652 x = vx;
00653 y = vy;
00654 z = vz;
00655 }
00656
00657 template <typename T>
00658 BALL_INLINE
00659 void TVector3<T>::set(const TVector3<T>& vector)
00660 {
00661 x = vector.x;
00662 y = vector.y;
00663 z = vector.z;
00664 }
00665
00666 template <typename T>
00667 BALL_INLINE
00668 void TVector3<T>::set(const T& r, const TAngle<T> &phi, const TAngle<T> &theta)
00669 {
00670 x = r * cos(phi) * sin(theta);
00671 y = r * sin(phi) * sin(theta);
00672 z = r * cos(theta);
00673 }
00674
00675 template <typename T>
00676 BALL_INLINE
00677 TVector3<T>& TVector3<T>::operator = (const T* ptr)
00678 throw(Exception::NullPointer)
00679 {
00680 if (ptr == 0)
00681 {
00682 throw Exception::NullPointer(__FILE__, __LINE__);
00683 }
00684
00685 x = *ptr++;
00686 y = *ptr++;
00687 z = *ptr;
00688
00689 return *this;
00690 }
00691
00692 template <typename T>
00693 BALL_INLINE
00694 TVector3<T>& TVector3<T>::operator = (const TVector3<T>& vector)
00695 {
00696 x = vector.x;
00697 y = vector.y;
00698 z = vector.z;
00699
00700 return *this;
00701 }
00702
00703 template <typename T>
00704 BALL_INLINE
00705 TVector3<T>& TVector3<T>::operator = (T value)
00706 {
00707 x = y = z = value;
00708
00709 return *this;
00710 }
00711
00712 template <typename T>
00713 BALL_INLINE
00714 void TVector3<T>::get(T* ptr) const
00715 throw(Exception::NullPointer)
00716 {
00717 if (ptr == 0)
00718 {
00719 throw Exception::NullPointer(__FILE__, __LINE__);
00720 }
00721
00722 *ptr++ = x;
00723 *ptr++ = y;
00724 *ptr = z;
00725 }
00726
00727 template <typename T>
00728 BALL_INLINE
00729 void TVector3<T>::get(T& new_x, T& new_y, T& new_z) const
00730 {
00731 new_x = x;
00732 new_y = y;
00733 new_z = z;
00734 }
00735
00736 template <typename T>
00737 BALL_INLINE
00738 void TVector3<T>::get(TVector3<T>& vector) const
00739 {
00740 vector.x = x;
00741 vector.y = y;
00742 vector.z = z;
00743 }
00744
00745 template <typename T>
00746 BALL_INLINE
00747 void TVector3<T>::get(T& r, TAngle<T>& phi, TAngle<T>& theta) const
00748 {
00749 r = sqrt(x * x + y * y + z * z);
00750 phi = (Angle)getAngle_(x, y);
00751 theta = getAngle_(z, sqrt(x * x + y * y));
00752 }
00753
00754 template <typename T>
00755 BALL_INLINE
00756 void TVector3<T>::swap(TVector3<T>& vector)
00757 {
00758 T temp = x;
00759 x = vector.x;
00760 vector.x = temp;
00761
00762 temp = y;
00763 y = vector.y;
00764 vector.y = temp;
00765
00766 temp = z;
00767 z = vector.z;
00768 vector.z = temp;
00769 }
00770
00771 template <typename T>
00772 BALL_INLINE
00773 T TVector3<T>::getLength() const
00774 {
00775 return (T)sqrt(x * x + y * y + z * z);
00776 }
00777
00778 template <typename T>
00779 BALL_INLINE
00780 T TVector3<T>::getSquareLength() const
00781 {
00782 return (x * x + y * y + z * z);
00783 }
00784
00785 template <typename T>
00786 TVector3<T>& TVector3<T>::normalize()
00787 throw(Exception::DivisionByZero)
00788 {
00789 T len = sqrt(x * x + y * y + z * z);
00790
00791 if (Maths::isZero(len))
00792 {
00793 throw Exception::DivisionByZero(__FILE__, __LINE__);
00794 }
00795
00796 x /= len;
00797 y /= len;
00798 z /= len;
00799
00800 return *this;
00801 }
00802
00803 template <typename T>
00804 BALL_INLINE
00805 TVector3<T>& TVector3<T>::negate()
00806 {
00807 x *= -1;
00808 y *= -1;
00809 z *= -1;
00810 return *this;
00811 }
00812
00813 template <typename T>
00814 BALL_INLINE
00815 const TVector3<T>& TVector3<T>::getZero()
00816 {
00817 static TVector3<T> null_vector(0, 0, 0);
00818 return null_vector;
00819 }
00820
00821 template <typename T>
00822 BALL_INLINE
00823 const TVector3<T>& TVector3<T>::getUnit()
00824 {
00825 static TVector3<T> unit_vector(1, 1, 1);
00826 return unit_vector;
00827 }
00828
00829 template <typename T>
00830 BALL_INLINE
00831 T& TVector3<T>::operator [] (Position position)
00832 throw(Exception::IndexOverflow)
00833 {
00834 if (position > 2)
00835 {
00836 throw Exception::IndexOverflow(__FILE__, __LINE__, position);
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 T& TVector3<T>::operator [] (Position position) const
00851 throw(Exception::IndexOverflow)
00852 {
00853 if (position > 2)
00854 {
00855 throw Exception::IndexOverflow(__FILE__, __LINE__);
00856 }
00857 switch (position)
00858 {
00859 case 0: return x;
00860 case 1: return y;
00861 case 2:
00862 default:
00863 return z;
00864 }
00865 }
00866
00867 template <typename T>
00868 BALL_INLINE
00869 const TVector3<T>& TVector3<T>::operator + () const
00870 {
00871 return *this;
00872 }
00873
00874 template <typename T>
00875 BALL_INLINE
00876 TVector3<T> TVector3<T>::operator - () const
00877 {
00878 return TVector3<T>(-x, -y, -z);
00879 }
00880
00881 template <typename T>
00882 BALL_INLINE
00883 TVector3<T>& TVector3<T>::operator += (const TVector3<T>& vector)
00884 {
00885 x += vector.x;
00886 y += vector.y;
00887 z += vector.z;
00888
00889 return *this;
00890 }
00891
00892 template <typename T>
00893 BALL_INLINE
00894 TVector3<T>& TVector3<T>::operator -= (const TVector3<T>& vector)
00895 {
00896 x -= vector.x;
00897 y -= vector.y;
00898 z -= vector.z;
00899
00900 return *this;
00901 }
00902
00903 template <typename T>
00904 BALL_INLINE
00905 TVector3<T> TVector3<T>::operator * (const T& scalar) const
00906 {
00907 return TVector3<T>(x * scalar, y * scalar, z * scalar);
00908 }
00909
00910 template <typename T>
00911 BALL_INLINE
00912 TVector3<T>& TVector3<T>::operator *= (const T &scalar)
00913 {
00914 x *= scalar;
00915 y *= scalar;
00916 z *= scalar;
00917
00918 return *this;
00919 }
00920
00921 template <typename T>
00922 TVector3<T> TVector3<T>::operator / (const T& lambda) const
00923 throw(Exception::DivisionByZero)
00924 {
00925 if (lambda == (T)0)
00926 {
00927 throw Exception::DivisionByZero(__FILE__, __LINE__);
00928 }
00929 return TVector3<T>(x / lambda, y / lambda, z / lambda);
00930 }
00931
00932 template <typename T>
00933 TVector3<T>& TVector3<T>::operator /= (const T& lambda)
00934 throw(Exception::DivisionByZero)
00935 {
00936 if (lambda == (T)0)
00937 {
00938 throw Exception::DivisionByZero(__FILE__, __LINE__);
00939 }
00940 x /= lambda;
00941 y /= lambda;
00942 z /= lambda;
00943
00944 return *this;
00945 }
00946
00947 template <typename T>
00948 BALL_INLINE
00949 T TVector3<T>::operator * (const TVector3<T>& vector) const
00950 {
00951 return (x * vector.x + y * vector.y + z * vector.z);
00952 }
00953
00954 template <typename T>
00955 TVector3<T> TVector3<T>::operator % (const TVector3<T>& v) const
00956 {
00957 return TVector3(y * v.z - z * v.y, z * v.x - x * v.z, x * v.y - y * v.x);
00958 }
00959
00960 template <typename T>
00961 BALL_INLINE
00962 TVector3<T>& TVector3<T>::operator %= (const TVector3<T>& v)
00963 {
00964 set(y * v.z - z * v.y, z * v.x - x * v.z, x * v.y - y * v.x);
00965 return *this;
00966 }
00967
00968 template <typename T>
00969 BALL_INLINE
00970 T TVector3<T>::getDistance(const TVector3<T>& v) const
00971 {
00972 T dx = x - v.x;
00973 T dy = y - v.y;
00974 T dz = z - v.z;
00975
00976 return (T)sqrt(dx * dx + dy * dy + dz * dz);
00977 }
00978
00979 template <typename T>
00980 BALL_INLINE T
00981 TVector3<T>::getSquareDistance(const TVector3<T>& v) const
00982 {
00983 T dx = x - v.x;
00984 T dy = y - v.y;
00985 T dz = z - v.z;
00986
00987 return (dx * dx + dy * dy + dz * dz);
00988 }
00989
00990 template <typename T>
00991 BALL_INLINE
00992 TAngle<T> TVector3<T>::getAngle(const TVector3<T>& vector) const
00993 throw(Exception::DivisionByZero)
00994 {
00995 T length_product = getSquareLength() * vector.getSquareLength();
00996
00997 if (length_product == (T)0)
00998 {
00999 throw Exception::DivisionByZero(__FILE__, __LINE__);
01000 }
01001
01002 T acos_arg = ((*this) * vector) / sqrt(length_product);
01003
01004
01005
01006
01007 if (fabs(acos_arg) > 1.0)
01008 {
01009 return (TAngle<T>)0.0;
01010 }
01011
01012 return (TAngle<T>)acos(acos_arg);
01013 }
01014
01015 template <typename T>
01016 BALL_INLINE
01017 TVector3<T> TVector3<T>::getOrthogonalProjection(const TVector3<T>& direction) const
01018 {
01019 return ((direction * (*this)) / (direction * direction) * direction);
01020 }
01021
01022 template <typename T>
01023 TVector3<T> TVector3<T>::getPerpendicularNormalization
01024 (const TVector3<T> &a, const TVector3<T> &b, const TVector3<T> &c)
01025 {
01026 TVector3 diff1(b.x - a.x, b.y - a.y, b.z - a.z);
01027 TVector3 diff2(b.x - c.x, b.y - c.y, b.z - c.z);
01028
01029 return TVector3
01030 (diff1.y * diff2.z - diff1.z * diff2.y,
01031 diff1.z * diff2.x - diff1.x * diff2.z,
01032 diff1.x * diff2.y - diff1.y * diff2.x);
01033 }
01034
01035 template <typename T>
01036 BALL_INLINE
01037 T TVector3<T>::getTripleProduct
01038 (const TVector3<T>& a,
01039 const TVector3<T>& b,
01040 const TVector3<T>& c)
01041 {
01042 return ( a.x * (b.y * c.z - b.z * c.y)
01043 + a.y * (b.z * c.x - b.x * c.z)
01044 + a.z * (b.x * c.y - b.y * c.x));
01045 }
01046
01047 template <typename T>
01048 BALL_INLINE
01049 bool TVector3<T>::operator == (const TVector3<T>& v) const
01050 {
01051 return (Maths::isEqual(x, v.x) && Maths::isEqual(y, v.y) && Maths::isEqual(z, v.z));
01052 }
01053
01054 template <typename T>
01055 BALL_INLINE
01056 bool TVector3<T>::operator < (const TVector3<T>& v) const
01057 {
01058 return (x < v.x || y < v.y || z < v.z);
01059 }
01060
01061
01062 template <typename T>
01063 BALL_INLINE
01064 bool TVector3<T>::operator != (const TVector3<T>& v) const
01065 {
01066 return (Maths::isNotEqual(x, v.x) || Maths::isNotEqual(y, v.y) || Maths::isNotEqual(z, v.z));
01067 }
01068
01069 template <typename T>
01070 BALL_INLINE
01071 bool TVector3<T>::isOrthogonalTo(const TVector3<T>& v) const
01072 {
01073 return Maths::isZero((*this) * v);
01074 }
01075
01076 template <typename T>
01077 BALL_INLINE
01078 bool TVector3<T>::isValid() const
01079 {
01080 return true;
01081 }
01082
01083 template <typename T>
01084 BALL_INLINE
01085 bool TVector3<T>::isZero() const
01086 {
01087 return (Maths::isZero(x) && Maths::isZero(y) && Maths::isZero(z));
01088 }
01089
01090 template <typename T>
01091 void TVector3<T>::dump(std::ostream& s, Size depth) const
01092 {
01093 BALL_DUMP_STREAM_PREFIX(s);
01094
01095 BALL_DUMP_HEADER(s, this, this);
01096
01097 BALL_DUMP_DEPTH(s, depth);
01098 s << " (x = " << x << ", y = " << y << ", z = " << z << ")" << std::endl;
01099
01100 BALL_DUMP_STREAM_SUFFIX(s);
01101 }
01102
01106 typedef TVector3<float> Vector3;
01107
01108 template <typename T>
01109 BALL_INLINE
01110 TVector3<T> TVector3<T>::operator + (const TVector3<T>& b) const
01111 {
01112 return TVector3<T>(x + b.x, y + b.y, z + b.z);
01113 }
01114
01115 template <typename T>
01116 BALL_INLINE
01117 TVector3<T> TVector3<T>::operator - (const TVector3<T>& b) const
01118 {
01119 return TVector3<T>(x - b.x, y - b.y, z - b.z);
01120 }
01121
01122 template <typename T>
01123 void TVector3<T>::write(PersistenceManager& pm) const
01124 {
01125 pm.writePrimitive(x, "x");
01126 pm.writePrimitive(y, "y");
01127 pm.writePrimitive(z, "z");
01128 }
01129
01130 template <typename T>
01131 bool TVector3<T>::read(PersistenceManager& pm)
01132 {
01133 pm.readPrimitive(x, "x");
01134 pm.readPrimitive(y, "y");
01135 pm.readPrimitive(z, "z");
01136
01137 return true;
01138 }
01139
01140
01141 template <typename T>
01142 BALL_INLINE
01143 TVector3<T> operator * (const T& scalar, const TVector3<T>& vector)
01144 {
01145 return TVector3<T>(scalar * vector.x, scalar * vector.y, scalar * vector.z);
01146 }
01147
01148 template <typename T>
01149 std::istream& operator >> (std::istream& s, TVector3<T>& v)
01150 {
01151 char c;
01152 s >> c >> v.x >> v.y >> v.z >> c;
01153
01154 return s;
01155 }
01156
01157 template <typename T>
01158 std::ostream& operator << (std::ostream& s, const TVector3<T>& v)
01159 {
01160 s << "(" << v.x << ' ' << v.y << ' ' << v.z << ')';
01161
01162 return s;
01163 }
01164
01165 #ifdef BALL_COMPILER_MSVC
01166 #include <vector>
01167 template class BALL_EXPORT std::vector<Vector3>;
01168 #endif
01169
01170 }
01171
01172 #endif // BALL_MATHS_VECTOR3_H
