Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
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
00049 m_Rotation.FromAxisAngle(0, 0, 1, rPose2.GetHeading() - rPose1.GetHeading());
00050 m_InverseRotation.FromAxisAngle(0, 0, 1, rPose1.GetHeading() - rPose2.GetHeading());
00051
00052
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 }