RotationVectorConstraintData.h
Go to the documentation of this file.
// This file is a part of the OpenSurgSim project.
// Copyright 2016, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef SURGSIM_PHYSICS_ROTATIONVECTORCONSTRAINTDATA_H
#define SURGSIM_PHYSICS_ROTATIONVECTORCONSTRAINTDATA_H
#include "SurgSim/Collision/CollisionPair.h"
#include "SurgSim/Physics/ConstraintData.h"
#include "SurgSim/Physics/Fem1DRepresentation.h"
#include "SurgSim/Physics/Fem1DElementBeam.h"
#include "SurgSim/Physics/RigidRepresentationBase.h"
#include "SurgSim/Math/Vector.h"
namespace SurgSim
{
namespace Physics
{
class RotationVectorRigidFem1DConstraintData : public ConstraintData
{
public:
RotationVectorRigidFem1DConstraintData() :
ConstraintData()
{
}
virtual ~RotationVectorRigidFem1DConstraintData()
{
}
void setRigidOrFixedRotation(std::shared_ptr<SurgSim::Physics::RigidRepresentationBase> rigid,
const SurgSim::Math::Matrix33d& rigidRAtGrasp)
{
SURGSIM_ASSERT(nullptr != rigid) << "Need a valid rigid/fixed representation";
m_rigidRAtGrasp = rigidRAtGrasp;
m_rigid = rigid;
}
void setFem1DRotation(std::shared_ptr<SurgSim::Physics::Fem1DRepresentation> beams, size_t beamId)
{
SURGSIM_ASSERT(nullptr != beams) << "Need a valid fem1D representation";
SURGSIM_ASSERT(beams->getNumFemElements() > beamId) << "The beam id " << beamId
<< " does not exists, the fem1d has " << beams->getNumFemElements() << " beams";
auto beam = std::dynamic_pointer_cast<SurgSim::Physics::Fem1DElementBeam>(beams->getFemElement(beamId));
m_beams = beams;
m_beamR0 = beam->getInitialRotation();
const auto& rotVecBeamNode0 = beams->getCurrentState()->getPositions().segment<3>(6 * beam->getNodeId(0) + 3);
SurgSim::Math::Matrix33d R = SurgSim::Math::Matrix33d::Identity();
if (!rotVecBeamNode0.isApprox(SurgSim::Math::Vector3d::Zero()))
{
R = SurgSim::Math::makeRotationMatrix(rotVecBeamNode0.norm(), rotVecBeamNode0.normalized());
}
m_beamRAtGrasp = R * m_beamR0;
}
SurgSim::Math::Vector3d getCurrentRotationVector() const
{
SURGSIM_ASSERT(nullptr != m_rigid) << "Did you call setRigidOrFixedRotation prior to using this class ?";
SURGSIM_ASSERT(nullptr != m_beams) << "Did you call setFem1DRotation prior to using this class ?";
const auto& rigidR = m_rigid->getCurrentState().getPose().linear();
Eigen::AngleAxisd angleAxis(rigidR * m_rigidRAtGrasp.inverse() * m_beamRAtGrasp * m_beamR0.inverse());
return angleAxis.angle() * angleAxis.axis();
}
private:
std::shared_ptr<SurgSim::Physics::RigidRepresentationBase> m_rigid;
SurgSim::Math::Matrix33d m_rigidRAtGrasp;
std::shared_ptr<SurgSim::Physics::Fem1DRepresentation> m_beams;
SurgSim::Math::Matrix33d m_beamR0, m_beamRAtGrasp;
};
}; // namespace Physics
}; // namespace SurgSim
#endif // SURGSIM_PHYSICS_ROTATIONVECTORCONSTRAINTDATA_H
Fem1DRepresentation.h
SurgSim
Definition: CompoundShapeToGraphics.cpp:29
Fem1DElementBeam.h
SURGSIM_ASSERT
#define SURGSIM_ASSERT(condition)
Assert that condition is true.
Definition: Assert.h:77
SurgSim::Physics::RotationVectorRigidFem1DConstraintData::getCurrentRotationVector
SurgSim::Math::Vector3d getCurrentRotationVector() const
Definition: RotationVectorConstraintData.h:95
SurgSim::Physics::ConstraintData
Base class for all CosntraintData Derived classes should be specific to a given constraint.
Definition: ConstraintData.h:27
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::Physics::RotationVectorRigidFem1DConstraintData::m_beamRAtGrasp
SurgSim::Math::Matrix33d m_beamRAtGrasp
Definition: RotationVectorConstraintData.h:114
RigidRepresentationBase.h
SurgSim::Physics::RotationVectorRigidFem1DConstraintData::~RotationVectorRigidFem1DConstraintData
virtual ~RotationVectorRigidFem1DConstraintData()
Destructor.
Definition: RotationVectorConstraintData.h:53
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
SurgSim::Physics::Fem1DElementBeam
1D FemElement based on a beam volume discretization with a fixed cross section
Definition: Fem1DElementBeam.h:37
SurgSim::Physics::RotationVectorRigidFem1DConstraintData
Class for rotation vector constraint data between a rigid/fixed representation and Fem1d beam being c...
Definition: RotationVectorConstraintData.h:43
SurgSim::Physics::RotationVectorRigidFem1DConstraintData::m_rigid
std::shared_ptr< SurgSim::Physics::RigidRepresentationBase > m_rigid
Rigid/Fixed representation.
Definition: RotationVectorConstraintData.h:107
Vector.h
Definitions of small fixed-size vector types.
SurgSim::Math::Matrix33d
Eigen::Matrix< double, 3, 3, Eigen::RowMajor > Matrix33d
A 3x3 matrix of doubles.
Definition: Matrix.h:51
ConstraintData.h
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::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_beams
std::shared_ptr< SurgSim::Physics::Fem1DRepresentation > m_beams
Fem1D representation.
Definition: RotationVectorConstraintData.h:112
SurgSim::Math::Vector3d
Eigen::Matrix< double, 3, 1 > Vector3d
A 3D vector of doubles.
Definition: Vector.h:57
CollisionPair.h
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::RotationVectorRigidFem1DConstraintData::RotationVectorRigidFem1DConstraintData
RotationVectorRigidFem1DConstraintData()
Default constructor.
Definition: RotationVectorConstraintData.h:47