RotationVectorConstraintData.h
Go to the documentation of this file.
1 // This file is a part of the OpenSurgSim project.
2 // Copyright 2016, 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_ROTATIONVECTORCONSTRAINTDATA_H
17 #define SURGSIM_PHYSICS_ROTATIONVECTORCONSTRAINTDATA_H
18 
24 #include "SurgSim/Math/Vector.h"
25 
26 namespace SurgSim
27 {
28 
29 namespace Physics
30 {
31 
44 {
45 public:
49  {
50  }
51 
54  {
55  }
56 
60  void setRigidOrFixedRotation(std::shared_ptr<SurgSim::Physics::RigidRepresentationBase> rigid,
61  const SurgSim::Math::Matrix33d& rigidRAtGrasp)
62  {
63  SURGSIM_ASSERT(nullptr != rigid) << "Need a valid rigid/fixed representation";
64 
65  m_rigidRAtGrasp = rigidRAtGrasp;
66  m_rigid = rigid;
67  }
68 
72  void setFem1DRotation(std::shared_ptr<SurgSim::Physics::Fem1DRepresentation> beams, size_t beamId)
73  {
74  SURGSIM_ASSERT(nullptr != beams) << "Need a valid fem1D representation";
75  SURGSIM_ASSERT(beams->getNumFemElements() > beamId) << "The beam id " << beamId
76  << " does not exists, the fem1d has " << beams->getNumFemElements() << " beams";
77 
78  auto beam = std::dynamic_pointer_cast<SurgSim::Physics::Fem1DElementBeam>(beams->getFemElement(beamId));
79 
80  m_beams = beams;
81 
82  m_beamR0 = beam->getInitialRotation();
83 
84  const auto& rotVecBeamNode0 = beams->getCurrentState()->getPositions().segment<3>(6 * beam->getNodeId(0) + 3);
85  SurgSim::Math::Matrix33d R = SurgSim::Math::Matrix33d::Identity();
86  if (!rotVecBeamNode0.isApprox(SurgSim::Math::Vector3d::Zero()))
87  {
88  R = SurgSim::Math::makeRotationMatrix(rotVecBeamNode0.norm(), rotVecBeamNode0.normalized());
89  }
91  }
92 
96  {
97  SURGSIM_ASSERT(nullptr != m_rigid) << "Did you call setRigidOrFixedRotation prior to using this class ?";
98  SURGSIM_ASSERT(nullptr != m_beams) << "Did you call setFem1DRotation prior to using this class ?";
99 
100  const auto& rigidR = m_rigid->getCurrentState().getPose().linear();
101  Eigen::AngleAxisd angleAxis(rigidR * m_rigidRAtGrasp.inverse() * m_beamRAtGrasp * m_beamR0.inverse());
102  return angleAxis.angle() * angleAxis.axis();
103  }
104 
105 private:
107  std::shared_ptr<SurgSim::Physics::RigidRepresentationBase> m_rigid;
110 
112  std::shared_ptr<SurgSim::Physics::Fem1DRepresentation> m_beams;
115 };
116 
117 }; // namespace Physics
118 
119 }; // namespace SurgSim
120 
121 #endif // SURGSIM_PHYSICS_ROTATIONVECTORCONSTRAINTDATA_H
SURGSIM_ASSERT
#define SURGSIM_ASSERT(condition)
Assert that condition is true.
Definition: Assert.h:77
SurgSim::Physics::FemRepresentation::getFemElement
std::shared_ptr< FemElement > getFemElement(size_t femElementId)
Retrieves a given FemElement from its id.
Definition: FemRepresentation.cpp:170
CollisionPair.h
Vector.h
SurgSim::Physics::RotationVectorRigidFem1DConstraintData::m_rigid
std::shared_ptr< SurgSim::Physics::RigidRepresentationBase > m_rigid
Rigid/Fixed representation.
Definition: RotationVectorConstraintData.h:107
SurgSim::Physics::RotationVectorRigidFem1DConstraintData::getCurrentRotationVector
SurgSim::Math::Vector3d getCurrentRotationVector() const
Definition: RotationVectorConstraintData.h:95
SurgSim::Math::Vector3d
Eigen::Matrix< double, 3, 1 > Vector3d
A 3D vector of doubles.
Definition: Vector.h:57
SurgSim::Physics::RotationVectorRigidFem1DConstraintData::m_beamRAtGrasp
SurgSim::Math::Matrix33d m_beamRAtGrasp
Definition: RotationVectorConstraintData.h:114
SurgSim
Definition: CompoundShapeToGraphics.cpp:29
SurgSim::Physics::RotationVectorRigidFem1DConstraintData::setRigidOrFixedRotation
void setRigidOrFixedRotation(std::shared_ptr< SurgSim::Physics::RigidRepresentationBase > rigid, const SurgSim::Math::Matrix33d &rigidRAtGrasp)
Set the rigid/fixed object part that will control the fem1d.
Definition: RotationVectorConstraintData.h:60
SurgSim::Math::Matrix33d
Eigen::Matrix< double, 3, 3, Eigen::RowMajor > Matrix33d
A 3x3 matrix of doubles.
Definition: Matrix.h:51
SurgSim::Physics::FemRepresentation::getNumFemElements
size_t getNumFemElements() const
Gets the number of FemElement.
Definition: FemRepresentation.cpp:165
SurgSim::Physics::RotationVectorRigidFem1DConstraintData::setFem1DRotation
void setFem1DRotation(std::shared_ptr< SurgSim::Physics::Fem1DRepresentation > beams, size_t beamId)
Set the fem1d object part.
Definition: RotationVectorConstraintData.h:72
SurgSim::Physics::RotationVectorRigidFem1DConstraintData::m_rigidRAtGrasp
SurgSim::Math::Matrix33d m_rigidRAtGrasp
Rigid/Fixed rotation information at the time of the constraint creation.
Definition: RotationVectorConstraintData.h:109
SurgSim::Physics::ConstraintData
Base class for all CosntraintData Derived classes should be specific to a given constraint.
Definition: ConstraintData.h:27
SurgSim::Physics::RotationVectorRigidFem1DConstraintData::RotationVectorRigidFem1DConstraintData
RotationVectorRigidFem1DConstraintData()
Default constructor.
Definition: RotationVectorConstraintData.h:47
ConstraintData.h
SurgSim::Physics::DeformableRepresentation::getCurrentState
virtual const std::shared_ptr< SurgSim::Math::OdeState > getCurrentState() const
Return the current state of the deformable representation.
Definition: DeformableRepresentation.cpp:110
SurgSim::Physics::RotationVectorRigidFem1DConstraintData::m_beamR0
SurgSim::Math::Matrix33d m_beamR0
The beam initial rotation and rotation at the time of the constraint creation.
Definition: RotationVectorConstraintData.h:114
SurgSim::Physics::RotationVectorRigidFem1DConstraintData::~RotationVectorRigidFem1DConstraintData
virtual ~RotationVectorRigidFem1DConstraintData()
Destructor.
Definition: RotationVectorConstraintData.h:53
RigidRepresentationBase.h
Fem1DElementBeam.h
SurgSim::Physics::RotationVectorRigidFem1DConstraintData
Class for rotation vector constraint data between a rigid/fixed representation and Fem1d beam being c...
Definition: RotationVectorConstraintData.h:43
Fem1DRepresentation.h
SurgSim::Physics::RotationVectorRigidFem1DConstraintData::m_beams
std::shared_ptr< SurgSim::Physics::Fem1DRepresentation > m_beams
Fem1D representation.
Definition: RotationVectorConstraintData.h:112
SurgSim::Math::makeRotationMatrix
Eigen::Matrix< T, 3, 3 > makeRotationMatrix(const T &angle, const Eigen::Matrix< T, 3, 1, VOpt > &axis)
Create a rotation matrix corresponding to the specified angle (in radians) and axis.
Definition: Matrix.h:74