Commit bc70e369 authored by Henry Weller's avatar Henry Weller
Browse files

rigidBodyDynamics: Removed quaternion counter and index: 'nw', 'wIndex'

Replaced with 'unitQuaterion()' virtual function to indicate if the
joint uses a unit quaternion to represent rotation.
parent df1cb90d
......@@ -61,7 +61,7 @@ int main(int argc, char *argv[])
Field<spatialVector> fx(sphericalJoint.nBodies(), Zero);
// Set the angle of the pendulum to 0.3rad
sphericalJoint.joints()[1]
sphericalJoint.joints()[1].unitQuaternion
(
quaternion(quaternion::ZYX, vector(0.3, 0, 0)),
sphericalJoint.state().q()
......@@ -87,7 +87,7 @@ int main(int argc, char *argv[])
// using 'gnuplot sphericalJoint.gnuplot'
omegaFile
<< t << " "
<< sphericalJoint.joints()[1]
<< sphericalJoint.joints()[1].unitQuaternion
(
sphericalJoint.state().q()
).eulerAngles(quaternion::ZYX).x()
......
......@@ -84,9 +84,9 @@ Foam::RBD::joints::Rs::~Rs()
// * * * * * * * * * * * * * * Member Functions * * * * * * * * * * * * * * //
Foam::label Foam::RBD::joints::Rs::nw() const
bool Foam::RBD::joints::Rs::unitQuaternion() const
{
return 1;
return true;
}
......@@ -97,7 +97,7 @@ void Foam::RBD::joints::Rs::jcalc
const scalarField& qDot
) const
{
J.X.E() = operator()(q).R().T();
J.X.E() = joint::unitQuaternion(q).R().T();
J.X.r() = Zero;
J.S = Zero;
......
......@@ -88,9 +88,8 @@ public:
// Member Functions
//- Return the number of additional state variable for this joint
// For the quaternion this is 1 in addition to the 3 stored in q
virtual label nw() const;
//- Return true as this joint describes rotation using a quaternion
virtual bool unitQuaternion() const;
//- Update the model state for this joint
virtual void jcalc
......
......@@ -86,12 +86,6 @@ Foam::RBD::joints::composite::~composite()
// * * * * * * * * * * * * * * Member Functions * * * * * * * * * * * * * * //
Foam::label Foam::RBD::joints::composite::nw() const
{
return last().nw();
}
void Foam::RBD::joints::composite::jcalc
(
joint::XSvc& J,
......
......@@ -101,9 +101,6 @@ public:
// Member Functions
//- Return the number of additional state variables need by this joint
virtual label nw() const;
//- Update the model state for this joint
virtual void jcalc
(
......
......@@ -93,9 +93,6 @@ protected:
//- Index of this joints data in the rigidBodyModel state
label qIndex_;
//- Index of this joints quaternion data in the rigidBodyModel state
label wIndex_;
private:
......@@ -113,12 +110,6 @@ private:
return qIndex_;
}
//- Allow the rigidBodyModel to set the wIndex for this joint
label& wIndex()
{
return wIndex_;
}
public:
......@@ -213,8 +204,8 @@ public:
//- Return the number of degrees of freedom in this joint
inline label nDoF() const;
//- Return the number of additional state variables need by this joint
inline virtual label nw() const;
//- Return true if this joint describes rotation using a quaternion
inline virtual bool unitQuaternion() const;
//- Return the index of this joint in the model
inline label index() const;
......@@ -223,10 +214,6 @@ public:
// in the rigidBodyModel state fields
inline label qIndex() const;
//- Return start index for the additional state variables for this joint
// in the rigidBodyModel w state field
inline label wIndex() const;
//- Return the joint motion sub-space
inline const List<spatialVector>& S() const;
......@@ -245,16 +232,16 @@ public:
// Member Operators
//- Return the state quaternion for this joint
//- Return the unit quaternion for this joint
// if it uses a quaternion representation for rotation
inline quaternion operator()
inline quaternion unitQuaternion
(
const scalarField& q
) const;
//- Set the state quaternion for this joint
//- Set the unit quaternion for this joint
// if it uses a quaternion representation for rotation
inline void operator()
inline void unitQuaternion
(
const quaternion& quat,
scalarField& q
......
......@@ -29,8 +29,7 @@ inline Foam::RBD::joint::joint(const label nDoF)
:
S_(nDoF),
index_(0),
qIndex_(0),
wIndex_(0)
qIndex_(0)
{}
......@@ -41,9 +40,9 @@ inline Foam::label Foam::RBD::joint::nDoF() const
return S_.size();
}
inline Foam::label Foam::RBD::joint::nw() const
inline bool Foam::RBD::joint::unitQuaternion() const
{
return 0;
return false;
}
inline Foam::label Foam::RBD::joint::index() const
......@@ -56,11 +55,6 @@ inline Foam::label Foam::RBD::joint::qIndex() const
return qIndex_;
}
inline Foam::label Foam::RBD::joint::wIndex() const
{
return wIndex_;
}
inline const Foam::List<Foam::spatialVector>& Foam::RBD::joint::S() const
{
return S_;
......@@ -69,12 +63,12 @@ inline const Foam::List<Foam::spatialVector>& Foam::RBD::joint::S() const
// * * * * * * * * * * * * * * * Member Operators * * * * * * * * * * * * * //
inline Foam::quaternion Foam::RBD::joint::operator()
inline Foam::quaternion Foam::RBD::joint::unitQuaternion
(
const scalarField& q
) const
{
if (nw() != 1)
if (!unitQuaternion())
{
FatalErrorInFunction
<< "Attempt to get the quaternion for a non-spherical joint"
......@@ -85,13 +79,13 @@ inline Foam::quaternion Foam::RBD::joint::operator()
}
inline void Foam::RBD::joint::operator()
inline void Foam::RBD::joint::unitQuaternion
(
const quaternion& quat,
scalarField& q
) const
{
if (nw() != 1)
if (!unitQuaternion())
{
FatalErrorInFunction
<< "Attempt to set quaternion for a non-spherical joint"
......
......@@ -52,7 +52,7 @@ void Foam::RBD::rigidBodyModel::initializeRootBody()
XT_.append(spatialTransform());
nDoF_ = 0;
nw_ = 0;
unitQuaternions_ = false;
resizeState();
}
......@@ -204,11 +204,10 @@ Foam::label Foam::RBD::rigidBodyModel::join_
joint& curJoint = joints_[joints_.size() - 1];
curJoint.index() = joints_.size() - 1;
curJoint.qIndex() = prevJoint.qIndex() + prevJoint.nDoF();
curJoint.wIndex() = prevJoint.wIndex() + prevJoint.nw();
// Increment the degrees of freedom
nDoF_ += curJoint.nDoF();
nw_ += curJoint.nw();
unitQuaternions_ = unitQuaternions_ || curJoint.unitQuaternion();
resizeState();
......
......@@ -123,11 +123,8 @@ protected:
// used to set the size of the of joint state fields q, qDot and qDdot.
label nDoF_;
//- The number of additional state variable needed to describe the joint
// states. Typically this is the number of quaternion joints for which
// the 'w' element is additional to the 3-degrees of freedom of the
// joint.
label nw_;
//- True if any of the joints using quaternions
bool unitQuaternions_;
//- Motion restraints
PtrList<restraint> restraints_;
......@@ -242,11 +239,8 @@ public:
// used to set the size of the of joint state fields q, qDot and qDdot.
inline label nDoF() const;
//- Return the number of additional state variable needed to describe
// the joint states. Typically this is the number of quaternion
// joints for which the 'w' element is additional to the 3-degrees of
// freedom of the joint.
inline label nw() const;
//- Return true if any of the joints using quaternions
inline bool unitQuaternions() const;
//- Return the acceleration due to gravity
inline const vector& g() const;
......
......@@ -58,9 +58,9 @@ inline Foam::label Foam::RBD::rigidBodyModel::nDoF() const
}
inline Foam::label Foam::RBD::rigidBodyModel::nw() const
inline bool Foam::RBD::rigidBodyModel::unitQuaternions() const
{
return nw_;
return unitQuaternions_;
}
......
......@@ -56,13 +56,13 @@ Foam::RBD::rigidBodySolver::~rigidBodySolver()
void Foam::RBD::rigidBodySolver::correctQuaternionJoints()
{
if (model_.nw())
if (model_.unitQuaternions())
{
forAll (model_.joints(), i)
{
const label qi = model_.joints()[i].qIndex();
if (model_.joints()[i].nw() == 1)
if (model_.joints()[i].unitQuaternion())
{
// Calculate the change in the normalized quaternion axis
vector dv((q().block<vector>(qi) - q0().block<vector>(qi)));
......@@ -76,11 +76,11 @@ void Foam::RBD::rigidBodySolver::correctQuaternionJoints()
// Transform the previous time quaternion
quaternion quat
(
normalize(model_.joints()[i](q0())*dQuat)
normalize(model_.joints()[i].unitQuaternion(q0())*dQuat)
);
// Update the joint state
model_.joints()[i](quat, q());
model_.joints()[i].unitQuaternion(quat, q());
}
}
}
......
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