Geometry.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (C) 2006-2011, SRI International (R)
00003  *
00004  * This program is free software: you can redistribute it and/or modify
00005  * it under the terms of the GNU Lesser General Public License as published by
00006  * the Free Software Foundation, either version 3 of the License, or
00007  * (at your option) any later version.
00008  *
00009  * This program is distributed in the hope that it will be useful,
00010  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00011  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00012  * GNU Lesser General Public License for more details.
00013  *
00014  * You should have received a copy of the GNU Lesser General Public License
00015  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
00016  */
00017 
00018 #include <OpenKarto/Geometry.h>
00019 #include <OpenKarto/String.h>
00020 
00021 namespace karto
00022 {
00023 
00027 
00028   Pose2::Pose2()
00029     : m_Heading(0.0)
00030   {
00031   }
00032 
00033   Pose2::Pose2(const Vector2d& rPosition, kt_double heading)
00034     : m_Position(rPosition)
00035     , m_Heading(heading)
00036   {
00037   }
00038 
00039   Pose2::Pose2(kt_double x, kt_double y, kt_double heading)
00040     : m_Position(x, y)
00041     , m_Heading(heading)
00042   {
00043   }
00044 
00045   Pose2::Pose2(const Pose3& rPose)
00046     : m_Position(rPose.GetPosition().GetX(), rPose.GetPosition().GetY())
00047   {
00048     kt_double t1, t2;
00049 
00050     // calculates heading from orientation
00051     rPose.GetOrientation().ToEulerAngles(m_Heading, t1, t2);
00052   }
00053 
00054   Pose2::Pose2(const Pose2& rOther)
00055     : m_Position(rOther.m_Position)
00056     , m_Heading(rOther.m_Heading)
00057   {
00058   }
00059 
00063 
00064   BoundingBox2::BoundingBox2()
00065     : m_Minimum(DBL_MAX, DBL_MAX)
00066     , m_Maximum(-DBL_MAX, -DBL_MAX)
00067   {
00068   }
00069 
00070   BoundingBox2::BoundingBox2(const Vector2d& rMinimum, const Vector2d& rMaximum)
00071     : m_Minimum(rMinimum)
00072     , m_Maximum(rMaximum)
00073   {
00074   }
00075 
00079 
00080   BoundingBox3::BoundingBox3()
00081   : m_Minimum(DBL_MAX, DBL_MAX, DBL_MAX)
00082   , m_Maximum(-DBL_MAX, -DBL_MAX, -DBL_MAX)
00083   {
00084   }
00085 
00086   BoundingBox3::~BoundingBox3()
00087   {
00088   }
00089 
00093 
00094   void Quaternion::ToAngleAxis(kt_double& rAngle, karto::Vector3d& rAxis) const
00095   {
00096     // The quaternion representing the rotation is
00097     //   q = cos(A/2)+sin(A/2)*(x*i+y*j+z*k)
00098 
00099     kt_double fSqrLength = m_Values[0] * m_Values[0] + m_Values[1] * m_Values[1] + m_Values[2] * m_Values[2];
00100     if ( fSqrLength > 0.0 )
00101     {
00102       rAngle = 2.0 * acos(m_Values[3]);
00103       kt_double fInvLength = 1.0 / sqrt(fSqrLength);
00104       rAxis.SetX(m_Values[0] * fInvLength);
00105       rAxis.SetY(m_Values[1] * fInvLength);
00106       rAxis.SetZ(m_Values[2] * fInvLength);
00107     }
00108     else
00109     {
00110       // angle is 0 (mod 2*pi), so any axis will do
00111       rAngle = 0.0;
00112       rAxis.SetX(1.0);
00113       rAxis.SetY(0.0);
00114       rAxis.SetZ(0.0);
00115     }
00116   }
00117 
00118   void Quaternion::FromAngleAxis(kt_double angleInRadians, const karto::Vector3d& rAxis)
00119   {
00120     kt_double axisLength = rAxis.Length();
00121     if (axisLength < KT_TOLERANCE)
00122     {
00123       assert(false);
00124       // special case for zero length
00125       *this = Quaternion();
00126       return;
00127     }
00128     kt_double halfAngle = 0.5*angleInRadians;
00129 
00130     kt_double inverseLength  = 1.0;// / axisLength;
00131     kt_double sinHalfAngle = sin(halfAngle);
00132     kt_double cosHalfAngle = cos(halfAngle);
00133 
00134     m_Values[0] = inverseLength * sinHalfAngle * rAxis.GetX();
00135     m_Values[1] = inverseLength * sinHalfAngle * rAxis.GetY();
00136     m_Values[2] = inverseLength * sinHalfAngle * rAxis.GetZ();
00137     m_Values[3] = cosHalfAngle;
00138   }
00139 
00140   void Quaternion::ToEulerAngles(kt_double& rYaw, kt_double& rPitch, kt_double& rRoll) const 
00141   {
00142     kt_double test = m_Values[0] * m_Values[1] + m_Values[2] * m_Values[3];
00143 
00144     if (test > 0.499)
00145     { 
00146       // singularity at north pole
00147       rYaw = 2 * atan2(m_Values[0], m_Values[3]);;
00148       rPitch = KT_PI_2;
00149       rRoll = 0;
00150     }
00151     else if (test < -0.499)
00152     { 
00153       // singularity at south pole
00154       rYaw = -2 * atan2(m_Values[0], m_Values[3]);
00155       rPitch = -KT_PI_2;
00156       rRoll = 0;
00157     }
00158     else
00159     {
00160       kt_double sqx = m_Values[0] * m_Values[0];
00161       kt_double sqy = m_Values[1] * m_Values[1];
00162       kt_double sqz = m_Values[2] * m_Values[2];
00163 
00164       rYaw = atan2(2 * m_Values[1] * m_Values[3] - 2 * m_Values[0] * m_Values[2], 1 - 2 * sqy - 2 * sqz);
00165       rPitch = asin(2 * test);
00166       rRoll = atan2(2 * m_Values[0] * m_Values[3] - 2 * m_Values[1] * m_Values[2], 1 - 2 * sqx - 2 * sqz);
00167     }
00168   }
00169 
00170   void Quaternion::FromEulerAngles(kt_double yaw, kt_double pitch, kt_double roll)
00171   {
00172     kt_double angle;
00173 
00174     angle = yaw * 0.5; 
00175     kt_double cYaw = cos(angle);
00176     kt_double sYaw = sin(angle);
00177 
00178     angle = pitch * 0.5; 
00179     kt_double cPitch = cos(angle);
00180     kt_double sPitch = sin(angle);
00181 
00182     angle = roll * 0.5; 
00183     kt_double cRoll = cos(angle);
00184     kt_double sRoll = sin(angle);
00185 
00186     m_Values[0] = sYaw * sPitch * cRoll + cYaw * cPitch * sRoll;
00187     m_Values[1] = sYaw * cPitch * cRoll + cYaw * sPitch * sRoll;
00188     m_Values[2] = cYaw * sPitch * cRoll - sYaw * cPitch * sRoll;
00189     m_Values[3] = cYaw * cPitch * cRoll - sYaw * sPitch * sRoll;
00190   }
00191 
00192   const String Quaternion::ToString() const
00193   {
00194     String valueString;
00195     valueString.Append(StringHelper::ToString(GetX()));
00196     valueString.Append(" ");
00197     valueString.Append(StringHelper::ToString(GetY()));
00198     valueString.Append(" ");
00199     valueString.Append(StringHelper::ToString(GetZ()));
00200     valueString.Append(" ");
00201     valueString.Append(StringHelper::ToString(GetW()));
00202     return valueString;
00203   }
00204 
00208 
00209 }


nav2d_karto
Author(s): Sebastian Kasperski
autogenerated on Sun Apr 2 2017 04:05:23