Geometry.h
Go to the documentation of this file.
00001 /*
00002  * Copyright (C) 2006-2011, SRI International (R)
00003  *
00004  * This program is free software: you can redistribute it and/or modify
00005  * it under the terms of the GNU Lesser General Public License as published by
00006  * the Free Software Foundation, either version 3 of the License, or
00007  * (at your option) any later version.
00008  *
00009  * This program is distributed in the hope that it will be useful,
00010  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00011  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00012  * GNU Lesser General Public License for more details.
00013  *
00014  * You should have received a copy of the GNU Lesser General Public License
00015  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
00016  */
00017 
00018 #pragma once
00019 
00020 #ifndef __OpenKarto_Geometry_h__
00021 #define __OpenKarto_Geometry_h__
00022 
00023 #include <OpenKarto/List.h>
00024 #include <OpenKarto/StringHelper.h>
00025 #include <OpenKarto/Math.h>
00026 
00027 #include <string.h>
00028 
00029 namespace karto
00030 {
00031 
00035 
00037 
00038 
00042   template<typename T>
00043   class Size2
00044   {
00045   public:
00049     Size2()
00050       : m_Width(0)
00051       , m_Height(0)
00052     {
00053     }
00054 
00060     Size2(T width, T height)
00061       : m_Width(width)
00062       , m_Height(height)
00063     {
00064     }
00065 
00069     Size2(const Size2& rOther)
00070       : m_Width(rOther.m_Width)
00071       , m_Height(rOther.m_Height)
00072     {
00073     }
00074 
00075   public:
00080     inline const T GetWidth() const
00081     {
00082       return m_Width;
00083     }
00084 
00089     inline void SetWidth(T width)
00090     {
00091       m_Width = width;
00092     }
00093 
00098     inline const T GetHeight() const
00099     {
00100       return m_Height;
00101     }
00102 
00107     inline void SetHeight(T height)
00108     {
00109       m_Height = height;
00110     }
00111 
00116     inline const karto::String ToString() const
00117     {
00118       String valueString;
00119       valueString.Append(StringHelper::ToString(GetWidth()));
00120       valueString.Append(" ");
00121       valueString.Append(StringHelper::ToString(GetHeight()));
00122       return valueString;
00123     }
00124 
00125   public:
00129     inline Size2& operator=(const Size2& rOther)
00130     {
00131       m_Width = rOther.m_Width;
00132       m_Height = rOther.m_Height;
00133 
00134       return(*this);
00135     }
00136 
00140     inline kt_bool operator==(const Size2& rOther) const
00141     {
00142       return (m_Width == rOther.m_Width && m_Height == rOther.m_Height);
00143     }
00144 
00148     inline kt_bool operator!=(const Size2& rOther) const
00149     {
00150       return (m_Width != rOther.m_Width || m_Height != rOther.m_Height);
00151     }
00152 
00156     friend KARTO_FORCEINLINE std::ostream& operator << (std::ostream& rStream, const Size2& rSize)
00157     {
00158       rStream << rSize.ToString().ToCString();
00159       return rStream;
00160     }
00161 
00162   private:
00163     T m_Width;
00164     T m_Height;
00165   }; // class Size2<T>
00166 
00170 
00174   template<typename T>
00175   class Size3
00176   {
00177   public:
00181     inline Size3()
00182       : m_Width(0.0)
00183       , m_Height(0.0)
00184       , m_Depth(0.0)
00185     {
00186     }
00187 
00194     inline Size3(T width, T height, T depth)
00195       : m_Width(width)
00196       , m_Height(height)
00197       , m_Depth(depth)
00198     {
00199     }           
00200 
00204     inline Size3(const Size3& rOther)
00205       : m_Width(rOther.m_Width)
00206       , m_Height(rOther.m_Height)
00207     {
00208     }
00209 
00210   public:
00215     inline T GetWidth() const
00216     {
00217       return m_Width;
00218     }
00219     
00224     inline void SetWidth(T width)
00225     {
00226       m_Width = width;
00227     }
00228 
00233     inline T GetHeight() const
00234     {
00235       return m_Height;
00236     }
00237     
00242     inline void SetHeight(T height)
00243     {
00244       m_Height = height;
00245     }
00246 
00251     inline T GetDepth() const
00252     {
00253       return m_Depth;
00254     }
00255 
00260     inline void SetDepth(T depth)
00261     {
00262       m_Depth = depth;
00263     }
00264 
00269     inline const karto::String ToString() const
00270     {
00271       String valueString;
00272       valueString.Append(StringHelper::ToString(GetWidth()));
00273       valueString.Append(" ");
00274       valueString.Append(StringHelper::ToString(GetHeight()));
00275       valueString.Append(" ");
00276       valueString.Append(StringHelper::ToString(GetDepth()));
00277       return valueString;
00278     }
00279 
00280   public:
00284     inline Size3& operator=(const Size3& rOther)
00285     {
00286       m_Width = rOther.m_Width;
00287       m_Height = rOther.m_Height;
00288       m_Depth = rOther.m_Depth;
00289 
00290       return *this;
00291     }
00292 
00296     kt_bool operator==(const Size3& rOther) const 
00297     {
00298       return m_Width == rOther.m_Width && m_Height == rOther.m_Height && m_Depth == rOther.m_Depth; 
00299     }
00300     
00304     kt_bool operator!=(const Size3& rOther) const 
00305     {
00306       return m_Width != rOther.m_Width || m_Height != rOther.m_Height || m_Depth != rOther.m_Depth; 
00307     }
00308 
00309   private:
00310     T m_Width;
00311     T m_Height;
00312     T m_Depth;
00313   }; // class Size3
00314 
00318 
00322   template<typename T>
00323   class Vector2
00324   {
00325   public:
00329     Vector2()
00330     {
00331       m_Values[0] = 0;
00332       m_Values[1] = 0;
00333     }
00334     
00340     Vector2(T x, T y)
00341     {
00342       m_Values[0] = x;
00343       m_Values[1] = y;
00344     }
00345     
00346   public:
00351     inline const T& GetX() const
00352     {
00353       return m_Values[0];
00354     }
00355     
00360     inline void SetX(const T& x)
00361     {
00362       m_Values[0] = x;
00363     }
00364     
00369     inline const T& GetY() const
00370     {
00371       return m_Values[1];
00372     }
00373     
00378     inline void SetY(const T& y)
00379     {
00380       m_Values[1] = y;
00381     }
00382         
00387     inline void MakeFloor(const Vector2& rOther)
00388     {
00389       if (rOther.m_Values[0] < m_Values[0]) m_Values[0] = rOther.m_Values[0];
00390       if (rOther.m_Values[1] < m_Values[1]) m_Values[1] = rOther.m_Values[1];
00391     }
00392     
00397     inline void MakeCeil(const Vector2& rOther)
00398     {
00399       if (rOther.m_Values[0] > m_Values[0])
00400       {
00401         m_Values[0] = rOther.m_Values[0];
00402       }
00403       
00404       if (rOther.m_Values[1] > m_Values[1])
00405       {
00406         m_Values[1] = rOther.m_Values[1];
00407       }
00408     }
00409     
00414     inline kt_double SquaredLength() const
00415     {
00416       return math::Square(m_Values[0]) + math::Square(m_Values[1]);
00417     }
00418     
00423     inline kt_double Length() const
00424     {
00425       return sqrt(SquaredLength());
00426     }
00427     
00433     inline kt_double SquaredDistance(const Vector2& rOther) const
00434     {
00435       return (*this - rOther).SquaredLength();
00436     }
00437     
00443     inline kt_double Distance(const Vector2& rOther) const
00444     {
00445       return sqrt(SquaredDistance(rOther));
00446     }
00447     
00452     inline const String ToString() const
00453     {
00454       String valueString;
00455       valueString.Append(StringHelper::ToString(GetX()));
00456       valueString.Append(" ");
00457       valueString.Append(StringHelper::ToString(GetY()));
00458       return valueString;
00459     }
00460 
00461   public:
00465     inline void operator+=(const Vector2& rOther)
00466     {
00467       m_Values[0] += rOther.m_Values[0];
00468       m_Values[1] += rOther.m_Values[1];
00469     }
00470     
00474     inline void operator-=(const Vector2& rOther)
00475     {
00476       m_Values[0] -= rOther.m_Values[0];
00477       m_Values[1] -= rOther.m_Values[1];
00478     }
00479 
00483     inline const Vector2 operator+(const Vector2& rOther) const
00484     {
00485       return Vector2(m_Values[0] + rOther.m_Values[0], m_Values[1] + rOther.m_Values[1]);
00486     }
00487     
00491     inline const Vector2 operator-(const Vector2& rOther) const
00492     {
00493       return Vector2(m_Values[0] - rOther.m_Values[0], m_Values[1] - rOther.m_Values[1]);
00494     }
00495     
00499     inline void operator/=(T scalar)
00500     {
00501       m_Values[0] /= scalar;
00502       m_Values[1] /= scalar;
00503     }
00504     
00508     inline const Vector2 operator/(T scalar) const
00509     {
00510       return Vector2(m_Values[0] / scalar, m_Values[1] / scalar);
00511     }
00512         
00516     inline kt_double operator*(const Vector2& rOther) const
00517     {
00518       return m_Values[0] * rOther.m_Values[0] + m_Values[1] * rOther.m_Values[1];
00519     }    
00520     
00524     inline const Vector2 operator*(T scalar) const
00525     {
00526       return Vector2(m_Values[0] * scalar, m_Values[1] * scalar);
00527     }
00528     
00532     inline const Vector2 operator-(T scalar) const
00533     {
00534       return Vector2(m_Values[0] - scalar, m_Values[1] - scalar);
00535     }
00536 
00540     inline void operator*=(T scalar)
00541     {
00542       m_Values[0] *= scalar;
00543       m_Values[1] *= scalar;
00544     }
00545     
00549     inline kt_bool operator==(const Vector2& rOther) const
00550     {
00551       return (m_Values[0] == rOther.m_Values[0] && m_Values[1] == rOther.m_Values[1]);
00552     }
00553     
00557     inline kt_bool operator!=(const Vector2& rOther) const
00558     {
00559       return (m_Values[0] != rOther.m_Values[0] || m_Values[1] != rOther.m_Values[1]);
00560     }
00561     
00568     inline kt_bool operator<(const Vector2& rOther) const
00569     {
00570       if (m_Values[0] < rOther.m_Values[0])
00571       {
00572         return true;
00573       }
00574       else if (m_Values[0] > rOther.m_Values[0])
00575       {
00576         return false;
00577       }
00578       else 
00579       {
00580         return (m_Values[1] < rOther.m_Values[1]);
00581       }
00582     }
00583 
00587     friend KARTO_FORCEINLINE std::ostream& operator<<(std::ostream& rStream, const Vector2& rVector)
00588     {
00589       rStream << rVector.ToString().ToCString();
00590       return rStream;
00591     }
00592     
00593   private:
00594     T m_Values[2];
00595   }; // class Vector2<T>
00596 
00600   typedef Vector2<kt_int32s> Vector2i;
00601 
00605   typedef Vector2<kt_int32u> Vector2iu;
00606 
00610   typedef Vector2<kt_double> Vector2d;
00611   
00615 
00619   template<typename T>
00620   class Vector3
00621   {
00622   public:
00626     Vector3()
00627     {
00628       m_Values[0] = 0;
00629       m_Values[1] = 0;
00630       m_Values[2] = 0;
00631     }
00632 
00639     Vector3(T x, T y, T z)
00640     {
00641       m_Values[0] = x;
00642       m_Values[1] = y;
00643       m_Values[2] = z;
00644     }
00645 
00650     Vector3(const Vector2<T>& rVector)
00651     {
00652       m_Values[0] = rVector.GetX();
00653       m_Values[1] = rVector.GetY();
00654       m_Values[2] = 0.0;
00655     }
00656 
00660     Vector3(const Vector3& rOther)
00661     {
00662       m_Values[0] = rOther.m_Values[0];
00663       m_Values[1] = rOther.m_Values[1];
00664       m_Values[2] = rOther.m_Values[2];
00665     }
00666 
00667   public:
00672     inline const T& GetX() const 
00673     {
00674       return m_Values[0]; 
00675     }
00676 
00681     inline void SetX(const T& x)
00682     {
00683       m_Values[0] = x; 
00684     }
00685 
00690     inline const T& GetY() const 
00691     {
00692       return m_Values[1]; 
00693     }
00694 
00699     inline void SetY(const T& y)
00700     {
00701       m_Values[1] = y; 
00702     }
00703 
00708     inline const T& GetZ() const
00709     {
00710       return m_Values[2]; 
00711     }
00712 
00717     inline void SetZ(const T& z)
00718     {
00719       m_Values[2] = z; 
00720     } 
00721 
00726     inline Vector2<T> GetAsVector2() const
00727     {
00728       return Vector2<T>(m_Values[0], m_Values[1]);
00729     }
00730 
00735     inline void MakeFloor(const Vector3& rOther)
00736     {
00737       if (rOther.m_Values[0] < m_Values[0]) m_Values[0] = rOther.m_Values[0];
00738       if (rOther.m_Values[1] < m_Values[1]) m_Values[1] = rOther.m_Values[1];
00739       if (rOther.m_Values[2] < m_Values[2]) m_Values[2] = rOther.m_Values[2];
00740     }
00741 
00746     inline void MakeCeil(const Vector3& rOther)
00747     {
00748       if (rOther.m_Values[0] > m_Values[0]) m_Values[0] = rOther.m_Values[0];
00749       if (rOther.m_Values[1] > m_Values[1]) m_Values[1] = rOther.m_Values[1];
00750       if (rOther.m_Values[2] > m_Values[2]) m_Values[2] = rOther.m_Values[2];
00751     }
00752 
00757     inline kt_double SquaredLength() const
00758     {
00759       return math::Square(m_Values[0]) + math::Square(m_Values[1]) + math::Square(m_Values[2]);
00760     }
00761 
00766     inline kt_double Length() const
00767     {
00768       return sqrt(SquaredLength());
00769     }
00770     
00774     inline void Normalize()
00775     {
00776       kt_double length = Length();
00777       if (length > 0.0)
00778       {
00779         kt_double inversedLength = 1.0 / length;
00780 
00781         m_Values[0] *= inversedLength;
00782         m_Values[1] *= inversedLength;
00783         m_Values[2] *= inversedLength;
00784       }
00785     }
00786 
00791     inline const String ToString() const
00792     {
00793       String valueString;
00794       valueString.Append(StringHelper::ToString(GetX()));
00795       valueString.Append(" ");
00796       valueString.Append(StringHelper::ToString(GetY()));
00797       valueString.Append(" ");
00798       valueString.Append(StringHelper::ToString(GetZ()));
00799       return valueString;
00800     }
00801 
00802   public:
00806     inline Vector3& operator=(const Vector3& rOther)
00807     {
00808       m_Values[0] = rOther.m_Values[0];
00809       m_Values[1] = rOther.m_Values[1];
00810       m_Values[2] = rOther.m_Values[2];
00811 
00812       return *this;
00813     }
00814 
00818     inline const Vector3 operator+(const Vector3& rOther) const
00819     {
00820       return Vector3(m_Values[0] + rOther.m_Values[0], m_Values[1] + rOther.m_Values[1], m_Values[2] + rOther.m_Values[2]);
00821     }
00822 
00826     inline const Vector3 operator+(kt_double scalar) const
00827     {
00828       return Vector3(m_Values[0] + scalar, m_Values[1] + scalar, m_Values[2] + scalar);
00829     }
00830 
00834     inline const Vector3 operator-(const Vector3& rOther) const
00835     {
00836       return Vector3(m_Values[0] - rOther.m_Values[0], m_Values[1] - rOther.m_Values[1], m_Values[2] - rOther.m_Values[2]);
00837     }
00838 
00842     inline const Vector3 operator-(kt_double scalar) const
00843     {
00844       return Vector3(m_Values[0] - scalar, m_Values[1] - scalar, m_Values[2] - scalar);
00845     }
00846 
00850     inline const Vector3 operator*(T scalar) const
00851     {
00852       return Vector3(m_Values[0] * scalar, m_Values[1] * scalar, m_Values[2] * scalar);
00853     }
00854 
00858     inline Vector3& operator+=(const Vector3& rOther)
00859     {
00860       m_Values[0] += rOther.m_Values[0];
00861       m_Values[1] += rOther.m_Values[1];
00862       m_Values[2] += rOther.m_Values[2];
00863 
00864       return *this;
00865     }
00866 
00870     inline Vector3& operator-=(const Vector3& rOther)
00871     {
00872       m_Values[0] -= rOther.m_Values[0];
00873       m_Values[1] -= rOther.m_Values[1];
00874       m_Values[2] -= rOther.m_Values[2];
00875 
00876       return *this;
00877     }
00878 
00884     inline Vector3& operator*=(const Vector3& rOther)
00885     {
00886       m_Values[0] *= rOther.m_Values[0];
00887       m_Values[1] *= rOther.m_Values[1];
00888       m_Values[2] *= rOther.m_Values[2];
00889 
00890       return *this;
00891     }
00892 
00898     inline Vector3& operator/=(const Vector3& rOther)
00899     {
00900       m_Values[0] /= rOther.m_Values[0];
00901       m_Values[1] /= rOther.m_Values[1];
00902       m_Values[2] /= rOther.m_Values[2];
00903 
00904       return *this;
00905     }
00906 
00912     inline Vector3& operator+=(const T& rValue)
00913     {
00914       m_Values[0] += rValue;
00915       m_Values[1] += rValue;
00916       m_Values[2] += rValue;
00917 
00918       return *this;
00919     }
00920 
00926     inline Vector3& operator-=(const T& rValue)
00927     {
00928       m_Values[0] -= rValue;
00929       m_Values[1] -= rValue;
00930       m_Values[2] -= rValue;
00931 
00932       return *this;
00933     }
00934 
00938     inline Vector3& operator*=(const T& rValue)
00939     {
00940       m_Values[0] *= rValue;
00941       m_Values[1] *= rValue;
00942       m_Values[2] *= rValue;
00943 
00944       return *this;
00945     }
00946 
00952     inline Vector3& operator/=(const T& rValue)
00953     {
00954       m_Values[0] /= rValue;
00955       m_Values[1] /= rValue;
00956       m_Values[2] /= rValue;
00957 
00958       return *this;
00959     }
00960 
00966     inline const Vector3 operator^(const Vector3& rOther) const
00967     {
00968       return Vector3(
00969         m_Values[1] * rOther.m_Values[2] - m_Values[2] * rOther.m_Values[1],
00970         m_Values[2] * rOther.m_Values[0] - m_Values[0] * rOther.m_Values[2] ,
00971         m_Values[0] * rOther.m_Values[1] - m_Values[1] * rOther.m_Values[0]);
00972     }
00973 
00980     inline kt_bool operator<(const Vector3& rOther) const
00981     {
00982       if (m_Values[0] < rOther.m_Values[0])
00983       {
00984         return true;
00985       }
00986       else if (m_Values[0] > rOther.m_Values[0]) 
00987       {
00988         return false;
00989       }
00990       else if (m_Values[1] < rOther.m_Values[1])
00991       {
00992         return true;
00993       }
00994       else if (m_Values[1] > rOther.m_Values[1]) 
00995       {
00996         return false;
00997       }
00998       else
00999       {
01000         return (m_Values[2] < rOther.m_Values[2]);
01001       }
01002     }
01003 
01007     inline kt_bool operator==(const Vector3& rOther) const
01008     {
01009       return (m_Values[0] == rOther.m_Values[0] && m_Values[1] == rOther.m_Values[1] && m_Values[2] == rOther.m_Values[2]);
01010     }
01011 
01015     inline kt_bool operator!=(const Vector3& rOther) const
01016     {
01017       return (m_Values[0] != rOther.m_Values[0] || m_Values[1] != rOther.m_Values[1] || m_Values[2] != rOther.m_Values[2]);
01018     }
01019 
01023     friend KARTO_FORCEINLINE std::ostream& operator << (std::ostream& rStream, const Vector3& rVector)
01024     {
01025       rStream << rVector.ToString().ToCString();
01026       return rStream;
01027     }
01028 
01029   private:
01030     T m_Values[3];
01031   }; // class Vector3<T>
01032 
01036   typedef Vector3<kt_int32s> Vector3i;
01037 
01041   typedef Vector3<kt_int32u> Vector3iu;
01042 
01046   typedef Vector3<kt_double> Vector3d;
01047 
01051 
01055   template<typename T>
01056   class Vector4
01057   {
01058   public:
01062     Vector4()
01063     {
01064       m_Values[0] = 0;
01065       m_Values[1] = 0;
01066       m_Values[2] = 0;
01067       m_Values[3] = 0;
01068     }
01069 
01077     Vector4(T x, T y, T z, T w)
01078     {
01079       m_Values[0] = x;
01080       m_Values[1] = y;
01081       m_Values[2] = z;
01082       m_Values[3] = w;
01083     }
01084 
01088     Vector4(const Vector4& rOther)
01089     {
01090       m_Values[0] = rOther.m_Values[0];
01091       m_Values[1] = rOther.m_Values[1];
01092       m_Values[2] = rOther.m_Values[2];
01093       m_Values[3] = rOther.m_Values[3];
01094     }
01095 
01096   public:
01101     inline const T& GetX() const 
01102     {
01103       return m_Values[0]; 
01104     }
01105 
01110     inline void SetX(const T& x)
01111     {
01112       m_Values[0] = x; 
01113     }
01114 
01119     inline const T& GetY() const 
01120     {
01121       return m_Values[1]; 
01122     }
01123 
01128     inline void SetY(const T& y)
01129     {
01130       m_Values[1] = y;
01131     }
01132 
01137     inline const T& GetZ() const
01138     {
01139       return m_Values[2];
01140     }
01141 
01146     inline void SetZ(const T& z)
01147     {
01148       m_Values[2] = z;
01149     } 
01150 
01155     inline const T& GetW() const
01156     {
01157       return m_Values[3];
01158     }
01159 
01164     inline void SetW(const T& w)
01165     {
01166       m_Values[3] = w;
01167     } 
01168 
01173     inline const String ToString() const
01174     {
01175       String valueString;
01176       valueString.Append(StringHelper::ToString(GetX()));
01177       valueString.Append(" ");
01178       valueString.Append(StringHelper::ToString(GetY()));
01179       valueString.Append(" ");
01180       valueString.Append(StringHelper::ToString(GetZ()));
01181       valueString.Append(" ");
01182       valueString.Append(StringHelper::ToString(GetW()));
01183       return valueString;
01184     }
01185 
01186   public:
01190     inline Vector4& operator = (const Vector4& rOther)
01191     {
01192       m_Values[0] = rOther.m_Values[0];
01193       m_Values[1] = rOther.m_Values[1];
01194       m_Values[2] = rOther.m_Values[2];
01195       m_Values[3] = rOther.m_Values[3];
01196 
01197       return *this;
01198     }
01199 
01206     inline kt_bool operator<(const Vector4& rOther) const
01207     {
01208       if (m_Values[0] < rOther.m_Values[0])
01209       {
01210         return true;
01211       }
01212       else if (m_Values[0] > rOther.m_Values[0]) 
01213       {
01214         return false;
01215       }
01216       else if (m_Values[1] < rOther.m_Values[1])
01217       {
01218         return true;
01219       }
01220       else if (m_Values[1] > rOther.m_Values[1]) 
01221       {
01222         return false;
01223       }
01224       else if (m_Values[2] < rOther.m_Values[2])
01225       {
01226         return true;
01227       }
01228       else if (m_Values[2] > rOther.m_Values[2])
01229       {
01230         return false;
01231       }
01232       else
01233       {
01234         return (m_Values[3] < rOther.m_Values[3]);
01235       }
01236     }
01237 
01241     inline kt_bool operator==(const Vector4& rOther) const
01242     {
01243       return (m_Values[0] == rOther.m_Values[0] && m_Values[1] == rOther.m_Values[1] && m_Values[2] == rOther.m_Values[2] && m_Values[3] == rOther.m_Values[3]);
01244     }
01245 
01249     inline kt_bool operator!=(const Vector4& rOther) const
01250     {
01251       return (m_Values[0] != rOther.m_Values[0] || m_Values[1] != rOther.m_Values[1] || m_Values[2] != rOther.m_Values[2] || m_Values[3] != rOther.m_Values[3]);
01252     }
01253 
01257     friend KARTO_FORCEINLINE std::ostream& operator<<(std::ostream& rStream, const Vector4& rVector)
01258     {
01259       rStream << rVector.ToString().ToCString();
01260       return rStream;
01261     }
01262 
01263   private:
01264     T m_Values[4];
01265   }; // class Vector4<T>
01266 
01270   typedef Vector4<kt_int32s> Vector4i;
01271 
01275   typedef Vector4<kt_int32u> Vector4iu;
01276 
01280   typedef Vector4<kt_double> Vector4d;
01281 
01285 
01307   class KARTO_EXPORT Quaternion
01308   {
01309   public:
01313     inline Quaternion()
01314     {
01315       m_Values[0] = 0.0;
01316       m_Values[1] = 0.0;
01317       m_Values[2] = 0.0;
01318       m_Values[3] = 1.0; 
01319     }
01320 
01328     inline Quaternion(kt_double x, kt_double y, kt_double z, kt_double w)
01329     {
01330       m_Values[0] = x;
01331       m_Values[1] = y;
01332       m_Values[2] = z;
01333       m_Values[3] = w;
01334     }
01335 
01339     inline Quaternion(const Vector4d& rVector)
01340     {
01341       m_Values[0] = rVector.GetX();
01342       m_Values[1] = rVector.GetY();
01343       m_Values[2] = rVector.GetZ();
01344       m_Values[3] = rVector.GetW();
01345     }
01346 
01350     inline Quaternion(const Quaternion& rOther)
01351     {
01352       m_Values[0] = rOther.m_Values[0];
01353       m_Values[1] = rOther.m_Values[1];
01354       m_Values[2] = rOther.m_Values[2];
01355       m_Values[3] = rOther.m_Values[3];
01356     }
01357 
01358   public:
01363     inline kt_double GetX() const
01364     {
01365       return m_Values[0]; 
01366     }
01367 
01372     inline void SetX(kt_double x)
01373     {
01374       m_Values[0] = x; 
01375     }
01376 
01381     inline kt_double GetY() const
01382     {
01383       return m_Values[1]; 
01384     }
01385 
01390     inline void SetY(kt_double y)
01391     {
01392       m_Values[1] = y; 
01393     }
01394 
01399     inline kt_double GetZ() const
01400     {
01401       return m_Values[2]; 
01402     }
01403 
01408     inline void SetZ(kt_double z)
01409     {
01410       m_Values[2] = z; 
01411     }
01412 
01417     inline kt_double GetW() const
01418     {
01419       return m_Values[3]; 
01420     }
01421 
01426     inline void SetW(kt_double w)
01427     {
01428       m_Values[3] = w; 
01429     }
01430 
01435     inline const Vector4d GetAsVector4() const 
01436     {
01437       return Vector4d(m_Values[0], m_Values[1], m_Values[2], m_Values[3]);
01438     }
01439 
01445     void ToAngleAxis(kt_double& rAngle, karto::Vector3d& rAxis) const;
01446 
01450     void FromAngleAxis(kt_double angleInRadians, const karto::Vector3d& rAxis);
01451 
01459     void ToEulerAngles(kt_double& rYaw, kt_double& rPitch, kt_double& rRoll) const;
01460 
01468     void FromEulerAngles(kt_double yaw, kt_double pitch, kt_double roll);
01469 
01474     inline kt_double Length() const
01475     {
01476       return sqrt(Norm());
01477     }
01478 
01483     inline kt_double Norm() const
01484     {
01485       return m_Values[0]*m_Values[0] + m_Values[1]*m_Values[1] + m_Values[2]*m_Values[2] + m_Values[3]*m_Values[3];
01486     }
01487 
01491     inline void Normalize()
01492     {
01493       kt_double length = Length();
01494       if (length > 0.0)
01495       {
01496         kt_double inversedLength = 1.0 / length;
01497 
01498         m_Values[0] *= inversedLength;
01499         m_Values[1] *= inversedLength;
01500         m_Values[2] *= inversedLength;
01501         m_Values[3] *= inversedLength;
01502       }
01503     }
01504 
01509     const String ToString() const;
01510 
01511   public:
01515     inline Quaternion& operator=(const Quaternion& rOther)
01516     {
01517       m_Values[0] = rOther.m_Values[0];
01518       m_Values[1] = rOther.m_Values[1];
01519       m_Values[2] = rOther.m_Values[2];
01520       m_Values[3] = rOther.m_Values[3];
01521 
01522       return(*this);
01523     }
01524 
01528     inline const Quaternion operator*(const Quaternion& rOther) const
01529     {
01530       return Quaternion(
01531         rOther.m_Values[3] * m_Values[0] + rOther.m_Values[0] * m_Values[3] + rOther.m_Values[1] * m_Values[2] - rOther.m_Values[2] * m_Values[1],
01532         rOther.m_Values[3] * m_Values[1] - rOther.m_Values[0] * m_Values[2] + rOther.m_Values[1] * m_Values[3] + rOther.m_Values[2] * m_Values[0],
01533         rOther.m_Values[3] * m_Values[2] + rOther.m_Values[0] * m_Values[1] - rOther.m_Values[1] * m_Values[0] + rOther.m_Values[2] * m_Values[3],
01534         rOther.m_Values[3] * m_Values[3] - rOther.m_Values[0] * m_Values[0] - rOther.m_Values[1] * m_Values[1] - rOther.m_Values[2] * m_Values[2] );
01535     }
01536     
01540     inline kt_bool operator==(const Quaternion& rOther) const
01541     {
01542       return (m_Values[0] == rOther.m_Values[0] && m_Values[1] == rOther.m_Values[1] && m_Values[2] == rOther.m_Values[2] && m_Values[3] == rOther.m_Values[3]);
01543     }
01544 
01548     inline kt_bool operator!=(const Quaternion& rOther) const
01549     {
01550       return (m_Values[0] != rOther.m_Values[0] || m_Values[1] != rOther.m_Values[1] || m_Values[2] != rOther.m_Values[2] || m_Values[3] != rOther.m_Values[3]);
01551     }
01552 
01558     karto::Vector3d operator*(const karto::Vector3d& rVector) const
01559     {
01560       // nVidia SDK implementation
01561       karto::Vector3d uv, uuv; 
01562       karto::Vector3d qvec(m_Values[0], m_Values[1], m_Values[2]);
01563 
01564       uv = qvec ^ rVector;
01565       uuv = qvec ^ uv; 
01566 
01567       uv *= ( 2.0 * m_Values[3] ); 
01568       uuv *= 2.0; 
01569 
01570       return rVector + uv + uuv;
01571     }
01572 
01576     friend KARTO_FORCEINLINE std::ostream& operator<<(std::ostream& rStream, const Quaternion& rQuaternion)
01577     {
01578       rStream << rQuaternion.ToString().ToCString();
01579       return rStream;
01580     }
01581 
01582   private:
01583     kt_double m_Values[4];
01584   }; // class Quaternion
01585 
01589 
01593   class KARTO_EXPORT BoundingBox2
01594   {
01595   public:
01596     /*
01597      * Bounding box of maximal size
01598      */
01599     BoundingBox2();
01600 
01606     BoundingBox2(const Vector2d& rMinimum, const Vector2d& rMaximum);
01607 
01608   public:
01613     inline const Vector2d& GetMinimum() const
01614     { 
01615       return m_Minimum; 
01616     }
01617 
01622     inline void SetMinimum(const Vector2d& rMinimum)
01623     {
01624       m_Minimum = rMinimum;
01625     }
01626 
01631     inline const Vector2d& GetMaximum() const
01632     { 
01633       return m_Maximum;
01634     }
01635 
01640     inline void SetMaximum(const Vector2d& rMaximum)
01641     {
01642       m_Maximum = rMaximum;
01643     }
01644 
01649     inline Size2<kt_double> GetSize() const
01650     {
01651       Vector2d size = m_Maximum - m_Minimum;
01652 
01653       return Size2<kt_double>(size.GetX(), size.GetY());
01654     }
01655 
01660     inline void Add(const Vector2d& rPoint)
01661     {
01662       m_Minimum.MakeFloor(rPoint);
01663       m_Maximum.MakeCeil(rPoint);
01664     }
01665 
01670     inline void Add(const BoundingBox2& rBoundingBox)
01671     {
01672       Add(rBoundingBox.GetMinimum());
01673       Add(rBoundingBox.GetMaximum());
01674     }
01675     
01681     inline kt_bool Contains(const Vector2d& rPoint) const
01682     {
01683       return (math::InRange(rPoint.GetX(), m_Minimum.GetX(), m_Maximum.GetX()) &&
01684               math::InRange(rPoint.GetY(), m_Minimum.GetY(), m_Maximum.GetY()));
01685     }
01686 
01692     inline kt_bool Intersects(const BoundingBox2& rOther) const
01693     { 
01694       if ((m_Maximum.GetX() < rOther.m_Minimum.GetX()) || (m_Minimum.GetX() > rOther.m_Maximum.GetX()))
01695       {
01696         return false;
01697       }
01698       
01699       if ((m_Maximum.GetY() < rOther.m_Minimum.GetY()) || (m_Minimum.GetY() > rOther.m_Maximum.GetY()))
01700       {
01701         return false;
01702       }
01703 
01704       return true;
01705     }
01706 
01712     inline kt_bool Contains(const BoundingBox2& rOther) const
01713     {
01714       if ((m_Maximum.GetX() < rOther.m_Minimum.GetX()) || (m_Minimum.GetX() > rOther.m_Maximum.GetX()))
01715       {
01716         return false;
01717       }
01718       
01719       if ((m_Maximum.GetY() < rOther.m_Minimum.GetY()) || (m_Minimum.GetY() > rOther.m_Maximum.GetY()))
01720       {
01721         return false;
01722       }
01723       
01724       if ((m_Minimum.GetX() <= rOther.m_Minimum.GetX()) && (rOther.m_Maximum.GetX() <= m_Maximum.GetX()) && 
01725           (m_Minimum.GetY() <= rOther.m_Minimum.GetY()) && (rOther.m_Maximum.GetY() <= m_Maximum.GetY()))
01726       {
01727         return true;
01728       }
01729 
01730       return false;
01731     }
01732 
01733   private:
01734     Vector2d m_Minimum;
01735     Vector2d m_Maximum;
01736   }; // class BoundingBox2
01737 
01741   class KARTO_EXPORT BoundingBox3
01742   {
01743   public:
01744     BoundingBox3();
01745     virtual ~BoundingBox3();
01746 
01747   public:
01752     inline const Vector3d& GetMinimum() const
01753     { 
01754       return m_Minimum; 
01755     }
01756 
01761     inline void SetMinimum(const Vector3d& rMinimum)
01762     {
01763       m_Minimum = rMinimum;
01764     }
01765 
01770     inline const Vector3d& GetMaximum() const
01771     { 
01772       return m_Maximum;
01773     }
01774 
01779     inline void SetMaximum(const Vector3d& rMaximum)
01780     {
01781       m_Maximum = rMaximum;
01782     }
01783 
01784   private:
01785     Vector3d m_Minimum;
01786     Vector3d m_Maximum;
01787   };
01788 
01792   
01797   template<typename T>
01798   class Rectangle2
01799   {
01800   public:
01804     Rectangle2()
01805     {
01806     }
01807 
01815     Rectangle2(T x, T y, T width, T height)
01816       : m_Position(x, y)
01817       , m_Size(width, height)
01818     {
01819     }
01820 
01826     Rectangle2(const Vector2<T>& rPosition, const Size2<T>& rSize)
01827       : m_Position(rPosition)
01828       , m_Size(rSize)
01829     {
01830     }
01831 
01837     Rectangle2(const Vector2<T>& rTopLeft, const Vector2<T>& rBottomRight)
01838       : m_Position(rTopLeft)
01839       , m_Size(rBottomRight.GetX() - rTopLeft.GetX(), rBottomRight.GetY() - rTopLeft.GetY())
01840     {
01841     }
01842 
01846     Rectangle2(const Rectangle2& rOther)
01847       : m_Position(rOther.m_Position)
01848       , m_Size(rOther.m_Size)
01849     {
01850     }
01851 
01852   public:
01857     inline T GetX() const
01858     {
01859       return m_Position.GetX();
01860     }
01861     
01866     inline void SetX(const T& rX) 
01867     {
01868       m_Position.SetX(rX);
01869     }
01870 
01875     inline T GetY() const
01876     {
01877       return m_Position.GetY();
01878     }
01879 
01884     inline void SetY(const T& rY)
01885     {
01886       m_Position.SetY(rY);
01887     }
01888     
01893     inline T GetWidth() const
01894     {
01895       return m_Size.GetWidth();
01896     }
01897 
01902     inline void SetWidth(const T& rWidth)
01903     {
01904       m_Size.SetWidth(rWidth);
01905     }
01906     
01911     inline T GetHeight() const
01912     {
01913       return m_Size.GetHeight();
01914     }
01915 
01920     inline void SetHeight(const T& rHeight)
01921     {
01922       m_Size.SetHeight(rHeight);
01923     }
01924     
01929     inline const Vector2<T>& GetPosition() const
01930     {
01931       return m_Position;
01932     }
01933 
01939     inline void SetPosition(const T& rX, const T& rY)
01940     {
01941       m_Position = Vector2<T>(rX, rY);
01942     }
01943 
01948     inline void SetPosition(const Vector2<T>& rPosition)
01949     {
01950       m_Position = rPosition;
01951     }
01952 
01957     inline const Size2<T>& GetSize() const
01958     {
01959       return m_Size;
01960     }
01961 
01966     inline void SetSize(const Size2<T>& rSize) 
01967     {
01968       m_Size = rSize;
01969     }
01970 
01975     inline T GetLeft() const
01976     {
01977       return m_Position.GetX();
01978     }
01979 
01984     inline void SetLeft(const T& rLeft)
01985     {
01986       m_Position.SetX(rLeft);
01987     }
01988 
01993     inline T GetTop() const
01994     {
01995       return m_Position.GetY();
01996     }
01997 
02002     inline void SetTop(const T& rTop)
02003     {
02004       m_Position.SetY(rTop);
02005     }
02006 
02011     inline T GetRight() const
02012     {
02013       return m_Position.GetX() + m_Size.GetWidth();
02014     }
02015 
02020     inline void SetRight(const T& rRight)
02021     {
02022       m_Size.SetWidth(rRight - m_Position.GetX());
02023     }
02024 
02029     inline T GetBottom() const
02030     {
02031       return m_Position.GetY() + m_Size.GetHeight();
02032     }
02033 
02038     inline void SetBottom(const T& rBottom)
02039     {
02040       m_Size.SetHeight(rBottom - m_Position.GetY());
02041     }
02042 
02047     inline Vector2<T> GetTopLeft()
02048     {
02049       return Vector2<T>(GetLeft(), GetTop());
02050     }
02051 
02056     inline Vector2<T> GetTopRight()
02057     {
02058       return Vector2<T>(GetRight(), GetTop());
02059     }
02060 
02065     inline Vector2<T> GetBottomLeft()
02066     {
02067       return Vector2<T>(GetLeft(), GetBottom());
02068     }
02069 
02074     inline Vector2<T> GetBottomRight()
02075     {
02076       return Vector2<T>(GetRight(), GetBottom());
02077     }
02078 
02083     inline const Vector2<T> GetCenter() const
02084     {
02085       return Vector2<T>(m_Position.GetX() + m_Size.GetWidth() * 0.5, m_Position.GetY() + m_Size.GetHeight() * 0.5);
02086     }
02087 
02093     inline kt_bool Contains(const Rectangle2<T>& rOther)
02094     {
02095       T l1 = m_Position.GetX();
02096       T r1 = m_Position.GetX();
02097       if (m_Size.GetWidth() < 0)
02098         l1 += m_Size.GetWidth();
02099       else
02100         r1 += m_Size.GetWidth();
02101       if (l1 == r1) // null rect
02102         return false;
02103       
02104       T l2 = rOther.m_Position.GetX();
02105       T r2 = rOther.m_Position.GetX();
02106       if (rOther.m_Size.GetWidth() < 0)
02107         l2 += rOther.m_Size.GetWidth();
02108       else
02109         r2 += rOther.m_Size.GetWidth();
02110       if (l2 == r2) // null rect
02111         return false;
02112 
02113       if (l2 < l1 || r2 > r1)
02114         return false;
02115 
02116       T t1 = m_Position.GetY();
02117       T b1 = m_Position.GetY();
02118       if (m_Size.GetHeight() < 0)
02119         t1 += m_Size.GetHeight();
02120       else
02121         b1 += m_Size.GetHeight();
02122       if (t1 == b1) // null rect
02123         return false;
02124 
02125       T t2 = rOther.m_Position.GetY();
02126       T b2 = rOther.m_Position.GetY();
02127       if (rOther.m_Size.GetHeight() < 0)
02128         t2 += rOther.m_Size.GetHeight();
02129       else
02130         b2 += rOther.m_Size.GetHeight();
02131       if (t2 == b2) // null rect
02132         return false;
02133 
02134       if (t2 < t1 || b2 > b1)
02135         return false;
02136 
02137       return true;
02138     }
02139 
02140   public:
02144     Rectangle2& operator = (const Rectangle2& rOther)
02145     {
02146       m_Position = rOther.m_Position;
02147       m_Size = rOther.m_Size;
02148       
02149       return *this;
02150     }
02151 
02155     inline kt_bool operator == (const Rectangle2& rOther) const
02156     {
02157       return (m_Position == rOther.m_Position && m_Size == rOther.m_Size);
02158     }
02159 
02163     inline kt_bool operator != (const Rectangle2& rOther) const
02164     {
02165       return (m_Position != rOther.m_Position || m_Size != rOther.m_Size);
02166     }
02167 
02168   private:
02169     Vector2<T> m_Position;
02170     Size2<T> m_Size;
02171   }; // class Rectangle2<T>
02172 
02176 
02177   class Pose3;
02178 
02182   class KARTO_EXPORT Pose2
02183   {
02184   public:
02188     Pose2();
02189 
02195     Pose2(const Vector2d& rPosition, kt_double heading = 0);
02196 
02203     Pose2(kt_double x, kt_double y, kt_double heading);
02204 
02208     Pose2(const Pose3& rPose);
02209 
02213     Pose2(const Pose2& rOther);
02214 
02215   public:
02220     inline kt_double GetX() const
02221     {
02222       return m_Position.GetX();
02223     }
02224 
02229     inline void SetX(kt_double x)
02230     {
02231       m_Position.SetX(x);
02232     }
02233 
02238     inline kt_double GetY() const
02239     {
02240       return m_Position.GetY();
02241     }
02242 
02247     inline void SetY(kt_double y)
02248     {
02249       m_Position.SetY(y);
02250     }
02251 
02256     inline const Vector2d& GetPosition() const
02257     {
02258       return m_Position;
02259     }
02260 
02265     inline void SetPosition(const Vector2d& rPosition)
02266     {
02267       m_Position = rPosition;
02268     }
02269 
02274     inline kt_double GetHeading() const
02275     {
02276       return m_Heading;
02277     }
02278 
02283     inline void SetHeading(kt_double heading)
02284     {
02285       m_Heading = heading;
02286     }
02287 
02293     inline kt_double SquaredDistance(const Pose2& rOther) const
02294     {
02295       return m_Position.SquaredDistance(rOther.m_Position);
02296     }
02297     
02303     inline kt_double AngleTo(const Vector2d& rVector) const
02304     {
02305       kt_double angle = atan2(rVector.GetY() - GetY(), rVector.GetX() - GetX());
02306       return karto::math::NormalizeAngle(angle - GetHeading());
02307     }
02308     
02314     inline const String ToString(kt_int32u precision = 4) const
02315     {
02316       String valueString;
02317       valueString.Append(StringHelper::ToString(m_Position.GetX(), precision));
02318       valueString.Append(" ");
02319       valueString.Append(StringHelper::ToString(m_Position.GetY(), precision));
02320       valueString.Append(" ");
02321       valueString.Append(StringHelper::ToString(m_Heading, precision));
02322       return valueString;
02323     }
02324 
02325   public:
02329     inline Pose2& operator = (const Pose2& rOther)
02330     {
02331       m_Position = rOther.m_Position;
02332       m_Heading = rOther.m_Heading;
02333 
02334       return *this;
02335     }
02336 
02340     inline kt_bool operator==(const Pose2& rOther) const
02341     {
02342       return (m_Position == rOther.m_Position && m_Heading == rOther.m_Heading);
02343     }
02344 
02348     inline kt_bool operator!=(const Pose2& rOther) const
02349     {
02350       return (m_Position != rOther.m_Position || m_Heading != rOther.m_Heading);
02351     }
02352 
02356     inline void operator+=(const Pose2& rOther)
02357     {
02358       m_Position += rOther.m_Position;
02359       m_Heading = math::NormalizeAngle(m_Heading + rOther.m_Heading);
02360     }
02361 
02365     inline Pose2 operator+(const Pose2& rOther) const
02366     {
02367       return Pose2(m_Position + rOther.m_Position, math::NormalizeAngle(m_Heading + rOther.m_Heading));
02368     }
02369 
02373     inline Pose2 operator-(const Pose2& rOther) const
02374     {
02375       return Pose2(m_Position - rOther.m_Position, math::NormalizeAngle(m_Heading - rOther.m_Heading));
02376     }
02377 
02381     friend KARTO_FORCEINLINE std::ostream& operator << (std::ostream& rStream, const Pose2& rPose)
02382     {
02383       rStream << rPose.ToString();
02384       return rStream;
02385     }
02386     
02387   private:
02388     Vector2d m_Position;
02389 
02390     kt_double m_Heading;
02391   }; // class Pose2
02392 
02396   typedef List<Pose2> Pose2List;
02397 
02401 
02409   class Pose3
02410   {
02411   public:
02415     Pose3()
02416     {
02417     }
02418 
02423     Pose3(const Vector3d& rPosition)
02424       : m_Position(rPosition)
02425     {
02426     }
02427 
02433     Pose3(const Vector3d& rPosition, const karto::Quaternion& rOrientation)
02434       : m_Position(rPosition)
02435       , m_Orientation(rOrientation)
02436     {
02437     }
02438 
02442     Pose3(const Pose3& rOther)
02443       : m_Position(rOther.m_Position)
02444       , m_Orientation(rOther.m_Orientation)
02445     {
02446     }
02447 
02452     Pose3(const Pose2& rPose)
02453     {
02454       m_Position = Vector3d(rPose.GetX(), rPose.GetY(), 0.0);
02455       m_Orientation.FromEulerAngles(rPose.GetHeading(), 0.0, 0.0);
02456     }
02457 
02458   public:
02463     inline const Vector3d& GetPosition() const
02464     {
02465       return m_Position;
02466     }
02467 
02472     inline void SetPosition(const Vector3d& rPosition)
02473     {
02474       m_Position = rPosition;
02475     }
02476 
02481     inline const Quaternion& GetOrientation() const
02482     {
02483       return m_Orientation;
02484     }
02485 
02490     inline void SetOrientation(const Quaternion& rOrientation)
02491     {
02492       m_Orientation = rOrientation;
02493     }
02494 
02499     inline const String ToString() const
02500     {
02501       String valueString;
02502       valueString.Append(GetPosition().ToString());
02503       valueString.Append(" ");
02504       valueString.Append(GetOrientation().ToString());
02505       return valueString;
02506     }
02507     
02508   public:
02512     inline Pose3& operator = (const Pose3& rOther)
02513     {
02514       m_Position = rOther.m_Position;
02515       m_Orientation = rOther.m_Orientation;
02516 
02517       return *this;
02518     }
02519 
02523     inline kt_bool operator == (const Pose3& rOther) const
02524     {
02525       return (m_Position == rOther.m_Position && m_Orientation == rOther.m_Orientation);
02526     }
02527 
02531     inline kt_bool operator != (const Pose3& rOther) const
02532     {
02533       return (m_Position != rOther.m_Position || m_Orientation != rOther.m_Orientation);
02534     }
02535 
02539     friend KARTO_FORCEINLINE std::ostream& operator << (std::ostream& rStream, const Pose3& rPose)
02540     {
02541       rStream << rPose.ToString();
02542       return rStream;
02543     }
02544 
02545   private:
02546     Vector3d m_Position;
02547     Quaternion m_Orientation;
02548   }; // class Pose3
02549 
02553   
02557   class Matrix3
02558   {
02559   public:
02563     Matrix3()
02564     {
02565       Clear();
02566     }
02567 
02571     inline Matrix3(const Matrix3& rOther)
02572     {
02573       memcpy(m_Matrix, rOther.m_Matrix, 9*sizeof(kt_double));
02574     }
02575 
02576   public:
02580     void SetToIdentity()
02581     {
02582       memset(m_Matrix, 0, 9*sizeof(kt_double));
02583 
02584       for (kt_int32s i = 0; i < 3; i++)
02585       {
02586         m_Matrix[i][i] = 1.0;
02587       }
02588     }
02589 
02593     void Clear()
02594     {
02595       memset(m_Matrix, 0, 9*sizeof(kt_double));
02596     }
02597 
02605     void FromAxisAngle(kt_double x, kt_double y, kt_double z, const kt_double radians)
02606     {
02607       kt_double cosRadians = cos(radians);
02608       kt_double sinRadians = sin(radians);
02609       kt_double oneMinusCos = 1.0 - cosRadians;
02610 
02611       kt_double xx = x * x;
02612       kt_double yy = y * y;
02613       kt_double zz = z * z;
02614 
02615       kt_double xyMCos = x * y * oneMinusCos;
02616       kt_double xzMCos = x * z * oneMinusCos;
02617       kt_double yzMCos = y * z * oneMinusCos;
02618 
02619       kt_double xSin = x * sinRadians;
02620       kt_double ySin = y * sinRadians;
02621       kt_double zSin = z * sinRadians;
02622 
02623       m_Matrix[0][0] = xx * oneMinusCos + cosRadians;
02624       m_Matrix[0][1] = xyMCos - zSin;
02625       m_Matrix[0][2] = xzMCos + ySin;
02626 
02627       m_Matrix[1][0] = xyMCos + zSin;
02628       m_Matrix[1][1] = yy * oneMinusCos + cosRadians;
02629       m_Matrix[1][2] = yzMCos - xSin;
02630 
02631       m_Matrix[2][0] = xzMCos - ySin;
02632       m_Matrix[2][1] = yzMCos + xSin;
02633       m_Matrix[2][2] = zz * oneMinusCos + cosRadians;
02634     }
02635 
02640     Matrix3 Transpose() const
02641     {
02642       Matrix3 transpose;
02643 
02644       for (kt_int32u row = 0; row < 3; row++)
02645       {
02646         for (kt_int32u col = 0; col < 3; col++)
02647         {
02648           transpose.m_Matrix[row][col] = m_Matrix[col][row];
02649         }
02650       }
02651 
02652       return transpose;
02653     }
02654 
02659     Matrix3 Inverse() const
02660     {
02661       Matrix3 kInverse = *this;
02662       kt_bool haveInverse = InverseFast(kInverse, 1e-14);
02663       if (haveInverse == false)
02664       {
02665         assert(false);
02666       }
02667       return kInverse;
02668     }
02669     
02674     inline const String ToString() const
02675     {
02676       String valueString;
02677 
02678       for (int row = 0; row < 3; row++)
02679       {
02680         for (int col = 0; col < 3; col++)
02681         {
02682           valueString.Append(StringHelper::ToString(m_Matrix[row][col]));
02683           valueString.Append(" ");
02684         }
02685       }
02686       
02687       return valueString;
02688     }
02689 
02690   public:
02694     inline Matrix3& operator = (const Matrix3& rOther)
02695     {
02696       memcpy(m_Matrix, rOther.m_Matrix, 9*sizeof(kt_double));
02697       return *this;
02698     }
02699 
02706     inline kt_double& operator()(kt_int32u row, kt_int32u column)
02707     {
02708       return m_Matrix[row][column];
02709     }
02710 
02717     inline kt_double operator()(kt_int32u row, kt_int32u column) const
02718     {
02719       return m_Matrix[row][column];
02720     }
02721 
02725     Matrix3 operator*(const Matrix3& rOther) const
02726     {
02727       Matrix3 product;
02728 
02729       for (size_t row = 0; row < 3; row++)
02730       {
02731         for (size_t col = 0; col < 3; col++)
02732         {
02733           product.m_Matrix[row][col] = m_Matrix[row][0]*rOther.m_Matrix[0][col] + m_Matrix[row][1]*rOther.m_Matrix[1][col] + m_Matrix[row][2]*rOther.m_Matrix[2][col];
02734         }
02735       }
02736 
02737       return product;
02738     }
02739 
02743     inline Pose2 operator*(const Pose2& rPose2) const
02744     {
02745       Pose2 pose2;
02746 
02747       pose2.SetX(m_Matrix[0][0] * rPose2.GetX() + m_Matrix[0][1] * rPose2.GetY() + m_Matrix[0][2] * rPose2.GetHeading());
02748       pose2.SetY(m_Matrix[1][0] * rPose2.GetX() + m_Matrix[1][1] * rPose2.GetY() + m_Matrix[1][2] * rPose2.GetHeading());
02749       pose2.SetHeading(m_Matrix[2][0] * rPose2.GetX() + m_Matrix[2][1] * rPose2.GetY() + m_Matrix[2][2] * rPose2.GetHeading());
02750 
02751       return pose2;
02752     }
02753 
02757     inline void operator+=(const Matrix3& rkMatrix)
02758     {
02759       for (kt_int32u row = 0; row < 3; row++)
02760       {
02761         for (kt_int32u col = 0; col < 3; col++)
02762         {
02763           m_Matrix[row][col] += rkMatrix.m_Matrix[row][col];
02764         }
02765       }
02766     }
02767     
02771     inline kt_bool operator==(const Matrix3& rkMatrix)
02772     {
02773       for (kt_int32u row = 0; row < 3; row++)
02774       {
02775         for (kt_int32u col = 0; col < 3; col++)
02776         {
02777           if (math::DoubleEqual(m_Matrix[row][col], rkMatrix.m_Matrix[row][col]) == false)
02778           {
02779             return false;
02780           }
02781         }
02782       }
02783       
02784       return true;
02785     }
02786     
02790     friend KARTO_FORCEINLINE std::ostream& operator << (std::ostream& rStream, const Matrix3& rMatrix)
02791     {
02792       rStream << rMatrix.ToString();
02793       return rStream;
02794     }
02795 
02796   private:
02805     kt_bool InverseFast(Matrix3& rkInverse, kt_double fTolerance = KT_TOLERANCE) const
02806     {
02807       // Invert a 3x3 using cofactors.  This is about 8 times faster than
02808       // the Numerical Recipes code which uses Gaussian elimination.
02809       rkInverse.m_Matrix[0][0] = m_Matrix[1][1]*m_Matrix[2][2] - m_Matrix[1][2]*m_Matrix[2][1];
02810       rkInverse.m_Matrix[0][1] = m_Matrix[0][2]*m_Matrix[2][1] - m_Matrix[0][1]*m_Matrix[2][2];
02811       rkInverse.m_Matrix[0][2] = m_Matrix[0][1]*m_Matrix[1][2] - m_Matrix[0][2]*m_Matrix[1][1];
02812       rkInverse.m_Matrix[1][0] = m_Matrix[1][2]*m_Matrix[2][0] - m_Matrix[1][0]*m_Matrix[2][2];
02813       rkInverse.m_Matrix[1][1] = m_Matrix[0][0]*m_Matrix[2][2] - m_Matrix[0][2]*m_Matrix[2][0];
02814       rkInverse.m_Matrix[1][2] = m_Matrix[0][2]*m_Matrix[1][0] - m_Matrix[0][0]*m_Matrix[1][2];
02815       rkInverse.m_Matrix[2][0] = m_Matrix[1][0]*m_Matrix[2][1] - m_Matrix[1][1]*m_Matrix[2][0];
02816       rkInverse.m_Matrix[2][1] = m_Matrix[0][1]*m_Matrix[2][0] - m_Matrix[0][0]*m_Matrix[2][1];
02817       rkInverse.m_Matrix[2][2] = m_Matrix[0][0]*m_Matrix[1][1] - m_Matrix[0][1]*m_Matrix[1][0];
02818       
02819       kt_double fDet = m_Matrix[0][0]*rkInverse.m_Matrix[0][0] + m_Matrix[0][1]*rkInverse.m_Matrix[1][0]+ m_Matrix[0][2]*rkInverse.m_Matrix[2][0];
02820       
02821       if (fabs(fDet) <= fTolerance)
02822       {
02823         return false;
02824       }
02825       
02826       kt_double fInvDet = 1.0/fDet;
02827       for (size_t row = 0; row < 3; row++)
02828       {
02829         for (size_t col = 0; col < 3; col++)
02830         {
02831           rkInverse.m_Matrix[row][col] *= fInvDet;
02832         }
02833       }
02834       
02835       return true;
02836     }
02837     
02838   private:
02839     kt_double m_Matrix[3][3];
02840   }; // class Matrix3
02841 
02845 
02846   class Color
02847   {
02848   public:
02852     Color()
02853       : m_Red(0.0)
02854       , m_Green(0.0)
02855       , m_Blue(0.0)
02856       , m_Alpha(1.0)
02857     {
02858     }
02859 
02863     Color(const Color& rOther)
02864       : m_Red(rOther.m_Red)
02865       , m_Green(rOther.m_Green)
02866       , m_Blue(rOther.m_Blue)
02867       , m_Alpha(rOther.m_Alpha)
02868     {
02869     }
02870 
02878     Color(kt_double red, kt_double green, kt_double blue, kt_double alpha = 1.0)
02879       : m_Red(red)
02880       , m_Green(green)
02881       , m_Blue(blue)
02882       , m_Alpha(alpha)
02883     {
02884     }
02885 
02893     Color(kt_int8u red, kt_int8u green, kt_int8u blue, kt_int8u alpha = 255)
02894       : m_Red((kt_double)red/255.0)
02895       , m_Green((kt_double)green/255.0)
02896       , m_Blue((kt_double)blue/255.0)
02897       , m_Alpha((kt_double)alpha/255.0)
02898     {
02899     }
02900 
02904     virtual ~Color()
02905     {
02906     }
02907 
02908   public:
02913     const kt_double GetRed() const
02914     {
02915       return m_Red;
02916     }
02917 
02922     void SetRed(kt_double red)
02923     {
02924       m_Red = red;
02925     }
02926 
02931     const kt_double GetGreen() const
02932     {
02933       return m_Green;
02934     }
02935 
02940     void SetGreen(kt_double green)
02941     {
02942       m_Green = green;
02943     }
02944 
02949     const kt_double GetBlue() const
02950     {
02951       return m_Blue;
02952     }
02953 
02958     void SetBlue(kt_double blue)
02959     {
02960       m_Blue = blue;
02961     }
02962 
02967     const kt_double GetAlpha() const
02968     {
02969       return m_Alpha;
02970     }
02971 
02976     void SetAlpha(kt_double alpha)
02977     {
02978       m_Alpha = alpha;
02979     }
02980 
02985     const String ToString() const
02986     {
02987       String valueString;
02988       valueString.Append(StringHelper::ToString(GetRed()));
02989       valueString.Append(" ");
02990       valueString.Append(StringHelper::ToString(GetGreen()));
02991       valueString.Append(" ");
02992       valueString.Append(StringHelper::ToString(GetBlue()));
02993       valueString.Append(" ");
02994       valueString.Append(StringHelper::ToString(GetAlpha()));
02995       return valueString;
02996     }
02997 
02998     public:
03002     inline kt_bool operator == (const Color& rOther) const
03003     {
03004       return (m_Red == rOther.m_Red && m_Green == rOther.m_Green && m_Blue == rOther.m_Blue && m_Alpha == rOther.m_Alpha);
03005     }
03006 
03010     inline kt_bool operator != (const Color& rOther) const
03011     {
03012       return (m_Red != rOther.m_Red || m_Green != rOther.m_Green || m_Blue != rOther.m_Blue || m_Alpha != rOther.m_Alpha);
03013     }
03014 
03018     friend KARTO_FORCEINLINE std::ostream& operator << (std::ostream& rStream, const Color& rColor)
03019     {
03020       rStream << rColor.ToString();
03021       return rStream;
03022     }
03023 
03024   private:
03025     kt_double m_Red;
03026     kt_double m_Green;
03027     kt_double m_Blue;
03028     kt_double m_Alpha;
03029   }; // class Color
03030 
03034  
03035   namespace gps
03036   {
03040     class PointGps : public karto::Vector2d
03041     {
03042     public:
03046       PointGps()
03047       {
03048       }
03049       
03055       PointGps(kt_double latitude, kt_double longitude)
03056       {
03057         SetX(longitude);
03058         SetY(latitude);
03059       }
03060       
03064       PointGps(const PointGps& rOther)
03065       {
03066         SetX(rOther.GetLongitude());
03067         SetY(rOther.GetLatitude());
03068       }
03069       
03070     public:
03075       inline kt_double GetLatitude() const
03076       {
03077         return GetY();
03078       }
03079       
03084       inline void SetLatitude(kt_double latitude)
03085       {
03086         SetY(latitude);
03087       }
03088       
03093       inline kt_double GetLongitude() const
03094       {
03095         return GetX();
03096       }
03097       
03102       inline void SetLongitude(kt_double longitude)
03103       {
03104         SetX(longitude);
03105       }
03106       
03113       inline kt_double GetBearing(const PointGps& rOther)
03114       {
03115         kt_double lat1 = math::DegreesToRadians(GetLatitude());
03116         kt_double long1 = math::DegreesToRadians(GetLongitude());
03117         kt_double lat2 = math::DegreesToRadians(rOther.GetLatitude());
03118         kt_double long2 = math::DegreesToRadians(rOther.GetLongitude());
03119         
03120         kt_double deltaLong = long2 - long1;
03121         
03122         kt_double y = sin(deltaLong) * cos(lat2);
03123         kt_double x = cos(lat1) * sin(lat2) - sin(lat1) * cos(lat2) * cos(deltaLong);
03124         
03125         return math::RadiansToDegrees(atan2(y, x));
03126       }
03127       
03132       void AddOffset(const PointGps& rOffset)
03133       {
03134         AddOffset(rOffset.GetLatitude(), rOffset.GetLongitude());
03135       }
03136       
03142       void AddOffset(kt_double latitude, kt_double longitude)
03143       {
03144         SetX(GetLongitude() + longitude);
03145         SetY(GetLatitude() - latitude);
03146       }
03147       
03154       kt_double Distance(const PointGps& rOther)
03155       {
03156         kt_double dlon = rOther.GetLongitude() - GetLongitude();
03157 
03158         const kt_double slat1 = sin(math::DegreesToRadians(GetLatitude()));
03159         const kt_double clat1 = cos(math::DegreesToRadians(GetLatitude()));
03160 
03161         const kt_double slat2 = sin(math::DegreesToRadians(rOther.GetLatitude()));
03162         const kt_double clat2 = cos(math::DegreesToRadians(rOther.GetLatitude()));
03163 
03164         const kt_double sdlon = sin(math::DegreesToRadians(dlon));
03165         const kt_double cdlon = cos(math::DegreesToRadians(dlon));
03166 
03167         const kt_double t1 = clat2 * sdlon;
03168         const kt_double t2 = clat1 * slat2 - slat1 * clat2 * cdlon;
03169         const kt_double t3 = slat1 * slat2 + clat1 * clat2 * cdlon;
03170         const kt_double dist = atan2(sqrt(t1*t1 + t2*t2), t3);
03171 
03172         const kt_double earthRadius = 6372.795;
03173         return dist * earthRadius;
03174       }
03175 
03180       inline const String ToString() const
03181       {
03182         String valueString;
03183         valueString.Append(StringHelper::ToString(GetLatitude()));
03184         valueString.Append(" ");
03185         valueString.Append(StringHelper::ToString(GetLongitude()));
03186         return valueString;
03187       }
03188       
03192       friend KARTO_FORCEINLINE std::ostream& operator << (std::ostream& rStream, const PointGps& rPointGps)
03193       {
03194         rStream << rPointGps.ToString();
03195         return rStream;
03196       }
03197 
03198     public:
03204       inline const PointGps operator + (const PointGps& rOther) const
03205       {
03206         return PointGps(GetLatitude() + rOther.GetLatitude(), GetLongitude() + rOther.GetLongitude());
03207       }
03208       
03214       inline const PointGps operator - (const PointGps& rOther) const
03215       {
03216         return PointGps(GetLatitude() - rOther.GetLatitude(), GetLongitude() - rOther.GetLongitude());
03217       }
03218     };
03219 
03220     typedef List<PointGps> PointGpsList;
03221   }
03222 
03226 
03227   template<typename T>
03228   inline String StringHelper::ToString(const Size2<T>& rValue)
03229   {
03230     return rValue.ToString();
03231   }
03232 
03233   template<typename T>
03234   inline String StringHelper::ToString(const Vector2<T>& rValue)
03235   {
03236     return rValue.ToString();
03237   }
03238 
03239   template<typename T>
03240   inline String StringHelper::ToString(const Vector3<T>& rValue)
03241   {
03242     return rValue.ToString();
03243   }
03244 
03245   template<typename T>
03246   inline String StringHelper::ToString(const Vector4<T>& rValue)
03247   {
03248     return rValue.ToString();
03249   }
03250 
03251   template<typename T>
03252   inline static kt_bool FromString(const String& rStringValue, Vector3<T>& rValue)
03253   {
03254     karto::String tempString = rStringValue;
03255     kt_size_t index = tempString.FindFirstOf(" ");
03256     if (index != -1)
03257     {
03258       karto::String stringValue;
03259       T value;
03260 
03261       // Get X
03262       stringValue = tempString.SubString(0, index);
03263 
03264       value = 0;
03265       FromString(stringValue, value);
03266       rValue.SetX(value);
03267 
03268       // Get Y
03269       tempString = rStringValue.SubString(index + 1, rStringValue.Size());
03270       index = tempString.FindFirstOf(" ");
03271 
03272       stringValue = tempString.SubString(0, index);
03273 
03274       value = 0;
03275       FromString(stringValue, value);
03276       rValue.SetY(value);
03277 
03278       // Get Z
03279       tempString = rStringValue.SubString(index + 1, rStringValue.Size());
03280       index = tempString.FindFirstOf(" ");
03281 
03282       stringValue = tempString.SubString(index + 1, rStringValue.Size());
03283 
03284       value = 0;
03285       FromString(stringValue, value);
03286       rValue.SetZ(value);
03287 
03288       return true;
03289     }
03290 
03291     return false;
03292   }
03293 
03297 
03299 
03300 }
03301 
03302 #endif // __OpenKarto_Geometry_h__


nav2d_karto
Author(s): Sebastian Kasperski
autogenerated on Thu Aug 27 2015 14:07:25