Geometry.h

Go to the documentation of this file.

/*
 * Copyright (C) 2006-2011, SRI International (R)
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

#pragma once

#ifndef __OpenKarto_Geometry_h__
#define __OpenKarto_Geometry_h__

#include <OpenKarto/List.h>
#include <OpenKarto/StringHelper.h>
#include <OpenKarto/Math.h>

#include <string.h>

namespace karto
{




  template<typename T>
  class Size2
  {
  public:
    Size2()
      : m_Width(0)
      , m_Height(0)
    {
    }

    Size2(T width, T height)
      : m_Width(width)
      , m_Height(height)
    {
    }

    Size2(const Size2& rOther)
      : m_Width(rOther.m_Width)
      , m_Height(rOther.m_Height)
    {
    }

  public:
    inline const T GetWidth() const
    {
      return m_Width;
    }

    inline void SetWidth(T width)
    {
      m_Width = width;
    }

    inline const T GetHeight() const
    {
      return m_Height;
    }

    inline void SetHeight(T height)
    {
      m_Height = height;
    }

    inline const karto::String ToString() const
    {
      String valueString;
      valueString.Append(StringHelper::ToString(GetWidth()));
      valueString.Append(" ");
      valueString.Append(StringHelper::ToString(GetHeight()));
      return valueString;
    }

  public:
    inline Size2& operator=(const Size2& rOther)
    {
      m_Width = rOther.m_Width;
      m_Height = rOther.m_Height;

      return(*this);
    }

    inline kt_bool operator==(const Size2& rOther) const
    {
      return (m_Width == rOther.m_Width && m_Height == rOther.m_Height);
    }

    inline kt_bool operator!=(const Size2& rOther) const
    {
      return (m_Width != rOther.m_Width || m_Height != rOther.m_Height);
    }

    friend KARTO_FORCEINLINE std::ostream& operator << (std::ostream& rStream, const Size2& rSize)
    {
      rStream << rSize.ToString().ToCString();
      return rStream;
    }

  private:
    T m_Width;
    T m_Height;
  }; // class Size2<T>


  template<typename T>
  class Size3
  {
  public:
    inline Size3()
      : m_Width(0.0)
      , m_Height(0.0)
      , m_Depth(0.0)
    {
    }

    inline Size3(T width, T height, T depth)
      : m_Width(width)
      , m_Height(height)
      , m_Depth(depth)
    {
    }           

    inline Size3(const Size3& rOther)
      : m_Width(rOther.m_Width)
      , m_Height(rOther.m_Height)
    {
    }

  public:
    inline T GetWidth() const
    {
      return m_Width;
    }
    
    inline void SetWidth(T width)
    {
      m_Width = width;
    }

    inline T GetHeight() const
    {
      return m_Height;
    }
    
    inline void SetHeight(T height)
    {
      m_Height = height;
    }

    inline T GetDepth() const
    {
      return m_Depth;
    }

    inline void SetDepth(T depth)
    {
      m_Depth = depth;
    }

    inline const karto::String ToString() const
    {
      String valueString;
      valueString.Append(StringHelper::ToString(GetWidth()));
      valueString.Append(" ");
      valueString.Append(StringHelper::ToString(GetHeight()));
      valueString.Append(" ");
      valueString.Append(StringHelper::ToString(GetDepth()));
      return valueString;
    }

  public:
    inline Size3& operator=(const Size3& rOther)
    {
      m_Width = rOther.m_Width;
      m_Height = rOther.m_Height;
      m_Depth = rOther.m_Depth;

      return *this;
    }

    kt_bool operator==(const Size3& rOther) const 
    {
      return m_Width == rOther.m_Width && m_Height == rOther.m_Height && m_Depth == rOther.m_Depth; 
    }
    
    kt_bool operator!=(const Size3& rOther) const 
    {
      return m_Width != rOther.m_Width || m_Height != rOther.m_Height || m_Depth != rOther.m_Depth; 
    }

  private:
    T m_Width;
    T m_Height;
    T m_Depth;
  }; // class Size3


  template<typename T>
  class Vector2
  {
  public:
    Vector2()
    {
      m_Values[0] = 0;
      m_Values[1] = 0;
    }
    
    Vector2(T x, T y)
    {
      m_Values[0] = x;
      m_Values[1] = y;
    }
    
  public:
    inline const T& GetX() const
    {
      return m_Values[0];
    }
    
    inline void SetX(const T& x)
    {
      m_Values[0] = x;
    }
    
    inline const T& GetY() const
    {
      return m_Values[1];
    }
    
    inline void SetY(const T& y)
    {
      m_Values[1] = y;
    }
        
    inline void MakeFloor(const Vector2& rOther)
    {
      if (rOther.m_Values[0] < m_Values[0]) m_Values[0] = rOther.m_Values[0];
      if (rOther.m_Values[1] < m_Values[1]) m_Values[1] = rOther.m_Values[1];
    }
    
    inline void MakeCeil(const Vector2& rOther)
    {
      if (rOther.m_Values[0] > m_Values[0])
      {
        m_Values[0] = rOther.m_Values[0];
      }
      
      if (rOther.m_Values[1] > m_Values[1])
      {
        m_Values[1] = rOther.m_Values[1];
      }
    }
    
    inline kt_double SquaredLength() const
    {
      return math::Square(m_Values[0]) + math::Square(m_Values[1]);
    }
    
    inline kt_double Length() const
    {
      return sqrt(SquaredLength());
    }
    
    inline kt_double SquaredDistance(const Vector2& rOther) const
    {
      return (*this - rOther).SquaredLength();
    }
    
    inline kt_double Distance(const Vector2& rOther) const
    {
      return sqrt(SquaredDistance(rOther));
    }
    
    inline const String ToString() const
    {
      String valueString;
      valueString.Append(StringHelper::ToString(GetX()));
      valueString.Append(" ");
      valueString.Append(StringHelper::ToString(GetY()));
      return valueString;
    }

  public:
    inline void operator+=(const Vector2& rOther)
    {
      m_Values[0] += rOther.m_Values[0];
      m_Values[1] += rOther.m_Values[1];
    }
    
    inline void operator-=(const Vector2& rOther)
    {
      m_Values[0] -= rOther.m_Values[0];
      m_Values[1] -= rOther.m_Values[1];
    }

    inline const Vector2 operator+(const Vector2& rOther) const
    {
      return Vector2(m_Values[0] + rOther.m_Values[0], m_Values[1] + rOther.m_Values[1]);
    }
    
    inline const Vector2 operator-(const Vector2& rOther) const
    {
      return Vector2(m_Values[0] - rOther.m_Values[0], m_Values[1] - rOther.m_Values[1]);
    }
    
    inline void operator/=(T scalar)
    {
      m_Values[0] /= scalar;
      m_Values[1] /= scalar;
    }
    
    inline const Vector2 operator/(T scalar) const
    {
      return Vector2(m_Values[0] / scalar, m_Values[1] / scalar);
    }
        
    inline kt_double operator*(const Vector2& rOther) const
    {
      return m_Values[0] * rOther.m_Values[0] + m_Values[1] * rOther.m_Values[1];
    }    
    
    inline const Vector2 operator*(T scalar) const
    {
      return Vector2(m_Values[0] * scalar, m_Values[1] * scalar);
    }
    
    inline const Vector2 operator-(T scalar) const
    {
      return Vector2(m_Values[0] - scalar, m_Values[1] - scalar);
    }

    inline void operator*=(T scalar)
    {
      m_Values[0] *= scalar;
      m_Values[1] *= scalar;
    }
    
    inline kt_bool operator==(const Vector2& rOther) const
    {
      return (m_Values[0] == rOther.m_Values[0] && m_Values[1] == rOther.m_Values[1]);
    }
    
    inline kt_bool operator!=(const Vector2& rOther) const
    {
      return (m_Values[0] != rOther.m_Values[0] || m_Values[1] != rOther.m_Values[1]);
    }
    
    inline kt_bool operator<(const Vector2& rOther) const
    {
      if (m_Values[0] < rOther.m_Values[0])
      {
        return true;
      }
      else if (m_Values[0] > rOther.m_Values[0])
      {
        return false;
      }
      else 
      {
        return (m_Values[1] < rOther.m_Values[1]);
      }
    }

    friend KARTO_FORCEINLINE std::ostream& operator<<(std::ostream& rStream, const Vector2& rVector)
    {
      rStream << rVector.ToString().ToCString();
      return rStream;
    }
    
  private:
    T m_Values[2];
  }; // class Vector2<T>

  typedef Vector2<kt_int32s> Vector2i;

  typedef Vector2<kt_int32u> Vector2iu;

  typedef Vector2<kt_double> Vector2d;
  

  template<typename T>
  class Vector3
  {
  public:
    Vector3()
    {
      m_Values[0] = 0;
      m_Values[1] = 0;
      m_Values[2] = 0;
    }

    Vector3(T x, T y, T z)
    {
      m_Values[0] = x;
      m_Values[1] = y;
      m_Values[2] = z;
    }

    Vector3(const Vector2<T>& rVector)
    {
      m_Values[0] = rVector.GetX();
      m_Values[1] = rVector.GetY();
      m_Values[2] = 0.0;
    }

    Vector3(const Vector3& rOther)
    {
      m_Values[0] = rOther.m_Values[0];
      m_Values[1] = rOther.m_Values[1];
      m_Values[2] = rOther.m_Values[2];
    }

  public:
    inline const T& GetX() const 
    {
      return m_Values[0]; 
    }

    inline void SetX(const T& x)
    {
      m_Values[0] = x; 
    }

    inline const T& GetY() const 
    {
      return m_Values[1]; 
    }

    inline void SetY(const T& y)
    {
      m_Values[1] = y; 
    }

    inline const T& GetZ() const
    {
      return m_Values[2]; 
    }

    inline void SetZ(const T& z)
    {
      m_Values[2] = z; 
    } 

    inline Vector2<T> GetAsVector2() const
    {
      return Vector2<T>(m_Values[0], m_Values[1]);
    }

    inline void MakeFloor(const Vector3& rOther)
    {
      if (rOther.m_Values[0] < m_Values[0]) m_Values[0] = rOther.m_Values[0];
      if (rOther.m_Values[1] < m_Values[1]) m_Values[1] = rOther.m_Values[1];
      if (rOther.m_Values[2] < m_Values[2]) m_Values[2] = rOther.m_Values[2];
    }

    inline void MakeCeil(const Vector3& rOther)
    {
      if (rOther.m_Values[0] > m_Values[0]) m_Values[0] = rOther.m_Values[0];
      if (rOther.m_Values[1] > m_Values[1]) m_Values[1] = rOther.m_Values[1];
      if (rOther.m_Values[2] > m_Values[2]) m_Values[2] = rOther.m_Values[2];
    }

    inline kt_double SquaredLength() const
    {
      return math::Square(m_Values[0]) + math::Square(m_Values[1]) + math::Square(m_Values[2]);
    }

    inline kt_double Length() const
    {
      return sqrt(SquaredLength());
    }
    
    inline void Normalize()
    {
      kt_double length = Length();
      if (length > 0.0)
      {
        kt_double inversedLength = 1.0 / length;

        m_Values[0] *= inversedLength;
        m_Values[1] *= inversedLength;
        m_Values[2] *= inversedLength;
      }
    }

    inline const String ToString() const
    {
      String valueString;
      valueString.Append(StringHelper::ToString(GetX()));
      valueString.Append(" ");
      valueString.Append(StringHelper::ToString(GetY()));
      valueString.Append(" ");
      valueString.Append(StringHelper::ToString(GetZ()));
      return valueString;
    }

  public:
    inline Vector3& operator=(const Vector3& rOther)
    {
      m_Values[0] = rOther.m_Values[0];
      m_Values[1] = rOther.m_Values[1];
      m_Values[2] = rOther.m_Values[2];

      return *this;
    }

    inline const Vector3 operator+(const Vector3& rOther) const
    {
      return Vector3(m_Values[0] + rOther.m_Values[0], m_Values[1] + rOther.m_Values[1], m_Values[2] + rOther.m_Values[2]);
    }

    inline const Vector3 operator+(kt_double scalar) const
    {
      return Vector3(m_Values[0] + scalar, m_Values[1] + scalar, m_Values[2] + scalar);
    }

    inline const Vector3 operator-(const Vector3& rOther) const
    {
      return Vector3(m_Values[0] - rOther.m_Values[0], m_Values[1] - rOther.m_Values[1], m_Values[2] - rOther.m_Values[2]);
    }

    inline const Vector3 operator-(kt_double scalar) const
    {
      return Vector3(m_Values[0] - scalar, m_Values[1] - scalar, m_Values[2] - scalar);
    }

    inline const Vector3 operator*(T scalar) const
    {
      return Vector3(m_Values[0] * scalar, m_Values[1] * scalar, m_Values[2] * scalar);
    }

    inline Vector3& operator+=(const Vector3& rOther)
    {
      m_Values[0] += rOther.m_Values[0];
      m_Values[1] += rOther.m_Values[1];
      m_Values[2] += rOther.m_Values[2];

      return *this;
    }

    inline Vector3& operator-=(const Vector3& rOther)
    {
      m_Values[0] -= rOther.m_Values[0];
      m_Values[1] -= rOther.m_Values[1];
      m_Values[2] -= rOther.m_Values[2];

      return *this;
    }

    inline Vector3& operator*=(const Vector3& rOther)
    {
      m_Values[0] *= rOther.m_Values[0];
      m_Values[1] *= rOther.m_Values[1];
      m_Values[2] *= rOther.m_Values[2];

      return *this;
    }

    inline Vector3& operator/=(const Vector3& rOther)
    {
      m_Values[0] /= rOther.m_Values[0];
      m_Values[1] /= rOther.m_Values[1];
      m_Values[2] /= rOther.m_Values[2];

      return *this;
    }

    inline Vector3& operator+=(const T& rValue)
    {
      m_Values[0] += rValue;
      m_Values[1] += rValue;
      m_Values[2] += rValue;

      return *this;
    }

    inline Vector3& operator-=(const T& rValue)
    {
      m_Values[0] -= rValue;
      m_Values[1] -= rValue;
      m_Values[2] -= rValue;

      return *this;
    }

    inline Vector3& operator*=(const T& rValue)
    {
      m_Values[0] *= rValue;
      m_Values[1] *= rValue;
      m_Values[2] *= rValue;

      return *this;
    }

    inline Vector3& operator/=(const T& rValue)
    {
      m_Values[0] /= rValue;
      m_Values[1] /= rValue;
      m_Values[2] /= rValue;

      return *this;
    }

    inline const Vector3 operator^(const Vector3& rOther) const
    {
      return Vector3(
        m_Values[1] * rOther.m_Values[2] - m_Values[2] * rOther.m_Values[1],
        m_Values[2] * rOther.m_Values[0] - m_Values[0] * rOther.m_Values[2] ,
        m_Values[0] * rOther.m_Values[1] - m_Values[1] * rOther.m_Values[0]);
    }

    inline kt_bool operator<(const Vector3& rOther) const
    {
      if (m_Values[0] < rOther.m_Values[0])
      {
        return true;
      }
      else if (m_Values[0] > rOther.m_Values[0]) 
      {
        return false;
      }
      else if (m_Values[1] < rOther.m_Values[1])
      {
        return true;
      }
      else if (m_Values[1] > rOther.m_Values[1]) 
      {
        return false;
      }
      else
      {
        return (m_Values[2] < rOther.m_Values[2]);
      }
    }

    inline kt_bool operator==(const Vector3& rOther) const
    {
      return (m_Values[0] == rOther.m_Values[0] && m_Values[1] == rOther.m_Values[1] && m_Values[2] == rOther.m_Values[2]);
    }

    inline kt_bool operator!=(const Vector3& rOther) const
    {
      return (m_Values[0] != rOther.m_Values[0] || m_Values[1] != rOther.m_Values[1] || m_Values[2] != rOther.m_Values[2]);
    }

    friend KARTO_FORCEINLINE std::ostream& operator << (std::ostream& rStream, const Vector3& rVector)
    {
      rStream << rVector.ToString().ToCString();
      return rStream;
    }

  private:
    T m_Values[3];
  }; // class Vector3<T>

  typedef Vector3<kt_int32s> Vector3i;

  typedef Vector3<kt_int32u> Vector3iu;

  typedef Vector3<kt_double> Vector3d;


  template<typename T>
  class Vector4
  {
  public:
    Vector4()
    {
      m_Values[0] = 0;
      m_Values[1] = 0;
      m_Values[2] = 0;
      m_Values[3] = 0;
    }

    Vector4(T x, T y, T z, T w)
    {
      m_Values[0] = x;
      m_Values[1] = y;
      m_Values[2] = z;
      m_Values[3] = w;
    }

    Vector4(const Vector4& rOther)
    {
      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];
    }

  public:
    inline const T& GetX() const 
    {
      return m_Values[0]; 
    }

    inline void SetX(const T& x)
    {
      m_Values[0] = x; 
    }

    inline const T& GetY() const 
    {
      return m_Values[1]; 
    }

    inline void SetY(const T& y)
    {
      m_Values[1] = y;
    }

    inline const T& GetZ() const
    {
      return m_Values[2];
    }

    inline void SetZ(const T& z)
    {
      m_Values[2] = z;
    } 

    inline const T& GetW() const
    {
      return m_Values[3];
    }

    inline void SetW(const T& w)
    {
      m_Values[3] = w;
    } 

    inline const String ToString() const
    {
      String valueString;
      valueString.Append(StringHelper::ToString(GetX()));
      valueString.Append(" ");
      valueString.Append(StringHelper::ToString(GetY()));
      valueString.Append(" ");
      valueString.Append(StringHelper::ToString(GetZ()));
      valueString.Append(" ");
      valueString.Append(StringHelper::ToString(GetW()));
      return valueString;
    }

  public:
    inline Vector4& operator = (const Vector4& rOther)
    {
      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];

      return *this;
    }

    inline kt_bool operator<(const Vector4& rOther) const
    {
      if (m_Values[0] < rOther.m_Values[0])
      {
        return true;
      }
      else if (m_Values[0] > rOther.m_Values[0]) 
      {
        return false;
      }
      else if (m_Values[1] < rOther.m_Values[1])
      {
        return true;
      }
      else if (m_Values[1] > rOther.m_Values[1]) 
      {
        return false;
      }
      else if (m_Values[2] < rOther.m_Values[2])
      {
        return true;
      }
      else if (m_Values[2] > rOther.m_Values[2])
      {
        return false;
      }
      else
      {
        return (m_Values[3] < rOther.m_Values[3]);
      }
    }

    inline kt_bool operator==(const Vector4& rOther) const
    {
      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]);
    }

    inline kt_bool operator!=(const Vector4& rOther) const
    {
      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]);
    }

    friend KARTO_FORCEINLINE std::ostream& operator<<(std::ostream& rStream, const Vector4& rVector)
    {
      rStream << rVector.ToString().ToCString();
      return rStream;
    }

  private:
    T m_Values[4];
  }; // class Vector4<T>

  typedef Vector4<kt_int32s> Vector4i;

  typedef Vector4<kt_int32u> Vector4iu;

  typedef Vector4<kt_double> Vector4d;


  class KARTO_EXPORT Quaternion
  {
  public:
    inline Quaternion()
    {
      m_Values[0] = 0.0;
      m_Values[1] = 0.0;
      m_Values[2] = 0.0;
      m_Values[3] = 1.0; 
    }

    inline Quaternion(kt_double x, kt_double y, kt_double z, kt_double w)
    {
      m_Values[0] = x;
      m_Values[1] = y;
      m_Values[2] = z;
      m_Values[3] = w;
    }

    inline Quaternion(const Vector4d& rVector)
    {
      m_Values[0] = rVector.GetX();
      m_Values[1] = rVector.GetY();
      m_Values[2] = rVector.GetZ();
      m_Values[3] = rVector.GetW();
    }

    inline Quaternion(const Quaternion& rOther)
    {
      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];
    }

  public:
    inline kt_double GetX() const
    {
      return m_Values[0]; 
    }

    inline void SetX(kt_double x)
    {
      m_Values[0] = x; 
    }

    inline kt_double GetY() const
    {
      return m_Values[1]; 
    }

    inline void SetY(kt_double y)
    {
      m_Values[1] = y; 
    }

    inline kt_double GetZ() const
    {
      return m_Values[2]; 
    }

    inline void SetZ(kt_double z)
    {
      m_Values[2] = z; 
    }

    inline kt_double GetW() const
    {
      return m_Values[3]; 
    }

    inline void SetW(kt_double w)
    {
      m_Values[3] = w; 
    }

    inline const Vector4d GetAsVector4() const 
    {
      return Vector4d(m_Values[0], m_Values[1], m_Values[2], m_Values[3]);
    }

    void ToAngleAxis(kt_double& rAngle, karto::Vector3d& rAxis) const;

    void FromAngleAxis(kt_double angleInRadians, const karto::Vector3d& rAxis);

    void ToEulerAngles(kt_double& rYaw, kt_double& rPitch, kt_double& rRoll) const;

    void FromEulerAngles(kt_double yaw, kt_double pitch, kt_double roll);

    inline kt_double Length() const
    {
      return sqrt(Norm());
    }

    inline kt_double Norm() const
    {
      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];
    }

    inline void Normalize()
    {
      kt_double length = Length();
      if (length > 0.0)
      {
        kt_double inversedLength = 1.0 / length;

        m_Values[0] *= inversedLength;
        m_Values[1] *= inversedLength;
        m_Values[2] *= inversedLength;
        m_Values[3] *= inversedLength;
      }
    }

    const String ToString() const;

  public:
    inline Quaternion& operator=(const Quaternion& rOther)
    {
      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];

      return(*this);
    }

    inline const Quaternion operator*(const Quaternion& rOther) const
    {
      return Quaternion(
        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],
        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],
        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],
        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] );
    }
    
    inline kt_bool operator==(const Quaternion& rOther) const
    {
      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]);
    }

    inline kt_bool operator!=(const Quaternion& rOther) const
    {
      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]);
    }

    karto::Vector3d operator*(const karto::Vector3d& rVector) const
    {
      // nVidia SDK implementation
      karto::Vector3d uv, uuv; 
      karto::Vector3d qvec(m_Values[0], m_Values[1], m_Values[2]);

      uv = qvec ^ rVector;
      uuv = qvec ^ uv; 

      uv *= ( 2.0 * m_Values[3] ); 
      uuv *= 2.0; 

      return rVector + uv + uuv;
    }

    friend KARTO_FORCEINLINE std::ostream& operator<<(std::ostream& rStream, const Quaternion& rQuaternion)
    {
      rStream << rQuaternion.ToString().ToCString();
      return rStream;
    }

  private:
    kt_double m_Values[4];
  }; // class Quaternion


  class KARTO_EXPORT BoundingBox2
  {
  public:
    /*
     * Bounding box of maximal size
     */
    BoundingBox2();

    BoundingBox2(const Vector2d& rMinimum, const Vector2d& rMaximum);

  public:
    inline const Vector2d& GetMinimum() const
    { 
      return m_Minimum; 
    }

    inline void SetMinimum(const Vector2d& rMinimum)
    {
      m_Minimum = rMinimum;
    }

    inline const Vector2d& GetMaximum() const
    { 
      return m_Maximum;
    }

    inline void SetMaximum(const Vector2d& rMaximum)
    {
      m_Maximum = rMaximum;
    }

    inline Size2<kt_double> GetSize() const
    {
      Vector2d size = m_Maximum - m_Minimum;

      return Size2<kt_double>(size.GetX(), size.GetY());
    }

    inline void Add(const Vector2d& rPoint)
    {
      m_Minimum.MakeFloor(rPoint);
      m_Maximum.MakeCeil(rPoint);
    }

    inline void Add(const BoundingBox2& rBoundingBox)
    {
      Add(rBoundingBox.GetMinimum());
      Add(rBoundingBox.GetMaximum());
    }
    
    inline kt_bool Contains(const Vector2d& rPoint) const
    {
      return (math::InRange(rPoint.GetX(), m_Minimum.GetX(), m_Maximum.GetX()) &&
              math::InRange(rPoint.GetY(), m_Minimum.GetY(), m_Maximum.GetY()));
    }

    inline kt_bool Intersects(const BoundingBox2& rOther) const
    { 
      if ((m_Maximum.GetX() < rOther.m_Minimum.GetX()) || (m_Minimum.GetX() > rOther.m_Maximum.GetX()))
      {
        return false;
      }
      
      if ((m_Maximum.GetY() < rOther.m_Minimum.GetY()) || (m_Minimum.GetY() > rOther.m_Maximum.GetY()))
      {
        return false;
      }

      return true;
    }

    inline kt_bool Contains(const BoundingBox2& rOther) const
    {
      if ((m_Maximum.GetX() < rOther.m_Minimum.GetX()) || (m_Minimum.GetX() > rOther.m_Maximum.GetX()))
      {
        return false;
      }
      
      if ((m_Maximum.GetY() < rOther.m_Minimum.GetY()) || (m_Minimum.GetY() > rOther.m_Maximum.GetY()))
      {
        return false;
      }
      
      if ((m_Minimum.GetX() <= rOther.m_Minimum.GetX()) && (rOther.m_Maximum.GetX() <= m_Maximum.GetX()) && 
          (m_Minimum.GetY() <= rOther.m_Minimum.GetY()) && (rOther.m_Maximum.GetY() <= m_Maximum.GetY()))
      {
        return true;
      }

      return false;
    }

  private:
    Vector2d m_Minimum;
    Vector2d m_Maximum;
  }; // class BoundingBox2

  class KARTO_EXPORT BoundingBox3
  {
  public:
    BoundingBox3();
    virtual ~BoundingBox3();

  public:
    inline const Vector3d& GetMinimum() const
    { 
      return m_Minimum; 
    }

    inline void SetMinimum(const Vector3d& rMinimum)
    {
      m_Minimum = rMinimum;
    }

    inline const Vector3d& GetMaximum() const
    { 
      return m_Maximum;
    }

    inline void SetMaximum(const Vector3d& rMaximum)
    {
      m_Maximum = rMaximum;
    }

  private:
    Vector3d m_Minimum;
    Vector3d m_Maximum;
  };

  
  template<typename T>
  class Rectangle2
  {
  public:
    Rectangle2()
    {
    }

    Rectangle2(T x, T y, T width, T height)
      : m_Position(x, y)
      , m_Size(width, height)
    {
    }

    Rectangle2(const Vector2<T>& rPosition, const Size2<T>& rSize)
      : m_Position(rPosition)
      , m_Size(rSize)
    {
    }

    Rectangle2(const Vector2<T>& rTopLeft, const Vector2<T>& rBottomRight)
      : m_Position(rTopLeft)
      , m_Size(rBottomRight.GetX() - rTopLeft.GetX(), rBottomRight.GetY() - rTopLeft.GetY())
    {
    }

    Rectangle2(const Rectangle2& rOther)
      : m_Position(rOther.m_Position)
      , m_Size(rOther.m_Size)
    {
    }

  public:
    inline T GetX() const
    {
      return m_Position.GetX();
    }
    
    inline void SetX(const T& rX) 
    {
      m_Position.SetX(rX);
    }

    inline T GetY() const
    {
      return m_Position.GetY();
    }

    inline void SetY(const T& rY)
    {
      m_Position.SetY(rY);
    }
    
    inline T GetWidth() const
    {
      return m_Size.GetWidth();
    }

    inline void SetWidth(const T& rWidth)
    {
      m_Size.SetWidth(rWidth);
    }
    
    inline T GetHeight() const
    {
      return m_Size.GetHeight();
    }

    inline void SetHeight(const T& rHeight)
    {
      m_Size.SetHeight(rHeight);
    }
    
    inline const Vector2<T>& GetPosition() const
    {
      return m_Position;
    }

    inline void SetPosition(const T& rX, const T& rY)
    {
      m_Position = Vector2<T>(rX, rY);
    }

    inline void SetPosition(const Vector2<T>& rPosition)
    {
      m_Position = rPosition;
    }

    inline const Size2<T>& GetSize() const
    {
      return m_Size;
    }

    inline void SetSize(const Size2<T>& rSize) 
    {
      m_Size = rSize;
    }

    inline T GetLeft() const
    {
      return m_Position.GetX();
    }

    inline void SetLeft(const T& rLeft)
    {
      m_Position.SetX(rLeft);
    }

    inline T GetTop() const
    {
      return m_Position.GetY();
    }

    inline void SetTop(const T& rTop)
    {
      m_Position.SetY(rTop);
    }

    inline T GetRight() const
    {
      return m_Position.GetX() + m_Size.GetWidth();
    }

    inline void SetRight(const T& rRight)
    {
      m_Size.SetWidth(rRight - m_Position.GetX());
    }

    inline T GetBottom() const
    {
      return m_Position.GetY() + m_Size.GetHeight();
    }

    inline void SetBottom(const T& rBottom)
    {
      m_Size.SetHeight(rBottom - m_Position.GetY());
    }

    inline Vector2<T> GetTopLeft()
    {
      return Vector2<T>(GetLeft(), GetTop());
    }

    inline Vector2<T> GetTopRight()
    {
      return Vector2<T>(GetRight(), GetTop());
    }

    inline Vector2<T> GetBottomLeft()
    {
      return Vector2<T>(GetLeft(), GetBottom());
    }

    inline Vector2<T> GetBottomRight()
    {
      return Vector2<T>(GetRight(), GetBottom());
    }

    inline const Vector2<T> GetCenter() const
    {
      return Vector2<T>(m_Position.GetX() + m_Size.GetWidth() * 0.5, m_Position.GetY() + m_Size.GetHeight() * 0.5);
    }

    inline kt_bool Contains(const Rectangle2<T>& rOther)
    {
      T l1 = m_Position.GetX();
      T r1 = m_Position.GetX();
      if (m_Size.GetWidth() < 0)
        l1 += m_Size.GetWidth();
      else
        r1 += m_Size.GetWidth();
      if (l1 == r1) // null rect
        return false;
      
      T l2 = rOther.m_Position.GetX();
      T r2 = rOther.m_Position.GetX();
      if (rOther.m_Size.GetWidth() < 0)
        l2 += rOther.m_Size.GetWidth();
      else
        r2 += rOther.m_Size.GetWidth();
      if (l2 == r2) // null rect
        return false;

      if (l2 < l1 || r2 > r1)
        return false;

      T t1 = m_Position.GetY();
      T b1 = m_Position.GetY();
      if (m_Size.GetHeight() < 0)
        t1 += m_Size.GetHeight();
      else
        b1 += m_Size.GetHeight();
      if (t1 == b1) // null rect
        return false;

      T t2 = rOther.m_Position.GetY();
      T b2 = rOther.m_Position.GetY();
      if (rOther.m_Size.GetHeight() < 0)
        t2 += rOther.m_Size.GetHeight();
      else
        b2 += rOther.m_Size.GetHeight();
      if (t2 == b2) // null rect
        return false;

      if (t2 < t1 || b2 > b1)
        return false;

      return true;
    }

  public:
    Rectangle2& operator = (const Rectangle2& rOther)
    {
      m_Position = rOther.m_Position;
      m_Size = rOther.m_Size;
      
      return *this;
    }

    inline kt_bool operator == (const Rectangle2& rOther) const
    {
      return (m_Position == rOther.m_Position && m_Size == rOther.m_Size);
    }

    inline kt_bool operator != (const Rectangle2& rOther) const
    {
      return (m_Position != rOther.m_Position || m_Size != rOther.m_Size);
    }

  private:
    Vector2<T> m_Position;
    Size2<T> m_Size;
  }; // class Rectangle2<T>


  class Pose3;

  class KARTO_EXPORT Pose2
  {
  public:
    Pose2();

    Pose2(const Vector2d& rPosition, kt_double heading = 0);

    Pose2(kt_double x, kt_double y, kt_double heading);

    Pose2(const Pose3& rPose);

    Pose2(const Pose2& rOther);

  public:
    inline kt_double GetX() const
    {
      return m_Position.GetX();
    }

    inline void SetX(kt_double x)
    {
      m_Position.SetX(x);
    }

    inline kt_double GetY() const
    {
      return m_Position.GetY();
    }

    inline void SetY(kt_double y)
    {
      m_Position.SetY(y);
    }

    inline const Vector2d& GetPosition() const
    {
      return m_Position;
    }

    inline void SetPosition(const Vector2d& rPosition)
    {
      m_Position = rPosition;
    }

    inline kt_double GetHeading() const
    {
      return m_Heading;
    }

    inline void SetHeading(kt_double heading)
    {
      m_Heading = heading;
    }

    inline kt_double SquaredDistance(const Pose2& rOther) const
    {
      return m_Position.SquaredDistance(rOther.m_Position);
    }
    
    inline kt_double AngleTo(const Vector2d& rVector) const
    {
      kt_double angle = atan2(rVector.GetY() - GetY(), rVector.GetX() - GetX());
      return karto::math::NormalizeAngle(angle - GetHeading());
    }
    
    inline const String ToString(kt_int32u precision = 4) const
    {
      String valueString;
      valueString.Append(StringHelper::ToString(m_Position.GetX(), precision));
      valueString.Append(" ");
      valueString.Append(StringHelper::ToString(m_Position.GetY(), precision));
      valueString.Append(" ");
      valueString.Append(StringHelper::ToString(m_Heading, precision));
      return valueString;
    }

  public:
    inline Pose2& operator = (const Pose2& rOther)
    {
      m_Position = rOther.m_Position;
      m_Heading = rOther.m_Heading;

      return *this;
    }

    inline kt_bool operator==(const Pose2& rOther) const
    {
      return (m_Position == rOther.m_Position && m_Heading == rOther.m_Heading);
    }

    inline kt_bool operator!=(const Pose2& rOther) const
    {
      return (m_Position != rOther.m_Position || m_Heading != rOther.m_Heading);
    }

    inline void operator+=(const Pose2& rOther)
    {
      m_Position += rOther.m_Position;
      m_Heading = math::NormalizeAngle(m_Heading + rOther.m_Heading);
    }

    inline Pose2 operator+(const Pose2& rOther) const
    {
      return Pose2(m_Position + rOther.m_Position, math::NormalizeAngle(m_Heading + rOther.m_Heading));
    }

    inline Pose2 operator-(const Pose2& rOther) const
    {
      return Pose2(m_Position - rOther.m_Position, math::NormalizeAngle(m_Heading - rOther.m_Heading));
    }

    friend KARTO_FORCEINLINE std::ostream& operator << (std::ostream& rStream, const Pose2& rPose)
    {
      rStream << rPose.ToString();
      return rStream;
    }
    
  private:
    Vector2d m_Position;

    kt_double m_Heading;
  }; // class Pose2

  typedef List<Pose2> Pose2List;


  class Pose3
  {
  public:
    Pose3()
    {
    }

    Pose3(const Vector3d& rPosition)
      : m_Position(rPosition)
    {
    }

    Pose3(const Vector3d& rPosition, const karto::Quaternion& rOrientation)
      : m_Position(rPosition)
      , m_Orientation(rOrientation)
    {
    }

    Pose3(const Pose3& rOther)
      : m_Position(rOther.m_Position)
      , m_Orientation(rOther.m_Orientation)
    {
    }

    Pose3(const Pose2& rPose)
    {
      m_Position = Vector3d(rPose.GetX(), rPose.GetY(), 0.0);
      m_Orientation.FromEulerAngles(rPose.GetHeading(), 0.0, 0.0);
    }

  public:
    inline const Vector3d& GetPosition() const
    {
      return m_Position;
    }

    inline void SetPosition(const Vector3d& rPosition)
    {
      m_Position = rPosition;
    }

    inline const Quaternion& GetOrientation() const
    {
      return m_Orientation;
    }

    inline void SetOrientation(const Quaternion& rOrientation)
    {
      m_Orientation = rOrientation;
    }

    inline const String ToString() const
    {
      String valueString;
      valueString.Append(GetPosition().ToString());
      valueString.Append(" ");
      valueString.Append(GetOrientation().ToString());
      return valueString;
    }
    
  public:
    inline Pose3& operator = (const Pose3& rOther)
    {
      m_Position = rOther.m_Position;
      m_Orientation = rOther.m_Orientation;

      return *this;
    }

    inline kt_bool operator == (const Pose3& rOther) const
    {
      return (m_Position == rOther.m_Position && m_Orientation == rOther.m_Orientation);
    }

    inline kt_bool operator != (const Pose3& rOther) const
    {
      return (m_Position != rOther.m_Position || m_Orientation != rOther.m_Orientation);
    }

    friend KARTO_FORCEINLINE std::ostream& operator << (std::ostream& rStream, const Pose3& rPose)
    {
      rStream << rPose.ToString();
      return rStream;
    }

  private:
    Vector3d m_Position;
    Quaternion m_Orientation;
  }; // class Pose3

  
  class Matrix3
  {
  public:
    Matrix3()
    {
      Clear();
    }

    inline Matrix3(const Matrix3& rOther)
    {
      memcpy(m_Matrix, rOther.m_Matrix, 9*sizeof(kt_double));
    }

  public:
    void SetToIdentity()
    {
      memset(m_Matrix, 0, 9*sizeof(kt_double));

      for (kt_int32s i = 0; i < 3; i++)
      {
        m_Matrix[i][i] = 1.0;
      }
    }

    void Clear()
    {
      memset(m_Matrix, 0, 9*sizeof(kt_double));
    }

    void FromAxisAngle(kt_double x, kt_double y, kt_double z, const kt_double radians)
    {
      kt_double cosRadians = cos(radians);
      kt_double sinRadians = sin(radians);
      kt_double oneMinusCos = 1.0 - cosRadians;

      kt_double xx = x * x;
      kt_double yy = y * y;
      kt_double zz = z * z;

      kt_double xyMCos = x * y * oneMinusCos;
      kt_double xzMCos = x * z * oneMinusCos;
      kt_double yzMCos = y * z * oneMinusCos;

      kt_double xSin = x * sinRadians;
      kt_double ySin = y * sinRadians;
      kt_double zSin = z * sinRadians;

      m_Matrix[0][0] = xx * oneMinusCos + cosRadians;
      m_Matrix[0][1] = xyMCos - zSin;
      m_Matrix[0][2] = xzMCos + ySin;

      m_Matrix[1][0] = xyMCos + zSin;
      m_Matrix[1][1] = yy * oneMinusCos + cosRadians;
      m_Matrix[1][2] = yzMCos - xSin;

      m_Matrix[2][0] = xzMCos - ySin;
      m_Matrix[2][1] = yzMCos + xSin;
      m_Matrix[2][2] = zz * oneMinusCos + cosRadians;
    }

    Matrix3 Transpose() const
    {
      Matrix3 transpose;

      for (kt_int32u row = 0; row < 3; row++)
      {
        for (kt_int32u col = 0; col < 3; col++)
        {
          transpose.m_Matrix[row][col] = m_Matrix[col][row];
        }
      }

      return transpose;
    }

    Matrix3 Inverse() const
    {
      Matrix3 kInverse = *this;
      kt_bool haveInverse = InverseFast(kInverse, 1e-14);
      if (haveInverse == false)
      {
        assert(false);
      }
      return kInverse;
    }
    
    inline const String ToString() const
    {
      String valueString;

      for (int row = 0; row < 3; row++)
      {
        for (int col = 0; col < 3; col++)
        {
          valueString.Append(StringHelper::ToString(m_Matrix[row][col]));
          valueString.Append(" ");
        }
      }
      
      return valueString;
    }

  public:
    inline Matrix3& operator = (const Matrix3& rOther)
    {
      memcpy(m_Matrix, rOther.m_Matrix, 9*sizeof(kt_double));
      return *this;
    }

    inline kt_double& operator()(kt_int32u row, kt_int32u column)
    {
      return m_Matrix[row][column];
    }

    inline kt_double operator()(kt_int32u row, kt_int32u column) const
    {
      return m_Matrix[row][column];
    }

    Matrix3 operator*(const Matrix3& rOther) const
    {
      Matrix3 product;

      for (size_t row = 0; row < 3; row++)
      {
        for (size_t col = 0; col < 3; col++)
        {
          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];
        }
      }

      return product;
    }

    inline Pose2 operator*(const Pose2& rPose2) const
    {
      Pose2 pose2;

      pose2.SetX(m_Matrix[0][0] * rPose2.GetX() + m_Matrix[0][1] * rPose2.GetY() + m_Matrix[0][2] * rPose2.GetHeading());
      pose2.SetY(m_Matrix[1][0] * rPose2.GetX() + m_Matrix[1][1] * rPose2.GetY() + m_Matrix[1][2] * rPose2.GetHeading());
      pose2.SetHeading(m_Matrix[2][0] * rPose2.GetX() + m_Matrix[2][1] * rPose2.GetY() + m_Matrix[2][2] * rPose2.GetHeading());

      return pose2;
    }

    inline void operator+=(const Matrix3& rkMatrix)
    {
      for (kt_int32u row = 0; row < 3; row++)
      {
        for (kt_int32u col = 0; col < 3; col++)
        {
          m_Matrix[row][col] += rkMatrix.m_Matrix[row][col];
        }
      }
    }
    
    inline kt_bool operator==(const Matrix3& rkMatrix)
    {
      for (kt_int32u row = 0; row < 3; row++)
      {
        for (kt_int32u col = 0; col < 3; col++)
        {
          if (math::DoubleEqual(m_Matrix[row][col], rkMatrix.m_Matrix[row][col]) == false)
          {
            return false;
          }
        }
      }
      
      return true;
    }
    
    friend KARTO_FORCEINLINE std::ostream& operator << (std::ostream& rStream, const Matrix3& rMatrix)
    {
      rStream << rMatrix.ToString();
      return rStream;
    }

  private:
    kt_bool InverseFast(Matrix3& rkInverse, kt_double fTolerance = KT_TOLERANCE) const
    {
      // Invert a 3x3 using cofactors.  This is about 8 times faster than
      // the Numerical Recipes code which uses Gaussian elimination.
      rkInverse.m_Matrix[0][0] = m_Matrix[1][1]*m_Matrix[2][2] - m_Matrix[1][2]*m_Matrix[2][1];
      rkInverse.m_Matrix[0][1] = m_Matrix[0][2]*m_Matrix[2][1] - m_Matrix[0][1]*m_Matrix[2][2];
      rkInverse.m_Matrix[0][2] = m_Matrix[0][1]*m_Matrix[1][2] - m_Matrix[0][2]*m_Matrix[1][1];
      rkInverse.m_Matrix[1][0] = m_Matrix[1][2]*m_Matrix[2][0] - m_Matrix[1][0]*m_Matrix[2][2];
      rkInverse.m_Matrix[1][1] = m_Matrix[0][0]*m_Matrix[2][2] - m_Matrix[0][2]*m_Matrix[2][0];
      rkInverse.m_Matrix[1][2] = m_Matrix[0][2]*m_Matrix[1][0] - m_Matrix[0][0]*m_Matrix[1][2];
      rkInverse.m_Matrix[2][0] = m_Matrix[1][0]*m_Matrix[2][1] - m_Matrix[1][1]*m_Matrix[2][0];
      rkInverse.m_Matrix[2][1] = m_Matrix[0][1]*m_Matrix[2][0] - m_Matrix[0][0]*m_Matrix[2][1];
      rkInverse.m_Matrix[2][2] = m_Matrix[0][0]*m_Matrix[1][1] - m_Matrix[0][1]*m_Matrix[1][0];
      
      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];
      
      if (fabs(fDet) <= fTolerance)
      {
        return false;
      }
      
      kt_double fInvDet = 1.0/fDet;
      for (size_t row = 0; row < 3; row++)
      {
        for (size_t col = 0; col < 3; col++)
        {
          rkInverse.m_Matrix[row][col] *= fInvDet;
        }
      }
      
      return true;
    }
    
  private:
    kt_double m_Matrix[3][3];
  }; // class Matrix3


  class Color
  {
  public:
    Color()
      : m_Red(0.0)
      , m_Green(0.0)
      , m_Blue(0.0)
      , m_Alpha(1.0)
    {
    }

    Color(const Color& rOther)
      : m_Red(rOther.m_Red)
      , m_Green(rOther.m_Green)
      , m_Blue(rOther.m_Blue)
      , m_Alpha(rOther.m_Alpha)
    {
    }

    Color(kt_double red, kt_double green, kt_double blue, kt_double alpha = 1.0)
      : m_Red(red)
      , m_Green(green)
      , m_Blue(blue)
      , m_Alpha(alpha)
    {
    }

    Color(kt_int8u red, kt_int8u green, kt_int8u blue, kt_int8u alpha = 255)
      : m_Red((kt_double)red/255.0)
      , m_Green((kt_double)green/255.0)
      , m_Blue((kt_double)blue/255.0)
      , m_Alpha((kt_double)alpha/255.0)
    {
    }

    virtual ~Color()
    {
    }

  public:
    const kt_double GetRed() const
    {
      return m_Red;
    }

    void SetRed(kt_double red)
    {
      m_Red = red;
    }

    const kt_double GetGreen() const
    {
      return m_Green;
    }

    void SetGreen(kt_double green)
    {
      m_Green = green;
    }

    const kt_double GetBlue() const
    {
      return m_Blue;
    }

    void SetBlue(kt_double blue)
    {
      m_Blue = blue;
    }

    const kt_double GetAlpha() const
    {
      return m_Alpha;
    }

    void SetAlpha(kt_double alpha)
    {
      m_Alpha = alpha;
    }

    const String ToString() const
    {
      String valueString;
      valueString.Append(StringHelper::ToString(GetRed()));
      valueString.Append(" ");
      valueString.Append(StringHelper::ToString(GetGreen()));
      valueString.Append(" ");
      valueString.Append(StringHelper::ToString(GetBlue()));
      valueString.Append(" ");
      valueString.Append(StringHelper::ToString(GetAlpha()));
      return valueString;
    }

    public:
    inline kt_bool operator == (const Color& rOther) const
    {
      return (m_Red == rOther.m_Red && m_Green == rOther.m_Green && m_Blue == rOther.m_Blue && m_Alpha == rOther.m_Alpha);
    }

    inline kt_bool operator != (const Color& rOther) const
    {
      return (m_Red != rOther.m_Red || m_Green != rOther.m_Green || m_Blue != rOther.m_Blue || m_Alpha != rOther.m_Alpha);
    }

    friend KARTO_FORCEINLINE std::ostream& operator << (std::ostream& rStream, const Color& rColor)
    {
      rStream << rColor.ToString();
      return rStream;
    }

  private:
    kt_double m_Red;
    kt_double m_Green;
    kt_double m_Blue;
    kt_double m_Alpha;
  }; // class Color

 
  namespace gps
  {
    class PointGps : public karto::Vector2d
    {
    public:
      PointGps()
      {
      }
      
      PointGps(kt_double latitude, kt_double longitude)
      {
        SetX(longitude);
        SetY(latitude);
      }
      
      PointGps(const PointGps& rOther)
      {
        SetX(rOther.GetLongitude());
        SetY(rOther.GetLatitude());
      }
      
    public:
      inline kt_double GetLatitude() const
      {
        return GetY();
      }
      
      inline void SetLatitude(kt_double latitude)
      {
        SetY(latitude);
      }
      
      inline kt_double GetLongitude() const
      {
        return GetX();
      }
      
      inline void SetLongitude(kt_double longitude)
      {
        SetX(longitude);
      }
      
      inline kt_double GetBearing(const PointGps& rOther)
      {
        kt_double lat1 = math::DegreesToRadians(GetLatitude());
        kt_double long1 = math::DegreesToRadians(GetLongitude());
        kt_double lat2 = math::DegreesToRadians(rOther.GetLatitude());
        kt_double long2 = math::DegreesToRadians(rOther.GetLongitude());
        
        kt_double deltaLong = long2 - long1;
        
        kt_double y = sin(deltaLong) * cos(lat2);
        kt_double x = cos(lat1) * sin(lat2) - sin(lat1) * cos(lat2) * cos(deltaLong);
        
        return math::RadiansToDegrees(atan2(y, x));
      }
      
      void AddOffset(const PointGps& rOffset)
      {
        AddOffset(rOffset.GetLatitude(), rOffset.GetLongitude());
      }
      
      void AddOffset(kt_double latitude, kt_double longitude)
      {
        SetX(GetLongitude() + longitude);
        SetY(GetLatitude() - latitude);
      }
      
      kt_double Distance(const PointGps& rOther)
      {
        kt_double dlon = rOther.GetLongitude() - GetLongitude();

        const kt_double slat1 = sin(math::DegreesToRadians(GetLatitude()));
        const kt_double clat1 = cos(math::DegreesToRadians(GetLatitude()));

        const kt_double slat2 = sin(math::DegreesToRadians(rOther.GetLatitude()));
        const kt_double clat2 = cos(math::DegreesToRadians(rOther.GetLatitude()));

        const kt_double sdlon = sin(math::DegreesToRadians(dlon));
        const kt_double cdlon = cos(math::DegreesToRadians(dlon));

        const kt_double t1 = clat2 * sdlon;
        const kt_double t2 = clat1 * slat2 - slat1 * clat2 * cdlon;
        const kt_double t3 = slat1 * slat2 + clat1 * clat2 * cdlon;
        const kt_double dist = atan2(sqrt(t1*t1 + t2*t2), t3);

        const kt_double earthRadius = 6372.795;
        return dist * earthRadius;
      }

      inline const String ToString() const
      {
        String valueString;
        valueString.Append(StringHelper::ToString(GetLatitude()));
        valueString.Append(" ");
        valueString.Append(StringHelper::ToString(GetLongitude()));
        return valueString;
      }
      
      friend KARTO_FORCEINLINE std::ostream& operator << (std::ostream& rStream, const PointGps& rPointGps)
      {
        rStream << rPointGps.ToString();
        return rStream;
      }

    public:
      inline const PointGps operator + (const PointGps& rOther) const
      {
        return PointGps(GetLatitude() + rOther.GetLatitude(), GetLongitude() + rOther.GetLongitude());
      }
      
      inline const PointGps operator - (const PointGps& rOther) const
      {
        return PointGps(GetLatitude() - rOther.GetLatitude(), GetLongitude() - rOther.GetLongitude());
      }
    };

    typedef List<PointGps> PointGpsList;
  }


  template<typename T>
  inline String StringHelper::ToString(const Size2<T>& rValue)
  {
    return rValue.ToString();
  }

  template<typename T>
  inline String StringHelper::ToString(const Vector2<T>& rValue)
  {
    return rValue.ToString();
  }

  template<typename T>
  inline String StringHelper::ToString(const Vector3<T>& rValue)
  {
    return rValue.ToString();
  }

  template<typename T>
  inline String StringHelper::ToString(const Vector4<T>& rValue)
  {
    return rValue.ToString();
  }

  template<typename T>
  inline static kt_bool FromString(const String& rStringValue, Vector3<T>& rValue)
  {
    karto::String tempString = rStringValue;
    kt_size_t index = tempString.FindFirstOf(" ");
    if (index != -1)
    {
      karto::String stringValue;
      T value;

      // Get X
      stringValue = tempString.SubString(0, index);

      value = 0;
      FromString(stringValue, value);
      rValue.SetX(value);

      // Get Y
      tempString = rStringValue.SubString(index + 1, rStringValue.Size());
      index = tempString.FindFirstOf(" ");

      stringValue = tempString.SubString(0, index);

      value = 0;
      FromString(stringValue, value);
      rValue.SetY(value);

      // Get Z
      tempString = rStringValue.SubString(index + 1, rStringValue.Size());
      index = tempString.FindFirstOf(" ");

      stringValue = tempString.SubString(index + 1, rStringValue.Size());

      value = 0;
      FromString(stringValue, value);
      rValue.SetZ(value);

      return true;
    }

    return false;
  }



}

#endif // __OpenKarto_Geometry_h__