RigidBodyTransform.cpp
Go to the documentation of this file.
00001 /*
00002  * Karto (tm) Robot Navigation Software - Software Development Kit
00003  * Release 2.1
00004  *
00005  * Copyright (C) 2006-2011, SRI International (R)
00006  *
00007  * The material contained in this release is copyrighted. It may not be copied,
00008  * reproduced, translated, reverse engineered, modified or reduced to any electronic
00009  * medium or machine-readable form without the prior written consent of
00010  * SRI International (R).
00011  *
00012  * Portions of files in this release may be unpublished work
00013  * containing SRI International (R) CONFIDENTIAL AND PROPRIETARY INFORMATION.
00014  * Disclosure, use, reverse engineering, modification, or reproduction without
00015  * written authorization of SRI International (R) is likewise prohibited.
00016  *
00017  * Karto (tm) is a Trademark of SRI International (R).
00018  *
00019  * Author(s): Benson Limketkai (bensonl@ai.sri.com)
00020   */
00021 
00022 #include <OpenKarto/Geometry.h>
00023 #include <OpenKarto/RigidBodyTransform.h>
00024 
00025 using namespace karto;
00026 
00027 RigidBodyTransform::RigidBodyTransform(const karto::Pose2& rPose)
00028 {
00029         SetTransform(karto::Pose2(), rPose);
00030 }
00031 
00032 RigidBodyTransform::RigidBodyTransform(const karto::Pose2& rPose1, const karto::Pose2& rPose2)
00033 {
00034   SetTransform(rPose1, rPose2);
00035 }
00036 
00037 void RigidBodyTransform::SetTransform(const karto::Pose2& rPose1, const karto::Pose2& rPose2)
00038 {
00039   m_Transform = rPose2;
00040   m_Transform.SetX(m_Transform.GetX() - rPose1.GetX());
00041   m_Transform.SetY(m_Transform.GetY() - rPose1.GetY());
00042 
00043   karto::Matrix3 rotation;
00044   rotation.FromAxisAngle(0, 0, 1, -rPose1.GetHeading());
00045   m_Transform = rotation * m_Transform;
00046   m_Transform.SetHeading(m_Transform.GetHeading() - rPose1.GetHeading());
00047 
00048   m_InvTransform = rPose1;
00049   m_InvTransform.SetX(m_InvTransform.GetX() - rPose2.GetX());
00050   m_InvTransform.SetY(m_InvTransform.GetY() - rPose2.GetY());
00051 
00052   karto::Matrix3 invRotation;
00053   invRotation.FromAxisAngle(0, 0, 1, -rPose2.GetHeading());
00054   m_InvTransform = invRotation * m_InvTransform;
00055   m_InvTransform.SetHeading(m_InvTransform.GetHeading() - rPose2.GetHeading());
00056 }
00057 
00058 karto::Pose2 RigidBodyTransform::TransformPose(const karto::Pose2& rSourcePose)
00059 {
00060   karto::Matrix3 rotation;
00061   rotation.FromAxisAngle(0, 0, 1, rSourcePose.GetHeading());
00062   karto::Pose2 newPose = rotation * m_Transform;
00063   newPose += rSourcePose;
00064 
00065   return newPose;
00066 }
00067 
00068 karto::Pose2 RigidBodyTransform::InverseTransformPose(const karto::Pose2& rSourcePose)
00069 {
00070   karto::Matrix3 rotation;
00071   rotation.FromAxisAngle(0, 0, 1, rSourcePose.GetHeading());
00072   karto::Pose2 newPose = rotation * m_InvTransform;
00073   newPose += rSourcePose;
00074 
00075   return newPose;
00076 }


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