PoseTransform.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2006-2011, SRI International (R)
3  *
4  * This program is free software: you can redistribute it and/or modify
5  * it under the terms of the GNU Lesser General Public License as published by
6  * the Free Software Foundation, either version 3 of the License, or
7  * (at your option) any later version.
8  *
9  * This program is distributed in the hope that it will be useful,
10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12  * GNU Lesser General Public License for more details.
13  *
14  * You should have received a copy of the GNU Lesser General Public License
15  * along with this program. If not, see <http://www.gnu.org/licenses/>.
16  */
17 
19 
20 namespace karto
21 {
22 
24  {
25  SetTransform(Pose2(), Pose2());
26  }
27 
29  {
30  SetTransform(Pose2(), rPose);
31  }
32 
33  Transform::Transform(const Pose2& rPose1, const Pose2& rPose2)
34  {
35  SetTransform(rPose1, rPose2);
36  }
37 
38  void Transform::SetTransform(const Pose2& rPose1, const Pose2& rPose2)
39  {
40  if (rPose1 == rPose2)
41  {
44  m_Transform = Pose2();
45  return;
46  }
47 
48  // heading transformation
49  m_Rotation.FromAxisAngle(0, 0, 1, rPose2.GetHeading() - rPose1.GetHeading());
50  m_InverseRotation.FromAxisAngle(0, 0, 1, rPose1.GetHeading() - rPose2.GetHeading());
51 
52  // position transformation
53  Pose2 newPosition;
54  if (rPose1.GetX() != 0.0 || rPose1.GetY() != 0.0)
55  {
56  newPosition = rPose2 - m_Rotation * rPose1;
57  }
58  else
59  {
60  newPosition = rPose2;
61  }
62 
63  m_Transform = Pose2(newPosition.GetPosition(), rPose2.GetHeading() - rPose1.GetHeading());
64  }
65 
66 }
kt_double GetHeading() const
Definition: Geometry.h:2274
kt_double GetX() const
Definition: Geometry.h:2220
void SetToIdentity()
Definition: Geometry.h:2580
const Vector2d & GetPosition() const
Definition: Geometry.h:2256
void SetTransform(const Pose2 &rPose1, const Pose2 &rPose2)
Matrix3 m_InverseRotation
Definition: PoseTransform.h:96
kt_double GetY() const
Definition: Geometry.h:2238
Definition: Any.cpp:20
void FromAxisAngle(kt_double x, kt_double y, kt_double z, const kt_double radians)
Definition: Geometry.h:2605


nav2d_karto
Author(s): Sebastian Kasperski
autogenerated on Thu Jun 6 2019 19:20:24