Go to the documentation of this file.
16 #ifndef SURGSIM_PHYSICS_ROTATIONVECTORCONSTRAINTDATA_H
17 #define SURGSIM_PHYSICS_ROTATIONVECTORCONSTRAINTDATA_H
63 SURGSIM_ASSERT(
nullptr != rigid) <<
"Need a valid rigid/fixed representation";
72 void setFem1DRotation(std::shared_ptr<SurgSim::Physics::Fem1DRepresentation> beams,
size_t beamId)
74 SURGSIM_ASSERT(
nullptr != beams) <<
"Need a valid fem1D representation";
78 auto beam = std::dynamic_pointer_cast<SurgSim::Physics::Fem1DElementBeam>(beams->
getFemElement(beamId));
82 m_beamR0 = beam->getInitialRotation();
84 const auto& rotVecBeamNode0 = beams->
getCurrentState()->getPositions().segment<3>(6 * beam->getNodeId(0) + 3);
86 if (!rotVecBeamNode0.isApprox(SurgSim::Math::Vector3d::Zero()))
97 SURGSIM_ASSERT(
nullptr !=
m_rigid) <<
"Did you call setRigidOrFixedRotation prior to using this class ?";
100 const auto& rigidR =
m_rigid->getCurrentState().getPose().linear();
102 return angleAxis.angle() * angleAxis.axis();
107 std::shared_ptr<SurgSim::Physics::RigidRepresentationBase>
m_rigid;
112 std::shared_ptr<SurgSim::Physics::Fem1DRepresentation>
m_beams;
121 #endif // SURGSIM_PHYSICS_ROTATIONVECTORCONSTRAINTDATA_H
#define SURGSIM_ASSERT(condition)
Assert that condition is true.
Definition: Assert.h:77
std::shared_ptr< FemElement > getFemElement(size_t femElementId)
Retrieves a given FemElement from its id.
Definition: FemRepresentation.cpp:170
std::shared_ptr< SurgSim::Physics::RigidRepresentationBase > m_rigid
Rigid/Fixed representation.
Definition: RotationVectorConstraintData.h:107
SurgSim::Math::Vector3d getCurrentRotationVector() const
Definition: RotationVectorConstraintData.h:95
Eigen::Matrix< double, 3, 1 > Vector3d
A 3D vector of doubles.
Definition: Vector.h:57
SurgSim::Math::Matrix33d m_beamRAtGrasp
Definition: RotationVectorConstraintData.h:114
Definition: CompoundShapeToGraphics.cpp:29
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
Eigen::Matrix< double, 3, 3, Eigen::RowMajor > Matrix33d
A 3x3 matrix of doubles.
Definition: Matrix.h:51
size_t getNumFemElements() const
Gets the number of FemElement.
Definition: FemRepresentation.cpp:165
void setFem1DRotation(std::shared_ptr< SurgSim::Physics::Fem1DRepresentation > beams, size_t beamId)
Set the fem1d object part.
Definition: RotationVectorConstraintData.h:72
SurgSim::Math::Matrix33d m_rigidRAtGrasp
Rigid/Fixed rotation information at the time of the constraint creation.
Definition: RotationVectorConstraintData.h:109
Base class for all CosntraintData Derived classes should be specific to a given constraint.
Definition: ConstraintData.h:27
RotationVectorRigidFem1DConstraintData()
Default constructor.
Definition: RotationVectorConstraintData.h:47
SurgSim::Math::Matrix33d m_beamR0
The beam initial rotation and rotation at the time of the constraint creation.
Definition: RotationVectorConstraintData.h:114
virtual ~RotationVectorRigidFem1DConstraintData()
Destructor.
Definition: RotationVectorConstraintData.h:53
Class for rotation vector constraint data between a rigid/fixed representation and Fem1d beam being c...
Definition: RotationVectorConstraintData.h:43
std::shared_ptr< SurgSim::Physics::Fem1DRepresentation > m_beams
Fem1D representation.
Definition: RotationVectorConstraintData.h:112
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