Geometry.h
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2006-2011, SRI International (R)
3  *
4  * This program is free software: you can redistribute it and/or modify
5  * it under the terms of the GNU Lesser General Public License as published by
6  * the Free Software Foundation, either version 3 of the License, or
7  * (at your option) any later version.
8  *
9  * This program is distributed in the hope that it will be useful,
10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12  * GNU Lesser General Public License for more details.
13  *
14  * You should have received a copy of the GNU Lesser General Public License
15  * along with this program. If not, see <http://www.gnu.org/licenses/>.
16  */
17 
18 #pragma once
19 
20 #ifndef __OpenKarto_Geometry_h__
21 #define __OpenKarto_Geometry_h__
22 
23 #include <OpenKarto/List.h>
24 #include <OpenKarto/StringHelper.h>
25 #include <OpenKarto/Math.h>
26 
27 #include <string.h>
28 
29 namespace karto
30 {
31 
35 
37 
38 
42  template<typename T>
43  class Size2
44  {
45  public:
50  : m_Width(0)
51  , m_Height(0)
52  {
53  }
54 
60  Size2(T width, T height)
61  : m_Width(width)
62  , m_Height(height)
63  {
64  }
65 
69  Size2(const Size2& rOther)
70  : m_Width(rOther.m_Width)
71  , m_Height(rOther.m_Height)
72  {
73  }
74 
75  public:
80  inline const T GetWidth() const
81  {
82  return m_Width;
83  }
84 
89  inline void SetWidth(T width)
90  {
91  m_Width = width;
92  }
93 
98  inline const T GetHeight() const
99  {
100  return m_Height;
101  }
102 
107  inline void SetHeight(T height)
108  {
109  m_Height = height;
110  }
111 
116  inline const karto::String ToString() const
117  {
118  String valueString;
119  valueString.Append(StringHelper::ToString(GetWidth()));
120  valueString.Append(" ");
121  valueString.Append(StringHelper::ToString(GetHeight()));
122  return valueString;
123  }
124 
125  public:
129  inline Size2& operator=(const Size2& rOther)
130  {
131  m_Width = rOther.m_Width;
132  m_Height = rOther.m_Height;
133 
134  return(*this);
135  }
136 
140  inline kt_bool operator==(const Size2& rOther) const
141  {
142  return (m_Width == rOther.m_Width && m_Height == rOther.m_Height);
143  }
144 
148  inline kt_bool operator!=(const Size2& rOther) const
149  {
150  return (m_Width != rOther.m_Width || m_Height != rOther.m_Height);
151  }
152 
156  friend KARTO_FORCEINLINE std::ostream& operator << (std::ostream& rStream, const Size2& rSize)
157  {
158  rStream << rSize.ToString().ToCString();
159  return rStream;
160  }
161 
162  private:
165  }; // class Size2<T>
166 
170 
174  template<typename T>
175  class Size3
176  {
177  public:
181  inline Size3()
182  : m_Width(0.0)
183  , m_Height(0.0)
184  , m_Depth(0.0)
185  {
186  }
187 
194  inline Size3(T width, T height, T depth)
195  : m_Width(width)
196  , m_Height(height)
197  , m_Depth(depth)
198  {
199  }
200 
204  inline Size3(const Size3& rOther)
205  : m_Width(rOther.m_Width)
206  , m_Height(rOther.m_Height)
207  {
208  }
209 
210  public:
215  inline T GetWidth() const
216  {
217  return m_Width;
218  }
219 
224  inline void SetWidth(T width)
225  {
226  m_Width = width;
227  }
228 
233  inline T GetHeight() const
234  {
235  return m_Height;
236  }
237 
242  inline void SetHeight(T height)
243  {
244  m_Height = height;
245  }
246 
251  inline T GetDepth() const
252  {
253  return m_Depth;
254  }
255 
260  inline void SetDepth(T depth)
261  {
262  m_Depth = depth;
263  }
264 
269  inline const karto::String ToString() const
270  {
271  String valueString;
272  valueString.Append(StringHelper::ToString(GetWidth()));
273  valueString.Append(" ");
274  valueString.Append(StringHelper::ToString(GetHeight()));
275  valueString.Append(" ");
276  valueString.Append(StringHelper::ToString(GetDepth()));
277  return valueString;
278  }
279 
280  public:
284  inline Size3& operator=(const Size3& rOther)
285  {
286  m_Width = rOther.m_Width;
287  m_Height = rOther.m_Height;
288  m_Depth = rOther.m_Depth;
289 
290  return *this;
291  }
292 
296  kt_bool operator==(const Size3& rOther) const
297  {
298  return m_Width == rOther.m_Width && m_Height == rOther.m_Height && m_Depth == rOther.m_Depth;
299  }
300 
304  kt_bool operator!=(const Size3& rOther) const
305  {
306  return m_Width != rOther.m_Width || m_Height != rOther.m_Height || m_Depth != rOther.m_Depth;
307  }
308 
309  private:
313  }; // class Size3
314 
318 
322  template<typename T>
323  class Vector2
324  {
325  public:
330  {
331  m_Values[0] = 0;
332  m_Values[1] = 0;
333  }
334 
340  Vector2(T x, T y)
341  {
342  m_Values[0] = x;
343  m_Values[1] = y;
344  }
345 
346  public:
351  inline const T& GetX() const
352  {
353  return m_Values[0];
354  }
355 
360  inline void SetX(const T& x)
361  {
362  m_Values[0] = x;
363  }
364 
369  inline const T& GetY() const
370  {
371  return m_Values[1];
372  }
373 
378  inline void SetY(const T& y)
379  {
380  m_Values[1] = y;
381  }
382 
387  inline void MakeFloor(const Vector2& rOther)
388  {
389  if (rOther.m_Values[0] < m_Values[0]) m_Values[0] = rOther.m_Values[0];
390  if (rOther.m_Values[1] < m_Values[1]) m_Values[1] = rOther.m_Values[1];
391  }
392 
397  inline void MakeCeil(const Vector2& rOther)
398  {
399  if (rOther.m_Values[0] > m_Values[0])
400  {
401  m_Values[0] = rOther.m_Values[0];
402  }
403 
404  if (rOther.m_Values[1] > m_Values[1])
405  {
406  m_Values[1] = rOther.m_Values[1];
407  }
408  }
409 
414  inline kt_double SquaredLength() const
415  {
416  return math::Square(m_Values[0]) + math::Square(m_Values[1]);
417  }
418 
423  inline kt_double Length() const
424  {
425  return sqrt(SquaredLength());
426  }
427 
433  inline kt_double SquaredDistance(const Vector2& rOther) const
434  {
435  return (*this - rOther).SquaredLength();
436  }
437 
443  inline kt_double Distance(const Vector2& rOther) const
444  {
445  return sqrt(SquaredDistance(rOther));
446  }
447 
452  inline const String ToString() const
453  {
454  String valueString;
455  valueString.Append(StringHelper::ToString(GetX()));
456  valueString.Append(" ");
457  valueString.Append(StringHelper::ToString(GetY()));
458  return valueString;
459  }
460 
461  public:
465  inline void operator+=(const Vector2& rOther)
466  {
467  m_Values[0] += rOther.m_Values[0];
468  m_Values[1] += rOther.m_Values[1];
469  }
470 
474  inline void operator-=(const Vector2& rOther)
475  {
476  m_Values[0] -= rOther.m_Values[0];
477  m_Values[1] -= rOther.m_Values[1];
478  }
479 
483  inline const Vector2 operator+(const Vector2& rOther) const
484  {
485  return Vector2(m_Values[0] + rOther.m_Values[0], m_Values[1] + rOther.m_Values[1]);
486  }
487 
491  inline const Vector2 operator-(const Vector2& rOther) const
492  {
493  return Vector2(m_Values[0] - rOther.m_Values[0], m_Values[1] - rOther.m_Values[1]);
494  }
495 
499  inline void operator/=(T scalar)
500  {
501  m_Values[0] /= scalar;
502  m_Values[1] /= scalar;
503  }
504 
508  inline const Vector2 operator/(T scalar) const
509  {
510  return Vector2(m_Values[0] / scalar, m_Values[1] / scalar);
511  }
512 
516  inline kt_double operator*(const Vector2& rOther) const
517  {
518  return m_Values[0] * rOther.m_Values[0] + m_Values[1] * rOther.m_Values[1];
519  }
520 
524  inline const Vector2 operator*(T scalar) const
525  {
526  return Vector2(m_Values[0] * scalar, m_Values[1] * scalar);
527  }
528 
532  inline const Vector2 operator-(T scalar) const
533  {
534  return Vector2(m_Values[0] - scalar, m_Values[1] - scalar);
535  }
536 
540  inline void operator*=(T scalar)
541  {
542  m_Values[0] *= scalar;
543  m_Values[1] *= scalar;
544  }
545 
549  inline kt_bool operator==(const Vector2& rOther) const
550  {
551  return (m_Values[0] == rOther.m_Values[0] && m_Values[1] == rOther.m_Values[1]);
552  }
553 
557  inline kt_bool operator!=(const Vector2& rOther) const
558  {
559  return (m_Values[0] != rOther.m_Values[0] || m_Values[1] != rOther.m_Values[1]);
560  }
561 
568  inline kt_bool operator<(const Vector2& rOther) const
569  {
570  if (m_Values[0] < rOther.m_Values[0])
571  {
572  return true;
573  }
574  else if (m_Values[0] > rOther.m_Values[0])
575  {
576  return false;
577  }
578  else
579  {
580  return (m_Values[1] < rOther.m_Values[1]);
581  }
582  }
583 
587  friend KARTO_FORCEINLINE std::ostream& operator<<(std::ostream& rStream, const Vector2& rVector)
588  {
589  rStream << rVector.ToString().ToCString();
590  return rStream;
591  }
592 
593  private:
594  T m_Values[2];
595  }; // class Vector2<T>
596 
601 
606 
611 
615 
619  template<typename T>
620  class Vector3
621  {
622  public:
627  {
628  m_Values[0] = 0;
629  m_Values[1] = 0;
630  m_Values[2] = 0;
631  }
632 
639  Vector3(T x, T y, T z)
640  {
641  m_Values[0] = x;
642  m_Values[1] = y;
643  m_Values[2] = z;
644  }
645 
650  Vector3(const Vector2<T>& rVector)
651  {
652  m_Values[0] = rVector.GetX();
653  m_Values[1] = rVector.GetY();
654  m_Values[2] = 0.0;
655  }
656 
660  Vector3(const Vector3& rOther)
661  {
662  m_Values[0] = rOther.m_Values[0];
663  m_Values[1] = rOther.m_Values[1];
664  m_Values[2] = rOther.m_Values[2];
665  }
666 
667  public:
672  inline const T& GetX() const
673  {
674  return m_Values[0];
675  }
676 
681  inline void SetX(const T& x)
682  {
683  m_Values[0] = x;
684  }
685 
690  inline const T& GetY() const
691  {
692  return m_Values[1];
693  }
694 
699  inline void SetY(const T& y)
700  {
701  m_Values[1] = y;
702  }
703 
708  inline const T& GetZ() const
709  {
710  return m_Values[2];
711  }
712 
717  inline void SetZ(const T& z)
718  {
719  m_Values[2] = z;
720  }
721 
726  inline Vector2<T> GetAsVector2() const
727  {
728  return Vector2<T>(m_Values[0], m_Values[1]);
729  }
730 
735  inline void MakeFloor(const Vector3& rOther)
736  {
737  if (rOther.m_Values[0] < m_Values[0]) m_Values[0] = rOther.m_Values[0];
738  if (rOther.m_Values[1] < m_Values[1]) m_Values[1] = rOther.m_Values[1];
739  if (rOther.m_Values[2] < m_Values[2]) m_Values[2] = rOther.m_Values[2];
740  }
741 
746  inline void MakeCeil(const Vector3& rOther)
747  {
748  if (rOther.m_Values[0] > m_Values[0]) m_Values[0] = rOther.m_Values[0];
749  if (rOther.m_Values[1] > m_Values[1]) m_Values[1] = rOther.m_Values[1];
750  if (rOther.m_Values[2] > m_Values[2]) m_Values[2] = rOther.m_Values[2];
751  }
752 
757  inline kt_double SquaredLength() const
758  {
759  return math::Square(m_Values[0]) + math::Square(m_Values[1]) + math::Square(m_Values[2]);
760  }
761 
766  inline kt_double Length() const
767  {
768  return sqrt(SquaredLength());
769  }
770 
774  inline void Normalize()
775  {
776  kt_double length = Length();
777  if (length > 0.0)
778  {
779  kt_double inversedLength = 1.0 / length;
780 
781  m_Values[0] *= inversedLength;
782  m_Values[1] *= inversedLength;
783  m_Values[2] *= inversedLength;
784  }
785  }
786 
791  inline const String ToString() const
792  {
793  String valueString;
794  valueString.Append(StringHelper::ToString(GetX()));
795  valueString.Append(" ");
796  valueString.Append(StringHelper::ToString(GetY()));
797  valueString.Append(" ");
798  valueString.Append(StringHelper::ToString(GetZ()));
799  return valueString;
800  }
801 
802  public:
806  inline Vector3& operator=(const Vector3& rOther)
807  {
808  m_Values[0] = rOther.m_Values[0];
809  m_Values[1] = rOther.m_Values[1];
810  m_Values[2] = rOther.m_Values[2];
811 
812  return *this;
813  }
814 
818  inline const Vector3 operator+(const Vector3& rOther) const
819  {
820  return Vector3(m_Values[0] + rOther.m_Values[0], m_Values[1] + rOther.m_Values[1], m_Values[2] + rOther.m_Values[2]);
821  }
822 
826  inline const Vector3 operator+(kt_double scalar) const
827  {
828  return Vector3(m_Values[0] + scalar, m_Values[1] + scalar, m_Values[2] + scalar);
829  }
830 
834  inline const Vector3 operator-(const Vector3& rOther) const
835  {
836  return Vector3(m_Values[0] - rOther.m_Values[0], m_Values[1] - rOther.m_Values[1], m_Values[2] - rOther.m_Values[2]);
837  }
838 
842  inline const Vector3 operator-(kt_double scalar) const
843  {
844  return Vector3(m_Values[0] - scalar, m_Values[1] - scalar, m_Values[2] - scalar);
845  }
846 
850  inline const Vector3 operator*(T scalar) const
851  {
852  return Vector3(m_Values[0] * scalar, m_Values[1] * scalar, m_Values[2] * scalar);
853  }
854 
858  inline Vector3& operator+=(const Vector3& rOther)
859  {
860  m_Values[0] += rOther.m_Values[0];
861  m_Values[1] += rOther.m_Values[1];
862  m_Values[2] += rOther.m_Values[2];
863 
864  return *this;
865  }
866 
870  inline Vector3& operator-=(const Vector3& rOther)
871  {
872  m_Values[0] -= rOther.m_Values[0];
873  m_Values[1] -= rOther.m_Values[1];
874  m_Values[2] -= rOther.m_Values[2];
875 
876  return *this;
877  }
878 
884  inline Vector3& operator*=(const Vector3& rOther)
885  {
886  m_Values[0] *= rOther.m_Values[0];
887  m_Values[1] *= rOther.m_Values[1];
888  m_Values[2] *= rOther.m_Values[2];
889 
890  return *this;
891  }
892 
898  inline Vector3& operator/=(const Vector3& rOther)
899  {
900  m_Values[0] /= rOther.m_Values[0];
901  m_Values[1] /= rOther.m_Values[1];
902  m_Values[2] /= rOther.m_Values[2];
903 
904  return *this;
905  }
906 
912  inline Vector3& operator+=(const T& rValue)
913  {
914  m_Values[0] += rValue;
915  m_Values[1] += rValue;
916  m_Values[2] += rValue;
917 
918  return *this;
919  }
920 
926  inline Vector3& operator-=(const T& rValue)
927  {
928  m_Values[0] -= rValue;
929  m_Values[1] -= rValue;
930  m_Values[2] -= rValue;
931 
932  return *this;
933  }
934 
938  inline Vector3& operator*=(const T& rValue)
939  {
940  m_Values[0] *= rValue;
941  m_Values[1] *= rValue;
942  m_Values[2] *= rValue;
943 
944  return *this;
945  }
946 
952  inline Vector3& operator/=(const T& rValue)
953  {
954  m_Values[0] /= rValue;
955  m_Values[1] /= rValue;
956  m_Values[2] /= rValue;
957 
958  return *this;
959  }
960 
966  inline const Vector3 operator^(const Vector3& rOther) const
967  {
968  return Vector3(
969  m_Values[1] * rOther.m_Values[2] - m_Values[2] * rOther.m_Values[1],
970  m_Values[2] * rOther.m_Values[0] - m_Values[0] * rOther.m_Values[2] ,
971  m_Values[0] * rOther.m_Values[1] - m_Values[1] * rOther.m_Values[0]);
972  }
973 
980  inline kt_bool operator<(const Vector3& rOther) const
981  {
982  if (m_Values[0] < rOther.m_Values[0])
983  {
984  return true;
985  }
986  else if (m_Values[0] > rOther.m_Values[0])
987  {
988  return false;
989  }
990  else if (m_Values[1] < rOther.m_Values[1])
991  {
992  return true;
993  }
994  else if (m_Values[1] > rOther.m_Values[1])
995  {
996  return false;
997  }
998  else
999  {
1000  return (m_Values[2] < rOther.m_Values[2]);
1001  }
1002  }
1003 
1007  inline kt_bool operator==(const Vector3& rOther) const
1008  {
1009  return (m_Values[0] == rOther.m_Values[0] && m_Values[1] == rOther.m_Values[1] && m_Values[2] == rOther.m_Values[2]);
1010  }
1011 
1015  inline kt_bool operator!=(const Vector3& rOther) const
1016  {
1017  return (m_Values[0] != rOther.m_Values[0] || m_Values[1] != rOther.m_Values[1] || m_Values[2] != rOther.m_Values[2]);
1018  }
1019 
1023  friend KARTO_FORCEINLINE std::ostream& operator << (std::ostream& rStream, const Vector3& rVector)
1024  {
1025  rStream << rVector.ToString().ToCString();
1026  return rStream;
1027  }
1028 
1029  private:
1030  T m_Values[3];
1031  }; // class Vector3<T>
1032 
1037 
1042 
1047 
1051 
1055  template<typename T>
1056  class Vector4
1057  {
1058  public:
1063  {
1064  m_Values[0] = 0;
1065  m_Values[1] = 0;
1066  m_Values[2] = 0;
1067  m_Values[3] = 0;
1068  }
1069 
1077  Vector4(T x, T y, T z, T w)
1078  {
1079  m_Values[0] = x;
1080  m_Values[1] = y;
1081  m_Values[2] = z;
1082  m_Values[3] = w;
1083  }
1084 
1088  Vector4(const Vector4& rOther)
1089  {
1090  m_Values[0] = rOther.m_Values[0];
1091  m_Values[1] = rOther.m_Values[1];
1092  m_Values[2] = rOther.m_Values[2];
1093  m_Values[3] = rOther.m_Values[3];
1094  }
1095 
1096  public:
1101  inline const T& GetX() const
1102  {
1103  return m_Values[0];
1104  }
1105 
1110  inline void SetX(const T& x)
1111  {
1112  m_Values[0] = x;
1113  }
1114 
1119  inline const T& GetY() const
1120  {
1121  return m_Values[1];
1122  }
1123 
1128  inline void SetY(const T& y)
1129  {
1130  m_Values[1] = y;
1131  }
1132 
1137  inline const T& GetZ() const
1138  {
1139  return m_Values[2];
1140  }
1141 
1146  inline void SetZ(const T& z)
1147  {
1148  m_Values[2] = z;
1149  }
1150 
1155  inline const T& GetW() const
1156  {
1157  return m_Values[3];
1158  }
1159 
1164  inline void SetW(const T& w)
1165  {
1166  m_Values[3] = w;
1167  }
1168 
1173  inline const String ToString() const
1174  {
1175  String valueString;
1176  valueString.Append(StringHelper::ToString(GetX()));
1177  valueString.Append(" ");
1178  valueString.Append(StringHelper::ToString(GetY()));
1179  valueString.Append(" ");
1180  valueString.Append(StringHelper::ToString(GetZ()));
1181  valueString.Append(" ");
1182  valueString.Append(StringHelper::ToString(GetW()));
1183  return valueString;
1184  }
1185 
1186  public:
1190  inline Vector4& operator = (const Vector4& rOther)
1191  {
1192  m_Values[0] = rOther.m_Values[0];
1193  m_Values[1] = rOther.m_Values[1];
1194  m_Values[2] = rOther.m_Values[2];
1195  m_Values[3] = rOther.m_Values[3];
1196 
1197  return *this;
1198  }
1199 
1206  inline kt_bool operator<(const Vector4& rOther) const
1207  {
1208  if (m_Values[0] < rOther.m_Values[0])
1209  {
1210  return true;
1211  }
1212  else if (m_Values[0] > rOther.m_Values[0])
1213  {
1214  return false;
1215  }
1216  else if (m_Values[1] < rOther.m_Values[1])
1217  {
1218  return true;
1219  }
1220  else if (m_Values[1] > rOther.m_Values[1])
1221  {
1222  return false;
1223  }
1224  else if (m_Values[2] < rOther.m_Values[2])
1225  {
1226  return true;
1227  }
1228  else if (m_Values[2] > rOther.m_Values[2])
1229  {
1230  return false;
1231  }
1232  else
1233  {
1234  return (m_Values[3] < rOther.m_Values[3]);
1235  }
1236  }
1237 
1241  inline kt_bool operator==(const Vector4& rOther) const
1242  {
1243  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]);
1244  }
1245 
1249  inline kt_bool operator!=(const Vector4& rOther) const
1250  {
1251  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]);
1252  }
1253 
1257  friend KARTO_FORCEINLINE std::ostream& operator<<(std::ostream& rStream, const Vector4& rVector)
1258  {
1259  rStream << rVector.ToString().ToCString();
1260  return rStream;
1261  }
1262 
1263  private:
1264  T m_Values[4];
1265  }; // class Vector4<T>
1266 
1271 
1276 
1281 
1285 
1308  {
1309  public:
1313  inline Quaternion()
1314  {
1315  m_Values[0] = 0.0;
1316  m_Values[1] = 0.0;
1317  m_Values[2] = 0.0;
1318  m_Values[3] = 1.0;
1319  }
1320 
1329  {
1330  m_Values[0] = x;
1331  m_Values[1] = y;
1332  m_Values[2] = z;
1333  m_Values[3] = w;
1334  }
1335 
1339  inline Quaternion(const Vector4d& rVector)
1340  {
1341  m_Values[0] = rVector.GetX();
1342  m_Values[1] = rVector.GetY();
1343  m_Values[2] = rVector.GetZ();
1344  m_Values[3] = rVector.GetW();
1345  }
1346 
1350  inline Quaternion(const Quaternion& rOther)
1351  {
1352  m_Values[0] = rOther.m_Values[0];
1353  m_Values[1] = rOther.m_Values[1];
1354  m_Values[2] = rOther.m_Values[2];
1355  m_Values[3] = rOther.m_Values[3];
1356  }
1357 
1358  public:
1363  inline kt_double GetX() const
1364  {
1365  return m_Values[0];
1366  }
1367 
1372  inline void SetX(kt_double x)
1373  {
1374  m_Values[0] = x;
1375  }
1376 
1381  inline kt_double GetY() const
1382  {
1383  return m_Values[1];
1384  }
1385 
1390  inline void SetY(kt_double y)
1391  {
1392  m_Values[1] = y;
1393  }
1394 
1399  inline kt_double GetZ() const
1400  {
1401  return m_Values[2];
1402  }
1403 
1408  inline void SetZ(kt_double z)
1409  {
1410  m_Values[2] = z;
1411  }
1412 
1417  inline kt_double GetW() const
1418  {
1419  return m_Values[3];
1420  }
1421 
1426  inline void SetW(kt_double w)
1427  {
1428  m_Values[3] = w;
1429  }
1430 
1435  inline const Vector4d GetAsVector4() const
1436  {
1437  return Vector4d(m_Values[0], m_Values[1], m_Values[2], m_Values[3]);
1438  }
1439 
1445  void ToAngleAxis(kt_double& rAngle, karto::Vector3d& rAxis) const;
1446 
1450  void FromAngleAxis(kt_double angleInRadians, const karto::Vector3d& rAxis);
1451 
1459  void ToEulerAngles(kt_double& rYaw, kt_double& rPitch, kt_double& rRoll) const;
1460 
1468  void FromEulerAngles(kt_double yaw, kt_double pitch, kt_double roll);
1469 
1474  inline kt_double Length() const
1475  {
1476  return sqrt(Norm());
1477  }
1478 
1483  inline kt_double Norm() const
1484  {
1485  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];
1486  }
1487 
1491  inline void Normalize()
1492  {
1493  kt_double length = Length();
1494  if (length > 0.0)
1495  {
1496  kt_double inversedLength = 1.0 / length;
1497 
1498  m_Values[0] *= inversedLength;
1499  m_Values[1] *= inversedLength;
1500  m_Values[2] *= inversedLength;
1501  m_Values[3] *= inversedLength;
1502  }
1503  }
1504 
1509  const String ToString() const;
1510 
1511  public:
1515  inline Quaternion& operator=(const Quaternion& rOther)
1516  {
1517  m_Values[0] = rOther.m_Values[0];
1518  m_Values[1] = rOther.m_Values[1];
1519  m_Values[2] = rOther.m_Values[2];
1520  m_Values[3] = rOther.m_Values[3];
1521 
1522  return(*this);
1523  }
1524 
1528  inline const Quaternion operator*(const Quaternion& rOther) const
1529  {
1530  return Quaternion(
1531  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],
1532  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],
1533  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],
1534  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] );
1535  }
1536 
1540  inline kt_bool operator==(const Quaternion& rOther) const
1541  {
1542  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]);
1543  }
1544 
1548  inline kt_bool operator!=(const Quaternion& rOther) const
1549  {
1550  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]);
1551  }
1552 
1559  {
1560  // nVidia SDK implementation
1561  karto::Vector3d uv, uuv;
1562  karto::Vector3d qvec(m_Values[0], m_Values[1], m_Values[2]);
1563 
1564  uv = qvec ^ rVector;
1565  uuv = qvec ^ uv;
1566 
1567  uv *= ( 2.0 * m_Values[3] );
1568  uuv *= 2.0;
1569 
1570  return rVector + uv + uuv;
1571  }
1572 
1576  friend KARTO_FORCEINLINE std::ostream& operator<<(std::ostream& rStream, const Quaternion& rQuaternion)
1577  {
1578  rStream << rQuaternion.ToString().ToCString();
1579  return rStream;
1580  }
1581 
1582  private:
1583  kt_double m_Values[4];
1584  }; // class Quaternion
1585 
1589 
1594  {
1595  public:
1596  /*
1597  * Bounding box of maximal size
1598  */
1599  BoundingBox2();
1600 
1606  BoundingBox2(const Vector2d& rMinimum, const Vector2d& rMaximum);
1607 
1608  public:
1613  inline const Vector2d& GetMinimum() const
1614  {
1615  return m_Minimum;
1616  }
1617 
1622  inline void SetMinimum(const Vector2d& rMinimum)
1623  {
1624  m_Minimum = rMinimum;
1625  }
1626 
1631  inline const Vector2d& GetMaximum() const
1632  {
1633  return m_Maximum;
1634  }
1635 
1640  inline void SetMaximum(const Vector2d& rMaximum)
1641  {
1642  m_Maximum = rMaximum;
1643  }
1644 
1649  inline Size2<kt_double> GetSize() const
1650  {
1651  Vector2d size = m_Maximum - m_Minimum;
1652 
1653  return Size2<kt_double>(size.GetX(), size.GetY());
1654  }
1655 
1660  inline void Add(const Vector2d& rPoint)
1661  {
1662  m_Minimum.MakeFloor(rPoint);
1663  m_Maximum.MakeCeil(rPoint);
1664  }
1665 
1670  inline void Add(const BoundingBox2& rBoundingBox)
1671  {
1672  Add(rBoundingBox.GetMinimum());
1673  Add(rBoundingBox.GetMaximum());
1674  }
1675 
1681  inline kt_bool Contains(const Vector2d& rPoint) const
1682  {
1683  return (math::InRange(rPoint.GetX(), m_Minimum.GetX(), m_Maximum.GetX()) &&
1684  math::InRange(rPoint.GetY(), m_Minimum.GetY(), m_Maximum.GetY()));
1685  }
1686 
1692  inline kt_bool Intersects(const BoundingBox2& rOther) const
1693  {
1694  if ((m_Maximum.GetX() < rOther.m_Minimum.GetX()) || (m_Minimum.GetX() > rOther.m_Maximum.GetX()))
1695  {
1696  return false;
1697  }
1698 
1699  if ((m_Maximum.GetY() < rOther.m_Minimum.GetY()) || (m_Minimum.GetY() > rOther.m_Maximum.GetY()))
1700  {
1701  return false;
1702  }
1703 
1704  return true;
1705  }
1706 
1712  inline kt_bool Contains(const BoundingBox2& rOther) const
1713  {
1714  if ((m_Maximum.GetX() < rOther.m_Minimum.GetX()) || (m_Minimum.GetX() > rOther.m_Maximum.GetX()))
1715  {
1716  return false;
1717  }
1718 
1719  if ((m_Maximum.GetY() < rOther.m_Minimum.GetY()) || (m_Minimum.GetY() > rOther.m_Maximum.GetY()))
1720  {
1721  return false;
1722  }
1723 
1724  if ((m_Minimum.GetX() <= rOther.m_Minimum.GetX()) && (rOther.m_Maximum.GetX() <= m_Maximum.GetX()) &&
1725  (m_Minimum.GetY() <= rOther.m_Minimum.GetY()) && (rOther.m_Maximum.GetY() <= m_Maximum.GetY()))
1726  {
1727  return true;
1728  }
1729 
1730  return false;
1731  }
1732 
1733  private:
1734  Vector2d m_Minimum;
1735  Vector2d m_Maximum;
1736  }; // class BoundingBox2
1737 
1742  {
1743  public:
1744  BoundingBox3();
1745  virtual ~BoundingBox3();
1746 
1747  public:
1752  inline const Vector3d& GetMinimum() const
1753  {
1754  return m_Minimum;
1755  }
1756 
1761  inline void SetMinimum(const Vector3d& rMinimum)
1762  {
1763  m_Minimum = rMinimum;
1764  }
1765 
1770  inline const Vector3d& GetMaximum() const
1771  {
1772  return m_Maximum;
1773  }
1774 
1779  inline void SetMaximum(const Vector3d& rMaximum)
1780  {
1781  m_Maximum = rMaximum;
1782  }
1783 
1784  private:
1785  Vector3d m_Minimum;
1786  Vector3d m_Maximum;
1787  };
1788 
1792 
1797  template<typename T>
1799  {
1800  public:
1805  {
1806  }
1807 
1815  Rectangle2(T x, T y, T width, T height)
1816  : m_Position(x, y)
1817  , m_Size(width, height)
1818  {
1819  }
1820 
1826  Rectangle2(const Vector2<T>& rPosition, const Size2<T>& rSize)
1827  : m_Position(rPosition)
1828  , m_Size(rSize)
1829  {
1830  }
1831 
1837  Rectangle2(const Vector2<T>& rTopLeft, const Vector2<T>& rBottomRight)
1838  : m_Position(rTopLeft)
1839  , m_Size(rBottomRight.GetX() - rTopLeft.GetX(), rBottomRight.GetY() - rTopLeft.GetY())
1840  {
1841  }
1842 
1846  Rectangle2(const Rectangle2& rOther)
1847  : m_Position(rOther.m_Position)
1848  , m_Size(rOther.m_Size)
1849  {
1850  }
1851 
1852  public:
1857  inline T GetX() const
1858  {
1859  return m_Position.GetX();
1860  }
1861 
1866  inline void SetX(const T& rX)
1867  {
1868  m_Position.SetX(rX);
1869  }
1870 
1875  inline T GetY() const
1876  {
1877  return m_Position.GetY();
1878  }
1879 
1884  inline void SetY(const T& rY)
1885  {
1886  m_Position.SetY(rY);
1887  }
1888 
1893  inline T GetWidth() const
1894  {
1895  return m_Size.GetWidth();
1896  }
1897 
1902  inline void SetWidth(const T& rWidth)
1903  {
1904  m_Size.SetWidth(rWidth);
1905  }
1906 
1911  inline T GetHeight() const
1912  {
1913  return m_Size.GetHeight();
1914  }
1915 
1920  inline void SetHeight(const T& rHeight)
1921  {
1922  m_Size.SetHeight(rHeight);
1923  }
1924 
1929  inline const Vector2<T>& GetPosition() const
1930  {
1931  return m_Position;
1932  }
1933 
1939  inline void SetPosition(const T& rX, const T& rY)
1940  {
1941  m_Position = Vector2<T>(rX, rY);
1942  }
1943 
1948  inline void SetPosition(const Vector2<T>& rPosition)
1949  {
1950  m_Position = rPosition;
1951  }
1952 
1957  inline const Size2<T>& GetSize() const
1958  {
1959  return m_Size;
1960  }
1961 
1966  inline void SetSize(const Size2<T>& rSize)
1967  {
1968  m_Size = rSize;
1969  }
1970 
1975  inline T GetLeft() const
1976  {
1977  return m_Position.GetX();
1978  }
1979 
1984  inline void SetLeft(const T& rLeft)
1985  {
1986  m_Position.SetX(rLeft);
1987  }
1988 
1993  inline T GetTop() const
1994  {
1995  return m_Position.GetY();
1996  }
1997 
2002  inline void SetTop(const T& rTop)
2003  {
2004  m_Position.SetY(rTop);
2005  }
2006 
2011  inline T GetRight() const
2012  {
2013  return m_Position.GetX() + m_Size.GetWidth();
2014  }
2015 
2020  inline void SetRight(const T& rRight)
2021  {
2022  m_Size.SetWidth(rRight - m_Position.GetX());
2023  }
2024 
2029  inline T GetBottom() const
2030  {
2031  return m_Position.GetY() + m_Size.GetHeight();
2032  }
2033 
2038  inline void SetBottom(const T& rBottom)
2039  {
2040  m_Size.SetHeight(rBottom - m_Position.GetY());
2041  }
2042 
2048  {
2049  return Vector2<T>(GetLeft(), GetTop());
2050  }
2051 
2057  {
2058  return Vector2<T>(GetRight(), GetTop());
2059  }
2060 
2066  {
2067  return Vector2<T>(GetLeft(), GetBottom());
2068  }
2069 
2075  {
2076  return Vector2<T>(GetRight(), GetBottom());
2077  }
2078 
2083  inline const Vector2<T> GetCenter() const
2084  {
2085  return Vector2<T>(m_Position.GetX() + m_Size.GetWidth() * 0.5, m_Position.GetY() + m_Size.GetHeight() * 0.5);
2086  }
2087 
2093  inline kt_bool Contains(const Rectangle2<T>& rOther)
2094  {
2095  T l1 = m_Position.GetX();
2096  T r1 = m_Position.GetX();
2097  if (m_Size.GetWidth() < 0)
2098  l1 += m_Size.GetWidth();
2099  else
2100  r1 += m_Size.GetWidth();
2101  if (l1 == r1) // null rect
2102  return false;
2103 
2104  T l2 = rOther.m_Position.GetX();
2105  T r2 = rOther.m_Position.GetX();
2106  if (rOther.m_Size.GetWidth() < 0)
2107  l2 += rOther.m_Size.GetWidth();
2108  else
2109  r2 += rOther.m_Size.GetWidth();
2110  if (l2 == r2) // null rect
2111  return false;
2112 
2113  if (l2 < l1 || r2 > r1)
2114  return false;
2115 
2116  T t1 = m_Position.GetY();
2117  T b1 = m_Position.GetY();
2118  if (m_Size.GetHeight() < 0)
2119  t1 += m_Size.GetHeight();
2120  else
2121  b1 += m_Size.GetHeight();
2122  if (t1 == b1) // null rect
2123  return false;
2124 
2125  T t2 = rOther.m_Position.GetY();
2126  T b2 = rOther.m_Position.GetY();
2127  if (rOther.m_Size.GetHeight() < 0)
2128  t2 += rOther.m_Size.GetHeight();
2129  else
2130  b2 += rOther.m_Size.GetHeight();
2131  if (t2 == b2) // null rect
2132  return false;
2133 
2134  if (t2 < t1 || b2 > b1)
2135  return false;
2136 
2137  return true;
2138  }
2139 
2140  public:
2145  {
2146  m_Position = rOther.m_Position;
2147  m_Size = rOther.m_Size;
2148 
2149  return *this;
2150  }
2151 
2155  inline kt_bool operator == (const Rectangle2& rOther) const
2156  {
2157  return (m_Position == rOther.m_Position && m_Size == rOther.m_Size);
2158  }
2159 
2163  inline kt_bool operator != (const Rectangle2& rOther) const
2164  {
2165  return (m_Position != rOther.m_Position || m_Size != rOther.m_Size);
2166  }
2167 
2168  private:
2171  }; // class Rectangle2<T>
2172 
2176 
2177  class Pose3;
2178 
2183  {
2184  public:
2188  Pose2();
2189 
2195  Pose2(const Vector2d& rPosition, kt_double heading = 0);
2196 
2203  Pose2(kt_double x, kt_double y, kt_double heading);
2204 
2208  Pose2(const Pose3& rPose);
2209 
2213  Pose2(const Pose2& rOther);
2214 
2215  public:
2220  inline kt_double GetX() const
2221  {
2222  return m_Position.GetX();
2223  }
2224 
2229  inline void SetX(kt_double x)
2230  {
2231  m_Position.SetX(x);
2232  }
2233 
2238  inline kt_double GetY() const
2239  {
2240  return m_Position.GetY();
2241  }
2242 
2247  inline void SetY(kt_double y)
2248  {
2249  m_Position.SetY(y);
2250  }
2251 
2256  inline const Vector2d& GetPosition() const
2257  {
2258  return m_Position;
2259  }
2260 
2265  inline void SetPosition(const Vector2d& rPosition)
2266  {
2267  m_Position = rPosition;
2268  }
2269 
2274  inline kt_double GetHeading() const
2275  {
2276  return m_Heading;
2277  }
2278 
2283  inline void SetHeading(kt_double heading)
2284  {
2285  m_Heading = heading;
2286  }
2287 
2293  inline kt_double SquaredDistance(const Pose2& rOther) const
2294  {
2295  return m_Position.SquaredDistance(rOther.m_Position);
2296  }
2297 
2303  inline kt_double AngleTo(const Vector2d& rVector) const
2304  {
2305  kt_double angle = atan2(rVector.GetY() - GetY(), rVector.GetX() - GetX());
2306  return karto::math::NormalizeAngle(angle - GetHeading());
2307  }
2308 
2314  inline const String ToString(kt_int32u precision = 4) const
2315  {
2316  String valueString;
2317  valueString.Append(StringHelper::ToString(m_Position.GetX(), precision));
2318  valueString.Append(" ");
2319  valueString.Append(StringHelper::ToString(m_Position.GetY(), precision));
2320  valueString.Append(" ");
2321  valueString.Append(StringHelper::ToString(m_Heading, precision));
2322  return valueString;
2323  }
2324 
2325  public:
2329  inline Pose2& operator = (const Pose2& rOther)
2330  {
2331  m_Position = rOther.m_Position;
2332  m_Heading = rOther.m_Heading;
2333 
2334  return *this;
2335  }
2336 
2340  inline kt_bool operator==(const Pose2& rOther) const
2341  {
2342  return (m_Position == rOther.m_Position && m_Heading == rOther.m_Heading);
2343  }
2344 
2348  inline kt_bool operator!=(const Pose2& rOther) const
2349  {
2350  return (m_Position != rOther.m_Position || m_Heading != rOther.m_Heading);
2351  }
2352 
2356  inline void operator+=(const Pose2& rOther)
2357  {
2358  m_Position += rOther.m_Position;
2359  m_Heading = math::NormalizeAngle(m_Heading + rOther.m_Heading);
2360  }
2361 
2365  inline Pose2 operator+(const Pose2& rOther) const
2366  {
2367  return Pose2(m_Position + rOther.m_Position, math::NormalizeAngle(m_Heading + rOther.m_Heading));
2368  }
2369 
2373  inline Pose2 operator-(const Pose2& rOther) const
2374  {
2375  return Pose2(m_Position - rOther.m_Position, math::NormalizeAngle(m_Heading - rOther.m_Heading));
2376  }
2377 
2381  friend KARTO_FORCEINLINE std::ostream& operator << (std::ostream& rStream, const Pose2& rPose)
2382  {
2383  rStream << rPose.ToString();
2384  return rStream;
2385  }
2386 
2387  private:
2388  Vector2d m_Position;
2389 
2391  }; // class Pose2
2392 
2397 
2401 
2409  class Pose3
2410  {
2411  public:
2416  {
2417  }
2418 
2423  Pose3(const Vector3d& rPosition)
2424  : m_Position(rPosition)
2425  {
2426  }
2427 
2433  Pose3(const Vector3d& rPosition, const karto::Quaternion& rOrientation)
2434  : m_Position(rPosition)
2435  , m_Orientation(rOrientation)
2436  {
2437  }
2438 
2442  Pose3(const Pose3& rOther)
2443  : m_Position(rOther.m_Position)
2444  , m_Orientation(rOther.m_Orientation)
2445  {
2446  }
2447 
2452  Pose3(const Pose2& rPose)
2453  {
2454  m_Position = Vector3d(rPose.GetX(), rPose.GetY(), 0.0);
2455  m_Orientation.FromEulerAngles(rPose.GetHeading(), 0.0, 0.0);
2456  }
2457 
2458  public:
2463  inline const Vector3d& GetPosition() const
2464  {
2465  return m_Position;
2466  }
2467 
2472  inline void SetPosition(const Vector3d& rPosition)
2473  {
2474  m_Position = rPosition;
2475  }
2476 
2481  inline const Quaternion& GetOrientation() const
2482  {
2483  return m_Orientation;
2484  }
2485 
2490  inline void SetOrientation(const Quaternion& rOrientation)
2491  {
2492  m_Orientation = rOrientation;
2493  }
2494 
2499  inline const String ToString() const
2500  {
2501  String valueString;
2502  valueString.Append(GetPosition().ToString());
2503  valueString.Append(" ");
2504  valueString.Append(GetOrientation().ToString());
2505  return valueString;
2506  }
2507 
2508  public:
2512  inline Pose3& operator = (const Pose3& rOther)
2513  {
2514  m_Position = rOther.m_Position;
2515  m_Orientation = rOther.m_Orientation;
2516 
2517  return *this;
2518  }
2519 
2523  inline kt_bool operator == (const Pose3& rOther) const
2524  {
2525  return (m_Position == rOther.m_Position && m_Orientation == rOther.m_Orientation);
2526  }
2527 
2531  inline kt_bool operator != (const Pose3& rOther) const
2532  {
2533  return (m_Position != rOther.m_Position || m_Orientation != rOther.m_Orientation);
2534  }
2535 
2539  friend KARTO_FORCEINLINE std::ostream& operator << (std::ostream& rStream, const Pose3& rPose)
2540  {
2541  rStream << rPose.ToString();
2542  return rStream;
2543  }
2544 
2545  private:
2546  Vector3d m_Position;
2548  }; // class Pose3
2549 
2553 
2557  class Matrix3
2558  {
2559  public:
2564  {
2565  Clear();
2566  }
2567 
2571  inline Matrix3(const Matrix3& rOther)
2572  {
2573  memcpy(m_Matrix, rOther.m_Matrix, 9*sizeof(kt_double));
2574  }
2575 
2576  public:
2581  {
2582  memset(m_Matrix, 0, 9*sizeof(kt_double));
2583 
2584  for (kt_int32s i = 0; i < 3; i++)
2585  {
2586  m_Matrix[i][i] = 1.0;
2587  }
2588  }
2589 
2593  void Clear()
2594  {
2595  memset(m_Matrix, 0, 9*sizeof(kt_double));
2596  }
2597 
2605  void FromAxisAngle(kt_double x, kt_double y, kt_double z, const kt_double radians)
2606  {
2607  kt_double cosRadians = cos(radians);
2608  kt_double sinRadians = sin(radians);
2609  kt_double oneMinusCos = 1.0 - cosRadians;
2610 
2611  kt_double xx = x * x;
2612  kt_double yy = y * y;
2613  kt_double zz = z * z;
2614 
2615  kt_double xyMCos = x * y * oneMinusCos;
2616  kt_double xzMCos = x * z * oneMinusCos;
2617  kt_double yzMCos = y * z * oneMinusCos;
2618 
2619  kt_double xSin = x * sinRadians;
2620  kt_double ySin = y * sinRadians;
2621  kt_double zSin = z * sinRadians;
2622 
2623  m_Matrix[0][0] = xx * oneMinusCos + cosRadians;
2624  m_Matrix[0][1] = xyMCos - zSin;
2625  m_Matrix[0][2] = xzMCos + ySin;
2626 
2627  m_Matrix[1][0] = xyMCos + zSin;
2628  m_Matrix[1][1] = yy * oneMinusCos + cosRadians;
2629  m_Matrix[1][2] = yzMCos - xSin;
2630 
2631  m_Matrix[2][0] = xzMCos - ySin;
2632  m_Matrix[2][1] = yzMCos + xSin;
2633  m_Matrix[2][2] = zz * oneMinusCos + cosRadians;
2634  }
2635 
2641  {
2642  Matrix3 transpose;
2643 
2644  for (kt_int32u row = 0; row < 3; row++)
2645  {
2646  for (kt_int32u col = 0; col < 3; col++)
2647  {
2648  transpose.m_Matrix[row][col] = m_Matrix[col][row];
2649  }
2650  }
2651 
2652  return transpose;
2653  }
2654 
2660  {
2661  Matrix3 kInverse = *this;
2662  kt_bool haveInverse = InverseFast(kInverse, 1e-14);
2663  if (haveInverse == false)
2664  {
2665  assert(false);
2666  }
2667  return kInverse;
2668  }
2669 
2674  inline const String ToString() const
2675  {
2676  String valueString;
2677 
2678  for (int row = 0; row < 3; row++)
2679  {
2680  for (int col = 0; col < 3; col++)
2681  {
2682  valueString.Append(StringHelper::ToString(m_Matrix[row][col]));
2683  valueString.Append(" ");
2684  }
2685  }
2686 
2687  return valueString;
2688  }
2689 
2690  public:
2694  inline Matrix3& operator = (const Matrix3& rOther)
2695  {
2696  memcpy(m_Matrix, rOther.m_Matrix, 9*sizeof(kt_double));
2697  return *this;
2698  }
2699 
2707  {
2708  return m_Matrix[row][column];
2709  }
2710 
2717  inline kt_double operator()(kt_int32u row, kt_int32u column) const
2718  {
2719  return m_Matrix[row][column];
2720  }
2721 
2725  Matrix3 operator*(const Matrix3& rOther) const
2726  {
2727  Matrix3 product;
2728 
2729  for (size_t row = 0; row < 3; row++)
2730  {
2731  for (size_t col = 0; col < 3; col++)
2732  {
2733  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];
2734  }
2735  }
2736 
2737  return product;
2738  }
2739 
2743  inline Pose2 operator*(const Pose2& rPose2) const
2744  {
2745  Pose2 pose2;
2746 
2747  pose2.SetX(m_Matrix[0][0] * rPose2.GetX() + m_Matrix[0][1] * rPose2.GetY() + m_Matrix[0][2] * rPose2.GetHeading());
2748  pose2.SetY(m_Matrix[1][0] * rPose2.GetX() + m_Matrix[1][1] * rPose2.GetY() + m_Matrix[1][2] * rPose2.GetHeading());
2749  pose2.SetHeading(m_Matrix[2][0] * rPose2.GetX() + m_Matrix[2][1] * rPose2.GetY() + m_Matrix[2][2] * rPose2.GetHeading());
2750 
2751  return pose2;
2752  }
2753 
2757  inline void operator+=(const Matrix3& rkMatrix)
2758  {
2759  for (kt_int32u row = 0; row < 3; row++)
2760  {
2761  for (kt_int32u col = 0; col < 3; col++)
2762  {
2763  m_Matrix[row][col] += rkMatrix.m_Matrix[row][col];
2764  }
2765  }
2766  }
2767 
2771  inline kt_bool operator==(const Matrix3& rkMatrix)
2772  {
2773  for (kt_int32u row = 0; row < 3; row++)
2774  {
2775  for (kt_int32u col = 0; col < 3; col++)
2776  {
2777  if (math::DoubleEqual(m_Matrix[row][col], rkMatrix.m_Matrix[row][col]) == false)
2778  {
2779  return false;
2780  }
2781  }
2782  }
2783 
2784  return true;
2785  }
2786 
2790  friend KARTO_FORCEINLINE std::ostream& operator << (std::ostream& rStream, const Matrix3& rMatrix)
2791  {
2792  rStream << rMatrix.ToString();
2793  return rStream;
2794  }
2795 
2796  private:
2805  kt_bool InverseFast(Matrix3& rkInverse, kt_double fTolerance = KT_TOLERANCE) const
2806  {
2807  // Invert a 3x3 using cofactors. This is about 8 times faster than
2808  // the Numerical Recipes code which uses Gaussian elimination.
2809  rkInverse.m_Matrix[0][0] = m_Matrix[1][1]*m_Matrix[2][2] - m_Matrix[1][2]*m_Matrix[2][1];
2810  rkInverse.m_Matrix[0][1] = m_Matrix[0][2]*m_Matrix[2][1] - m_Matrix[0][1]*m_Matrix[2][2];
2811  rkInverse.m_Matrix[0][2] = m_Matrix[0][1]*m_Matrix[1][2] - m_Matrix[0][2]*m_Matrix[1][1];
2812  rkInverse.m_Matrix[1][0] = m_Matrix[1][2]*m_Matrix[2][0] - m_Matrix[1][0]*m_Matrix[2][2];
2813  rkInverse.m_Matrix[1][1] = m_Matrix[0][0]*m_Matrix[2][2] - m_Matrix[0][2]*m_Matrix[2][0];
2814  rkInverse.m_Matrix[1][2] = m_Matrix[0][2]*m_Matrix[1][0] - m_Matrix[0][0]*m_Matrix[1][2];
2815  rkInverse.m_Matrix[2][0] = m_Matrix[1][0]*m_Matrix[2][1] - m_Matrix[1][1]*m_Matrix[2][0];
2816  rkInverse.m_Matrix[2][1] = m_Matrix[0][1]*m_Matrix[2][0] - m_Matrix[0][0]*m_Matrix[2][1];
2817  rkInverse.m_Matrix[2][2] = m_Matrix[0][0]*m_Matrix[1][1] - m_Matrix[0][1]*m_Matrix[1][0];
2818 
2819  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];
2820 
2821  if (fabs(fDet) <= fTolerance)
2822  {
2823  return false;
2824  }
2825 
2826  kt_double fInvDet = 1.0/fDet;
2827  for (size_t row = 0; row < 3; row++)
2828  {
2829  for (size_t col = 0; col < 3; col++)
2830  {
2831  rkInverse.m_Matrix[row][col] *= fInvDet;
2832  }
2833  }
2834 
2835  return true;
2836  }
2837 
2838  private:
2839  kt_double m_Matrix[3][3];
2840  }; // class Matrix3
2841 
2845 
2846  class Color
2847  {
2848  public:
2853  : m_Red(0.0)
2854  , m_Green(0.0)
2855  , m_Blue(0.0)
2856  , m_Alpha(1.0)
2857  {
2858  }
2859 
2863  Color(const Color& rOther)
2864  : m_Red(rOther.m_Red)
2865  , m_Green(rOther.m_Green)
2866  , m_Blue(rOther.m_Blue)
2867  , m_Alpha(rOther.m_Alpha)
2868  {
2869  }
2870 
2878  Color(kt_double red, kt_double green, kt_double blue, kt_double alpha = 1.0)
2879  : m_Red(red)
2880  , m_Green(green)
2881  , m_Blue(blue)
2882  , m_Alpha(alpha)
2883  {
2884  }
2885 
2893  Color(kt_int8u red, kt_int8u green, kt_int8u blue, kt_int8u alpha = 255)
2894  : m_Red((kt_double)red/255.0)
2895  , m_Green((kt_double)green/255.0)
2896  , m_Blue((kt_double)blue/255.0)
2897  , m_Alpha((kt_double)alpha/255.0)
2898  {
2899  }
2900 
2904  virtual ~Color()
2905  {
2906  }
2907 
2908  public:
2913  const kt_double GetRed() const
2914  {
2915  return m_Red;
2916  }
2917 
2922  void SetRed(kt_double red)
2923  {
2924  m_Red = red;
2925  }
2926 
2931  const kt_double GetGreen() const
2932  {
2933  return m_Green;
2934  }
2935 
2940  void SetGreen(kt_double green)
2941  {
2942  m_Green = green;
2943  }
2944 
2949  const kt_double GetBlue() const
2950  {
2951  return m_Blue;
2952  }
2953 
2958  void SetBlue(kt_double blue)
2959  {
2960  m_Blue = blue;
2961  }
2962 
2967  const kt_double GetAlpha() const
2968  {
2969  return m_Alpha;
2970  }
2971 
2976  void SetAlpha(kt_double alpha)
2977  {
2978  m_Alpha = alpha;
2979  }
2980 
2985  const String ToString() const
2986  {
2987  String valueString;
2988  valueString.Append(StringHelper::ToString(GetRed()));
2989  valueString.Append(" ");
2990  valueString.Append(StringHelper::ToString(GetGreen()));
2991  valueString.Append(" ");
2992  valueString.Append(StringHelper::ToString(GetBlue()));
2993  valueString.Append(" ");
2994  valueString.Append(StringHelper::ToString(GetAlpha()));
2995  return valueString;
2996  }
2997 
2998  public:
3002  inline kt_bool operator == (const Color& rOther) const
3003  {
3004  return (m_Red == rOther.m_Red && m_Green == rOther.m_Green && m_Blue == rOther.m_Blue && m_Alpha == rOther.m_Alpha);
3005  }
3006 
3010  inline kt_bool operator != (const Color& rOther) const
3011  {
3012  return (m_Red != rOther.m_Red || m_Green != rOther.m_Green || m_Blue != rOther.m_Blue || m_Alpha != rOther.m_Alpha);
3013  }
3014 
3018  friend KARTO_FORCEINLINE std::ostream& operator << (std::ostream& rStream, const Color& rColor)
3019  {
3020  rStream << rColor.ToString();
3021  return rStream;
3022  }
3023 
3024  private:
3029  }; // class Color
3030 
3034 
3035  namespace gps
3036  {
3040  class PointGps : public karto::Vector2d
3041  {
3042  public:
3047  {
3048  }
3049 
3055  PointGps(kt_double latitude, kt_double longitude)
3056  {
3057  SetX(longitude);
3058  SetY(latitude);
3059  }
3060 
3064  PointGps(const PointGps& rOther)
3065  {
3066  SetX(rOther.GetLongitude());
3067  SetY(rOther.GetLatitude());
3068  }
3069 
3070  public:
3075  inline kt_double GetLatitude() const
3076  {
3077  return GetY();
3078  }
3079 
3084  inline void SetLatitude(kt_double latitude)
3085  {
3086  SetY(latitude);
3087  }
3088 
3093  inline kt_double GetLongitude() const
3094  {
3095  return GetX();
3096  }
3097 
3102  inline void SetLongitude(kt_double longitude)
3103  {
3104  SetX(longitude);
3105  }
3106 
3113  inline kt_double GetBearing(const PointGps& rOther)
3114  {
3115  kt_double lat1 = math::DegreesToRadians(GetLatitude());
3116  kt_double long1 = math::DegreesToRadians(GetLongitude());
3117  kt_double lat2 = math::DegreesToRadians(rOther.GetLatitude());
3118  kt_double long2 = math::DegreesToRadians(rOther.GetLongitude());
3119 
3120  kt_double deltaLong = long2 - long1;
3121 
3122  kt_double y = sin(deltaLong) * cos(lat2);
3123  kt_double x = cos(lat1) * sin(lat2) - sin(lat1) * cos(lat2) * cos(deltaLong);
3124 
3125  return math::RadiansToDegrees(atan2(y, x));
3126  }
3127 
3132  void AddOffset(const PointGps& rOffset)
3133  {
3134  AddOffset(rOffset.GetLatitude(), rOffset.GetLongitude());
3135  }
3136 
3142  void AddOffset(kt_double latitude, kt_double longitude)
3143  {
3144  SetX(GetLongitude() + longitude);
3145  SetY(GetLatitude() - latitude);
3146  }
3147 
3154  kt_double Distance(const PointGps& rOther)
3155  {
3156  kt_double dlon = rOther.GetLongitude() - GetLongitude();
3157 
3158  const kt_double slat1 = sin(math::DegreesToRadians(GetLatitude()));
3159  const kt_double clat1 = cos(math::DegreesToRadians(GetLatitude()));
3160 
3161  const kt_double slat2 = sin(math::DegreesToRadians(rOther.GetLatitude()));
3162  const kt_double clat2 = cos(math::DegreesToRadians(rOther.GetLatitude()));
3163 
3164  const kt_double sdlon = sin(math::DegreesToRadians(dlon));
3165  const kt_double cdlon = cos(math::DegreesToRadians(dlon));
3166 
3167  const kt_double t1 = clat2 * sdlon;
3168  const kt_double t2 = clat1 * slat2 - slat1 * clat2 * cdlon;
3169  const kt_double t3 = slat1 * slat2 + clat1 * clat2 * cdlon;
3170  const kt_double dist = atan2(sqrt(t1*t1 + t2*t2), t3);
3171 
3172  const kt_double earthRadius = 6372.795;
3173  return dist * earthRadius;
3174  }
3175 
3180  inline const String ToString() const
3181  {
3182  String valueString;
3183  valueString.Append(StringHelper::ToString(GetLatitude()));
3184  valueString.Append(" ");
3185  valueString.Append(StringHelper::ToString(GetLongitude()));
3186  return valueString;
3187  }
3188 
3192  friend KARTO_FORCEINLINE std::ostream& operator << (std::ostream& rStream, const PointGps& rPointGps)
3193  {
3194  rStream << rPointGps.ToString();
3195  return rStream;
3196  }
3197 
3198  public:
3204  inline const PointGps operator + (const PointGps& rOther) const
3205  {
3206  return PointGps(GetLatitude() + rOther.GetLatitude(), GetLongitude() + rOther.GetLongitude());
3207  }
3208 
3214  inline const PointGps operator - (const PointGps& rOther) const
3215  {
3216  return PointGps(GetLatitude() - rOther.GetLatitude(), GetLongitude() - rOther.GetLongitude());
3217  }
3218  };
3219 
3221  }
3222 
3226 
3227  template<typename T>
3229  {
3230  return rValue.ToString();
3231  }
3232 
3233  template<typename T>
3235  {
3236  return rValue.ToString();
3237  }
3238 
3239  template<typename T>
3241  {
3242  return rValue.ToString();
3243  }
3244 
3245  template<typename T>
3247  {
3248  return rValue.ToString();
3249  }
3250 
3251  template<typename T>
3252  inline static kt_bool FromString(const String& rStringValue, Vector3<T>& rValue)
3253  {
3254  karto::String tempString = rStringValue;
3255  kt_size_t index = tempString.FindFirstOf(" ");
3256  if (index != -1)
3257  {
3258  karto::String stringValue;
3259  T value;
3260 
3261  // Get X
3262  stringValue = tempString.SubString(0, index);
3263 
3264  value = 0;
3265  FromString(stringValue, value);
3266  rValue.SetX(value);
3267 
3268  // Get Y
3269  tempString = rStringValue.SubString(index + 1, rStringValue.Size());
3270  index = tempString.FindFirstOf(" ");
3271 
3272  stringValue = tempString.SubString(0, index);
3273 
3274  value = 0;
3275  FromString(stringValue, value);
3276  rValue.SetY(value);
3277 
3278  // Get Z
3279  tempString = rStringValue.SubString(index + 1, rStringValue.Size());
3280  index = tempString.FindFirstOf(" ");
3281 
3282  stringValue = tempString.SubString(index + 1, rStringValue.Size());
3283 
3284  value = 0;
3285  FromString(stringValue, value);
3286  rValue.SetZ(value);
3287 
3288  return true;
3289  }
3290 
3291  return false;
3292  }
3293 
3297 
3299 
3300 }
3301 
3302 #endif // __OpenKarto_Geometry_h__
void Add(const BoundingBox2 &rBoundingBox)
Definition: Geometry.h:1670
kt_bool InverseFast(Matrix3 &rkInverse, kt_double fTolerance=KT_TOLERANCE) const
Definition: Geometry.h:2805
kt_double GetX() const
Definition: Geometry.h:1363
void Append(const String &rString)
Definition: String.cpp:82
T GetHeight() const
Definition: Geometry.h:1911
PointGps(const PointGps &rOther)
Definition: Geometry.h:3064
const Vector2 operator-(const Vector2 &rOther) const
Definition: Geometry.h:491
Vector4(const Vector4 &rOther)
Definition: Geometry.h:1088
void AddOffset(kt_double latitude, kt_double longitude)
Definition: Geometry.h:3142
const Vector2 operator*(T scalar) const
Definition: Geometry.h:524
kt_double Distance(const Vector2 &rOther) const
Definition: Geometry.h:443
Vector3 & operator=(const Vector3 &rOther)
Definition: Geometry.h:806
bool kt_bool
Definition: Types.h:145
Matrix3(const Matrix3 &rOther)
Definition: Geometry.h:2571
void SetSize(const Size2< T > &rSize)
Definition: Geometry.h:1966
Quaternion m_Orientation
Definition: Geometry.h:2547
void SetY(const T &y)
Definition: Geometry.h:699
Vector3 & operator/=(const T &rValue)
Definition: Geometry.h:952
void SetW(const T &w)
Definition: Geometry.h:1164
karto::Vector3d operator*(const karto::Vector3d &rVector) const
Definition: Geometry.h:1558
Vector2d m_Maximum
Definition: Geometry.h:1735
std::size_t kt_size_t
Definition: Types.h:138
kt_double SquaredLength() const
Definition: Geometry.h:757
const Vector3 operator^(const Vector3 &rOther) const
Definition: Geometry.h:966
kt_double GetHeading() const
Definition: Geometry.h:2274
T Square(T value)
Definition: Math.h:104
void MakeCeil(const Vector2 &rOther)
Definition: Geometry.h:397
const String ToString() const
Definition: Geometry.h:452
kt_double & operator()(kt_int32u row, kt_int32u column)
Definition: Geometry.h:2706
void SetWidth(T width)
Definition: Geometry.h:224
void SetWidth(const T &rWidth)
Definition: Geometry.h:1902
Vector2< T > GetTopRight()
Definition: Geometry.h:2056
Vector3(const Vector2< T > &rVector)
Definition: Geometry.h:650
kt_double m_Red
Definition: Geometry.h:3025
void SetX(kt_double x)
Definition: Geometry.h:1372
Pose2 operator-(const Pose2 &rOther) const
Definition: Geometry.h:2373
const Vector3d & GetPosition() const
Definition: Geometry.h:2463
void SetWidth(T width)
Definition: Geometry.h:89
void SetPosition(const T &rX, const T &rY)
Definition: Geometry.h:1939
kt_size_t FindFirstOf(const String &rValue) const
Definition: String.cpp:102
void SetX(const T &rX)
Definition: Geometry.h:1866
void SetMaximum(const Vector2d &rMaximum)
Definition: Geometry.h:1640
Rectangle2(const Vector2< T > &rTopLeft, const Vector2< T > &rBottomRight)
Definition: Geometry.h:1837
Vector3 & operator-=(const Vector3 &rOther)
Definition: Geometry.h:870
const T & GetY() const
Definition: Geometry.h:369
#define KARTO_EXPORT
Definition: Macros.h:78
Vector2< T > GetAsVector2() const
Definition: Geometry.h:726
const String ToString() const
Definition: Geometry.cpp:192
kt_double GetX() const
Definition: Geometry.h:2220
List< PointGps > PointGpsList
Definition: Geometry.h:3220
void SetX(const T &x)
Definition: Geometry.h:360
void operator*=(T scalar)
Definition: Geometry.h:540
void SetTop(const T &rTop)
Definition: Geometry.h:2002
Vector3< kt_int32u > Vector3iu
Definition: Geometry.h:1041
const T & GetX() const
Definition: Geometry.h:1101
String SubString(kt_size_t index) const
Definition: String.cpp:87
Size2(const Size2 &rOther)
Definition: Geometry.h:69
kt_double m_Green
Definition: Geometry.h:3026
kt_bool operator!=(const Size3 &rOther) const
Definition: Geometry.h:304
kt_bool operator==(const Vector4 &rOther) const
Definition: Geometry.h:1241
void SetZ(const T &z)
Definition: Geometry.h:1146
kt_double AngleTo(const Vector2d &rVector) const
Definition: Geometry.h:2303
T GetDepth() const
Definition: Geometry.h:251
Size2< T > m_Size
Definition: Geometry.h:2170
T GetWidth() const
Definition: Geometry.h:1893
kt_bool operator!=(const Pose2 &rOther) const
Definition: Geometry.h:2348
Vector3(const Vector3 &rOther)
Definition: Geometry.h:660
void SetY(const T &y)
Definition: Geometry.h:1128
Vector3 & operator-=(const T &rValue)
Definition: Geometry.h:926
friend KARTO_FORCEINLINE std::ostream & operator<<(std::ostream &rStream, const Size2 &rSize)
Definition: Geometry.h:156
kt_bool operator!=(const Vector2 &rOther) const
Definition: Geometry.h:557
void operator+=(const Vector2 &rOther)
Definition: Geometry.h:465
kt_double m_Alpha
Definition: Geometry.h:3028
const Vector3 operator+(const Vector3 &rOther) const
Definition: Geometry.h:818
kt_bool operator!=(const Size2 &rOther) const
Definition: Geometry.h:148
Matrix3 Transpose() const
Definition: Geometry.h:2640
const Vector3d & GetMinimum() const
Definition: Geometry.h:1752
void SetMinimum(const Vector3d &rMinimum)
Definition: Geometry.h:1761
void SetW(kt_double w)
Definition: Geometry.h:1426
TFSIMD_FORCE_INLINE const tfScalar & y() const
kt_double Length() const
Definition: Geometry.h:423
T GetTop() const
Definition: Geometry.h:1993
kt_bool operator!=(const Vector3 &rOther) const
Definition: Geometry.h:1015
Vector3 & operator+=(const T &rValue)
Definition: Geometry.h:912
void SetToIdentity()
Definition: Geometry.h:2580
Rectangle2(const Rectangle2 &rOther)
Definition: Geometry.h:1846
kt_double GetZ() const
Definition: Geometry.h:1399
const Vector2< T > GetCenter() const
Definition: Geometry.h:2083
kt_double Distance(const PointGps &rOther)
Definition: Geometry.h:3154
TFSIMD_FORCE_INLINE tfScalar angle(const Quaternion &q1, const Quaternion &q2)
Vector2< T > m_Position
Definition: Geometry.h:2169
void SetPosition(const Vector2< T > &rPosition)
Definition: Geometry.h:1948
Size3(T width, T height, T depth)
Definition: Geometry.h:194
uint8_t kt_int8u
Definition: Types.h:91
const T & GetX() const
Definition: Geometry.h:351
void SetLeft(const T &rLeft)
Definition: Geometry.h:1984
kt_double operator*(const Vector2 &rOther) const
Definition: Geometry.h:516
kt_bool Contains(const Rectangle2< T > &rOther)
Definition: Geometry.h:2093
const String ToString() const
Definition: Geometry.h:791
Vector2< kt_int32u > Vector2iu
Definition: Geometry.h:605
Vector2d m_Minimum
Definition: Geometry.h:1734
T GetHeight() const
Definition: Geometry.h:233
Vector2(T x, T y)
Definition: Geometry.h:340
Pose3(const Pose3 &rOther)
Definition: Geometry.h:2442
Size3(const Size3 &rOther)
Definition: Geometry.h:204
kt_bool operator==(const Pose2 &rOther) const
Definition: Geometry.h:2340
#define KARTO_FORCEINLINE
Definition: Macros.h:56
const kt_double GetRed() const
Definition: Geometry.h:2913
const T & GetY() const
Definition: Geometry.h:1119
Vector2< T > GetBottomRight()
Definition: Geometry.h:2074
Vector4(T x, T y, T z, T w)
Definition: Geometry.h:1077
Vector3d m_Position
Definition: Geometry.h:2546
kt_bool operator==(const Vector2 &rOther) const
Definition: Geometry.h:549
friend KARTO_FORCEINLINE std::ostream & operator<<(std::ostream &rStream, const Quaternion &rQuaternion)
Definition: Geometry.h:1576
kt_double SquaredDistance(const Pose2 &rOther) const
Definition: Geometry.h:2293
uint32_t kt_int32u
Definition: Types.h:111
T GetY() const
Definition: Geometry.h:1875
Quaternion(kt_double x, kt_double y, kt_double z, kt_double w)
Definition: Geometry.h:1328
kt_double GetBearing(const PointGps &rOther)
Definition: Geometry.h:3113
const kt_double GetGreen() const
Definition: Geometry.h:2931
void SetHeight(T height)
Definition: Geometry.h:242
const T & GetY() const
Definition: Geometry.h:690
void Clear()
Definition: Geometry.h:2593
void SetX(const T &x)
Definition: Geometry.h:681
void SetPosition(const Vector2d &rPosition)
Definition: Geometry.h:2265
Vector2< T > GetTopLeft()
Definition: Geometry.h:2047
kt_bool operator!=(const Vector4 &rOther) const
Definition: Geometry.h:1249
kt_bool operator<(const Vector3 &rOther) const
Definition: Geometry.h:980
Quaternion(const Quaternion &rOther)
Definition: Geometry.h:1350
Size2< kt_double > GetSize() const
Definition: Geometry.h:1649
Vector3 & operator*=(const Vector3 &rOther)
Definition: Geometry.h:884
void SetBlue(kt_double blue)
Definition: Geometry.h:2958
void SetGreen(kt_double green)
Definition: Geometry.h:2940
void SetRight(const T &rRight)
Definition: Geometry.h:2020
T GetLeft() const
Definition: Geometry.h:1975
void operator+=(const Pose2 &rOther)
Definition: Geometry.h:2356
Color(kt_double red, kt_double green, kt_double blue, kt_double alpha=1.0)
Definition: Geometry.h:2878
void SetPosition(const Vector3d &rPosition)
Definition: Geometry.h:2472
void SetY(const T &rY)
Definition: Geometry.h:1884
const String ToString() const
Definition: Geometry.h:1173
const kt_double GetAlpha() const
Definition: Geometry.h:2967
void MakeFloor(const Vector2 &rOther)
Definition: Geometry.h:387
void operator/=(T scalar)
Definition: Geometry.h:499
Vector3 & operator/=(const Vector3 &rOther)
Definition: Geometry.h:898
const Vector2 operator/(T scalar) const
Definition: Geometry.h:508
static String ToString(const char *value)
void Normalize()
Definition: Geometry.h:774
const kt_double KT_TOLERANCE
Definition: Math.h:71
Pose3(const Vector3d &rPosition)
Definition: Geometry.h:2423
T GetWidth() const
Definition: Geometry.h:215
kt_double GetLatitude() const
Definition: Geometry.h:3075
kt_bool operator==(const Size3 &rOther) const
Definition: Geometry.h:296
Pose2 operator+(const Pose2 &rOther) const
Definition: Geometry.h:2365
Vector4< kt_double > Vector4d
Definition: Geometry.h:1280
Vector3< kt_int32s > Vector3i
Definition: Geometry.h:1036
TFSIMD_FORCE_INLINE const tfScalar & x() const
const String ToString() const
Definition: Geometry.h:3180
Rectangle2(const Vector2< T > &rPosition, const Size2< T > &rSize)
Definition: Geometry.h:1826
Size2(T width, T height)
Definition: Geometry.h:60
kt_bool operator<(const Vector2 &rOther) const
Definition: Geometry.h:568
friend KARTO_FORCEINLINE std::ostream & operator<<(std::ostream &rStream, const Vector4 &rVector)
Definition: Geometry.h:1257
void SetLatitude(kt_double latitude)
Definition: Geometry.h:3084
const Vector2d & GetMinimum() const
Definition: Geometry.h:1613
const Vector3 operator-(kt_double scalar) const
Definition: Geometry.h:842
kt_double SquaredDistance(const Vector2 &rOther) const
Definition: Geometry.h:433
const Quaternion operator*(const Quaternion &rOther) const
Definition: Geometry.h:1528
const karto::String ToString() const
Definition: Geometry.h:269
kt_bool operator==(const Quaternion &rOther) const
Definition: Geometry.h:1540
Vector3< kt_double > Vector3d
Definition: Geometry.h:1046
kt_double GetY() const
Definition: Geometry.h:1381
const String ToString() const
Definition: Geometry.h:2499
int32_t kt_int32s
Definition: Types.h:106
T GetX() const
Definition: Geometry.h:1857
void operator-=(const Vector2 &rOther)
Definition: Geometry.h:474
void SetMaximum(const Vector3d &rMaximum)
Definition: Geometry.h:1779
const Vector2d & GetPosition() const
Definition: Geometry.h:2256
const T & GetZ() const
Definition: Geometry.h:1137
const Vector2< T > & GetPosition() const
Definition: Geometry.h:1929
Vector3d m_Minimum
Definition: Geometry.h:1785
kt_bool Contains(const BoundingBox2 &rOther) const
Definition: Geometry.h:1712
const Vector2 operator-(T scalar) const
Definition: Geometry.h:532
static kt_bool FromString(const String &rStringValue, Vector3< T > &rValue)
Definition: Geometry.h:3252
const Vector4d GetAsVector4() const
Definition: Geometry.h:1435
void SetHeight(const T &rHeight)
Definition: Geometry.h:1920
kt_bool operator==(const Vector3 &rOther) const
Definition: Geometry.h:1007
void SetHeight(T height)
Definition: Geometry.h:107
const Vector2 operator+(const Vector2 &rOther) const
Definition: Geometry.h:483
kt_double operator()(kt_int32u row, kt_int32u column) const
Definition: Geometry.h:2717
Pose3(const Vector3d &rPosition, const karto::Quaternion &rOrientation)
Definition: Geometry.h:2433
Color(kt_int8u red, kt_int8u green, kt_int8u blue, kt_int8u alpha=255)
Definition: Geometry.h:2893
Vector3 & operator+=(const Vector3 &rOther)
Definition: Geometry.h:858
Quaternion & operator=(const Quaternion &rOther)
Definition: Geometry.h:1515
void SetBottom(const T &rBottom)
Definition: Geometry.h:2038
void operator+=(const Matrix3 &rkMatrix)
Definition: Geometry.h:2757
const Vector3d & GetMaximum() const
Definition: Geometry.h:1770
Vector2< kt_double > Vector2d
Definition: Geometry.h:610
kt_bool operator<(const Vector4 &rOther) const
Definition: Geometry.h:1206
kt_double GetW() const
Definition: Geometry.h:1417
kt_bool operator!=(const Quaternion &rOther) const
Definition: Geometry.h:1548
Quaternion(const Vector4d &rVector)
Definition: Geometry.h:1339
double kt_double
Definition: Types.h:160
kt_double DegreesToRadians(kt_double degrees)
Definition: Math.h:83
void SetZ(kt_double z)
Definition: Geometry.h:1408
kt_double m_Matrix[3][3]
Definition: Geometry.h:2839
kt_double NormalizeAngle(kt_double angle)
Definition: Math.h:213
kt_bool InRange(const T &value, const T &a, const T &b)
Definition: Math.h:203
void SetY(kt_double y)
Definition: Geometry.h:1390
void SetX(const T &x)
Definition: Geometry.h:1110
const Vector3 operator-(const Vector3 &rOther) const
Definition: Geometry.h:834
kt_bool Contains(const Vector2d &rPoint) const
Definition: Geometry.h:1681
kt_bool operator==(const Size2 &rOther) const
Definition: Geometry.h:140
Rectangle2(T x, T y, T width, T height)
Definition: Geometry.h:1815
void SetAlpha(kt_double alpha)
Definition: Geometry.h:2976
void MakeFloor(const Vector3 &rOther)
Definition: Geometry.h:735
kt_bool DoubleEqual(kt_double a, kt_double b)
Definition: Math.h:162
kt_double SquaredLength() const
Definition: Geometry.h:414
kt_double Length() const
Definition: Geometry.h:766
const karto::String ToString() const
Definition: Geometry.h:116
kt_double GetLongitude() const
Definition: Geometry.h:3093
kt_double m_Values[4]
Definition: Geometry.h:1583
const String ToString(kt_int32u precision=4) const
Definition: Geometry.h:2314
const Quaternion & GetOrientation() const
Definition: Geometry.h:2481
const Vector3 operator*(T scalar) const
Definition: Geometry.h:850
kt_double GetY() const
Definition: Geometry.h:2238
const T GetWidth() const
Definition: Geometry.h:80
Pose3(const Pose2 &rPose)
Definition: Geometry.h:2452
void SetHeading(kt_double heading)
Definition: Geometry.h:2283
T GetBottom() const
Definition: Geometry.h:2029
TFSIMD_FORCE_INLINE tfScalar length(const Quaternion &q)
void SetX(kt_double x)
Definition: Geometry.h:2229
friend KARTO_FORCEINLINE std::ostream & operator<<(std::ostream &rStream, const Vector2 &rVector)
Definition: Geometry.h:587
Vector2d m_Position
Definition: Geometry.h:2388
Definition: Any.cpp:20
Vector2< T > GetBottomLeft()
Definition: Geometry.h:2065
const char * ToCString() const
Definition: String.cpp:72
Matrix3 operator*(const Matrix3 &rOther) const
Definition: Geometry.h:2725
const T & GetW() const
Definition: Geometry.h:1155
void SetDepth(T depth)
Definition: Geometry.h:260
Vector3(T x, T y, T z)
Definition: Geometry.h:639
Vector3d m_Maximum
Definition: Geometry.h:1786
const T & GetX() const
Definition: Geometry.h:672
kt_double Norm() const
Definition: Geometry.h:1483
kt_double Length() const
Definition: Geometry.h:1474
void SetLongitude(kt_double longitude)
Definition: Geometry.h:3102
const T & GetZ() const
Definition: Geometry.h:708
Length
const Vector2d & GetMaximum() const
Definition: Geometry.h:1631
void SetOrientation(const Quaternion &rOrientation)
Definition: Geometry.h:2490
void SetRed(kt_double red)
Definition: Geometry.h:2922
Vector2< kt_int32s > Vector2i
Definition: Geometry.h:600
Size2 & operator=(const Size2 &rOther)
Definition: Geometry.h:129
kt_bool Intersects(const BoundingBox2 &rOther) const
Definition: Geometry.h:1692
void SetZ(const T &z)
Definition: Geometry.h:717
Matrix3 Inverse() const
Definition: Geometry.h:2659
const Vector3 operator+(kt_double scalar) const
Definition: Geometry.h:826
Color(const Color &rOther)
Definition: Geometry.h:2863
const T GetHeight() const
Definition: Geometry.h:98
kt_double m_Heading
Definition: Geometry.h:2390
kt_size_t Size() const
Definition: String.cpp:77
const String ToString() const
Definition: Geometry.h:2985
T GetRight() const
Definition: Geometry.h:2011
const kt_double GetBlue() const
Definition: Geometry.h:2949
virtual ~Color()
Definition: Geometry.h:2904
void SetY(const T &y)
Definition: Geometry.h:378
List< Pose2 > Pose2List
Definition: Geometry.h:2396
void SetMinimum(const Vector2d &rMinimum)
Definition: Geometry.h:1622
const String ToString() const
Definition: Geometry.h:2674
void AddOffset(const PointGps &rOffset)
Definition: Geometry.h:3132
Vector4< kt_int32s > Vector4i
Definition: Geometry.h:1270
void Add(const Vector2d &rPoint)
Definition: Geometry.h:1660
kt_bool operator==(const Matrix3 &rkMatrix)
Definition: Geometry.h:2771
void SetY(kt_double y)
Definition: Geometry.h:2247
kt_double m_Blue
Definition: Geometry.h:3027
Size3 & operator=(const Size3 &rOther)
Definition: Geometry.h:284
Pose2 operator*(const Pose2 &rPose2) const
Definition: Geometry.h:2743
Vector4< kt_int32u > Vector4iu
Definition: Geometry.h:1275
kt_double RadiansToDegrees(kt_double radians)
Definition: Math.h:93
PointGps(kt_double latitude, kt_double longitude)
Definition: Geometry.h:3055
const Size2< T > & GetSize() const
Definition: Geometry.h:1957
void FromAxisAngle(kt_double x, kt_double y, kt_double z, const kt_double radians)
Definition: Geometry.h:2605
void MakeCeil(const Vector3 &rOther)
Definition: Geometry.h:746
Vector3 & operator*=(const T &rValue)
Definition: Geometry.h:938


nav2d_karto
Author(s): Sebastian Kasperski
autogenerated on Thu Jun 6 2019 19:20:24