Commit 591cb1d1 authored by Henry Weller's avatar Henry Weller
Browse files

rigidBodyDynamics/rigidBodySolvers: Added support for the integration of quaternion joints

parent ab6fcff6
......@@ -33,7 +33,7 @@ Foam::RBD::rigidBodyModelState::rigidBodyModelState
)
:
q_(model.nDoF(), Zero),
w_(model.nw(), Zero),
w_(model.nw(), 1),
qDot_(model.nDoF(), Zero),
qDdot_(model.nDoF(), Zero)
{}
......@@ -46,7 +46,7 @@ Foam::RBD::rigidBodyModelState::rigidBodyModelState
)
:
q_(dict.lookupOrDefault("q", scalarField(model.nDoF(), Zero))),
w_(dict.lookupOrDefault("w", scalarField(model.nw(), Zero))),
w_(dict.lookupOrDefault("w", scalarField(model.nw(), 1))),
qDot_(dict.lookupOrDefault("qDot", scalarField(model.nDoF(), Zero))),
qDdot_(dict.lookupOrDefault("qDdot", scalarField(model.nDoF(), Zero)))
{}
......
......@@ -137,6 +137,9 @@ public:
//- Return the motion state
inline const rigidBodyModelState& state() const;
//- Return the motion state for modification
inline rigidBodyModelState& state();
// Edit
......
......@@ -38,6 +38,13 @@ Foam::RBD::rigidBodyMotion::state() const
}
inline Foam::RBD::rigidBodyModelState&
Foam::RBD::rigidBodyMotion::state()
{
return motionState_;
}
inline void Foam::RBD::rigidBodyMotion::newTime()
{
motionState0_ = motionState_;
......
......@@ -82,6 +82,8 @@ void Foam::RBD::rigidBodySolvers::CrankNicolson::solve
// Correct position
q() = q0() + deltaT()*(voc_*qDot() + (1 - voc_)*qDot0());
correctQuaternionJoints();
// Update the body-state
model_.forwardDynamicsCorrection(state());
}
......
......@@ -92,6 +92,8 @@ void Foam::RBD::rigidBodySolvers::Newmark::solve
+ deltaT()*qDot0()
+ sqr(deltaT())*beta_*qDdot() + sqr(deltaT())*(0.5 - beta_)*qDdot0();
correctQuaternionJoints();
// Update the body-state
model_.forwardDynamicsCorrection(state());
}
......
......@@ -52,4 +52,40 @@ Foam::RBD::rigidBodySolver::~rigidBodySolver()
{}
// * * * * * * * * * * * * * * * Member Functions * * * * * * * * * * * * * //
void Foam::RBD::rigidBodySolver::correctQuaternionJoints()
{
if (model_.nw())
{
forAll (model_.joints(), i)
{
const label qi = model_.joints()[i].qIndex();
if (model_.joints()[i].nw() == 1)
{
// Calculate the change in the normalized quaternion axis
vector dv((q().block<vector>(qi) - q0().block<vector>(qi)));
scalar magDv = mag(dv);
if (magDv > SMALL)
{
// Calculate the quaternion corresponding to the change
quaternion dQuat(dv/magDv, cos(magDv), true);
// Transform the previous time quaternion
quaternion quat
(
normalize(model_.joints()[i](q0(), w0())*dQuat)
);
// Update the joint state
model_.joints()[i](quat, q(), w());
}
}
}
}
}
// ************************************************************************* //
......@@ -100,6 +100,9 @@ protected:
//- Acceleration damping coefficient (for steady-state simulations)
inline scalar aDamp() const;
//- Correct the quaternion joints based on the current change in q
void correctQuaternionJoints();
public:
......
......@@ -73,6 +73,8 @@ void Foam::RBD::rigidBodySolvers::symplectic::solve
qDot() = qDot0() + aDamp()*0.5*deltaT0()*qDdot();
q() = q0() + deltaT()*qDot();
correctQuaternionJoints();
// Update the body-state prior to the evaluation of the restraints
model_.forwardDynamicsCorrection(state());
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment