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";
75 SURGSIM_ASSERT(beams->getNumFemElements() > beamId) <<
"The beam id " << beamId
76 <<
" does not exists, the fem1d has " << beams->getNumFemElements() <<
" beams";
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
Definition: CompoundShapeToGraphics.cpp:29
#define SURGSIM_ASSERT(condition)
Assert that condition is true.
Definition: Assert.h:77
SurgSim::Math::Vector3d getCurrentRotationVector() const
Definition: RotationVectorConstraintData.h:95
Base class for all CosntraintData Derived classes should be specific to a given constraint.
Definition: ConstraintData.h:27
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 m_beamRAtGrasp
Definition: RotationVectorConstraintData.h:114
virtual ~RotationVectorRigidFem1DConstraintData()
Destructor.
Definition: RotationVectorConstraintData.h:53
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
1D FemElement based on a beam volume discretization with a fixed cross section
Definition: Fem1DElementBeam.h:37
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::RigidRepresentationBase > m_rigid
Rigid/Fixed representation.
Definition: RotationVectorConstraintData.h:107
Definitions of small fixed-size vector types.
Eigen::Matrix< double, 3, 3, Eigen::RowMajor > Matrix33d
A 3x3 matrix of doubles.
Definition: Matrix.h:51
SurgSim::Math::Matrix33d m_beamR0
The beam initial rotation and rotation at the time of the constraint creation.
Definition: RotationVectorConstraintData.h:114
void setFem1DRotation(std::shared_ptr< SurgSim::Physics::Fem1DRepresentation > beams, size_t beamId)
Set the fem1d object part.
Definition: RotationVectorConstraintData.h:72
std::shared_ptr< SurgSim::Physics::Fem1DRepresentation > m_beams
Fem1D representation.
Definition: RotationVectorConstraintData.h:112
Eigen::Matrix< double, 3, 1 > Vector3d
A 3D vector of doubles.
Definition: Vector.h:57
SurgSim::Math::Matrix33d m_rigidRAtGrasp
Rigid/Fixed rotation information at the time of the constraint creation.
Definition: RotationVectorConstraintData.h:109
RotationVectorRigidFem1DConstraintData()
Default constructor.
Definition: RotationVectorConstraintData.h:47