PoseTransform.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/PoseTransform.h>
00019 
00020 namespace karto
00021 {
00022 
00023   Transform::Transform()
00024   {
00025     SetTransform(Pose2(), Pose2());
00026   }
00027 
00028   Transform::Transform(const Pose2& rPose)
00029   {
00030     SetTransform(Pose2(), rPose);
00031   }
00032 
00033   Transform::Transform(const Pose2& rPose1, const Pose2& rPose2)
00034   {
00035     SetTransform(rPose1, rPose2);
00036   }
00037 
00038   void Transform::SetTransform(const Pose2& rPose1, const Pose2& rPose2)
00039   {
00040     if (rPose1 == rPose2)
00041     {
00042       m_Rotation.SetToIdentity();
00043       m_InverseRotation.SetToIdentity();
00044       m_Transform = Pose2();
00045       return;
00046     }
00047 
00048     // heading transformation
00049     m_Rotation.FromAxisAngle(0, 0, 1, rPose2.GetHeading() - rPose1.GetHeading());
00050     m_InverseRotation.FromAxisAngle(0, 0, 1, rPose1.GetHeading() - rPose2.GetHeading());
00051 
00052     // position transformation
00053     Pose2 newPosition;
00054     if (rPose1.GetX() != 0.0 || rPose1.GetY() != 0.0)
00055     {
00056       newPosition = rPose2 - m_Rotation * rPose1;
00057     }
00058     else
00059     {
00060       newPosition = rPose2;
00061     }
00062 
00063     m_Transform = Pose2(newPosition.GetPosition(), rPose2.GetHeading() - rPose1.GetHeading());
00064   }
00065 
00066 }


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