FixedRepresentationLocalization.h
Go to the documentation of this file.
1 // This file is a part of the OpenSurgSim project.
2 // Copyright 2013, SimQuest Solutions Inc.
3 //
4 // Licensed under the Apache License, Version 2.0 (the "License");
5 // you may not use this file except in compliance with the License.
6 // You may obtain a copy of the License at
7 //
8 // http://www.apache.org/licenses/LICENSE-2.0
9 //
10 // Unless required by applicable law or agreed to in writing, software
11 // distributed under the License is distributed on an "AS IS" BASIS,
12 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 // See the License for the specific language governing permissions and
14 // limitations under the License.
15 
16 #ifndef SURGSIM_PHYSICS_FIXEDREPRESENTATIONLOCALIZATION_H
17 #define SURGSIM_PHYSICS_FIXEDREPRESENTATIONLOCALIZATION_H
18 
21 
24 
25 namespace SurgSim
26 {
27 
28 namespace Physics
29 {
30 
33 {
34 public:
37  Localization()
38  {
39  }
40 
43  explicit FixedRepresentationLocalization(std::shared_ptr<Representation> representation) :
44  Localization(representation)
45  {
46  std::shared_ptr<FixedRepresentation> fixed = std::dynamic_pointer_cast<FixedRepresentation>(representation);
47  SURGSIM_ASSERT(fixed != nullptr) << "Unexpected representation type" << std::endl;
48  }
49 
52  {
53  }
54 
58  {
59  m_position = p;
60  }
61 
65  {
66  return m_position;
67  }
68 
69 private:
75  {
76  std::shared_ptr<FixedRepresentation> fixed = std::static_pointer_cast<FixedRepresentation>(getRepresentation());
77  const SurgSim::Math::RigidTransform3d& currentPose = fixed->getCurrentState().getPose();
78  const SurgSim::Math::RigidTransform3d& previousPose = fixed->getPreviousState().getPose();
79 
80  if (time == 0.0)
81  {
82  return previousPose * m_position;
83  }
84  else if (time == 1.0)
85  {
86  return currentPose * m_position;
87  }
88  else if (currentPose.isApprox(previousPose))
89  {
90  return currentPose * m_position;
91  }
92 
93  SurgSim::Math::RigidTransform3d pose = SurgSim::Math::interpolate(previousPose, currentPose, time);
94  return pose * m_position;
95  }
96 
99 };
100 
101 }; // namespace Physics
102 
103 }; // namespace SurgSim
104 
105 #endif // SURGSIM_PHYSICS_FIXEDREPRESENTATIONLOCALIZATION_H
SurgSim::Math::Vector3d doCalculatePosition(double time)
Calculates the global position of this localization.
Definition: FixedRepresentationLocalization.h:74
Definitions of quaternion types.
Definition: DriveElementFromInputBehavior.cpp:27
FixedRepresentationLocalization(std::shared_ptr< Representation > representation)
Constructor.
Definition: FixedRepresentationLocalization.h:43
const SurgSim::Math::RigidTransform3d & getPose() const
Get the rigid representation pose.
Definition: RigidRepresentationState.cpp:108
virtual ~FixedRepresentationLocalization()
Destructor.
Definition: FixedRepresentationLocalization.h:51
The FixedRepresentation class represents a physics entity without any motion nor compliance against w...
Definition: FixedRepresentation.h:33
#define SURGSIM_ASSERT(condition)
Assert that condition is true.
Definition: Assert.h:77
std::shared_ptr< Representation > getRepresentation() const
Gets the representation.
Definition: Localization.h:62
Eigen::Quaternion< T, QOpt > interpolate(const Eigen::Quaternion< T, QOpt > &q0, const Eigen::Quaternion< T, QOpt > &q1, T t)
Interpolate (slerp) between 2 quaternions.
Definition: Quaternion.h:149
void setLocalPosition(const SurgSim::Math::Vector3d &p)
Sets the local position.
Definition: FixedRepresentationLocalization.h:57
This class implement the localization on a FixedRepresentation, as a local position.
Definition: FixedRepresentationLocalization.h:32
FixedRepresentationLocalization()
Default constructor.
Definition: FixedRepresentationLocalization.h:36
Definitions of 2x2 and 3x3 rigid (isometric) transforms.
Eigen::Transform< double, 3, Eigen::Isometry > RigidTransform3d
A 3D rigid (isometric) transform, represented as doubles.
Definition: RigidTransform.h:46
const RigidRepresentationState & getCurrentState() const
Get the current state of the rigid representation.
Definition: RigidRepresentationBase.cpp:111
const SurgSim::Math::Vector3d & getLocalPosition() const
Gets the local position.
Definition: FixedRepresentationLocalization.h:64
This class localize a point on a representation (representation specific)
Definition: Localization.h:33
Eigen::Matrix< double, 3, 1 > Vector3d
A 3D vector of doubles.
Definition: Vector.h:56
SurgSim::Math::Vector3d m_position
3D position in local coordinates
Definition: FixedRepresentationLocalization.h:98