diff --git a/src/ODE/sixDOF/finiteRotation/finiteRotation.C b/src/ODE/sixDOF/finiteRotation/finiteRotation.C index 645724186..e52542116 100644 --- a/src/ODE/sixDOF/finiteRotation/finiteRotation.C +++ b/src/ODE/sixDOF/finiteRotation/finiteRotation.C @@ -26,7 +26,8 @@ Class Author Dubravko Matijasevic, FSB Zagreb. All rights reserved. - Update by Hrvoje Jasak + Hrvoje Jasak, FSB Zagreb. All rights reserved. + Vuko Vukcevic, FSB Zagreb. All rights reserved. \*---------------------------------------------------------------------------*/ @@ -82,9 +83,8 @@ Foam::vector Foam::finiteRotation::eulerAngles(const tensor& rotT) // Calculate roll angle rollAngle = atan2(rotT.yz(), rotT.zz()); - // Use mag to avoid negative value due to round-off - // HJ, 24/Feb/2016 - // Bugfix: sqr. SS, 18/Apr/2016 + // Use mag to avoid negative value due to round-off, HJ, 24/Feb/2016 + // Bugfix: missing sqr. SS, 18/Apr/2016 const scalar c2 = sqrt(sqr(rotT.xx()) + sqr(rotT.xy())); // Calculate pitch angle @@ -122,6 +122,14 @@ Foam::finiteRotation::finiteRotation {} +Foam::finiteRotation::finiteRotation(const tensor& R) +: + eInitial_(R), + rotTensor_(R), + rotIncrementTensor_(tensor::zero) +{} + + // * * * * * * * * * * * * * * * * Destructor * * * * * * * * * * * * * * * // Foam::finiteRotation::~finiteRotation() @@ -130,12 +138,23 @@ Foam::finiteRotation::~finiteRotation() // * * * * * * * * * * * * * * * Member Functions * * * * * * * * * * * * * // +void Foam::finiteRotation::updateRotation(const tensor& R) +{ + rotIncrementTensor_ = (R & rotTensor_.T()); + rotTensor_ = R; +} + + void Foam::finiteRotation::updateRotation(const HamiltonRodriguezRot& rot) { - tensor rotR = rot.R(); + updateRotation(rot.R()); +} - rotIncrementTensor_ = (rotR & (rotTensor_.T())); - rotTensor_ = rotR; + +void Foam::finiteRotation::updateRotationGivenIncrement(const tensor& incR) +{ + rotIncrementTensor_ = incR; + rotTensor_ = (incR & rotTensor_); } diff --git a/src/ODE/sixDOF/finiteRotation/finiteRotation.H b/src/ODE/sixDOF/finiteRotation/finiteRotation.H index 8d3f97822..cf496f375 100644 --- a/src/ODE/sixDOF/finiteRotation/finiteRotation.H +++ b/src/ODE/sixDOF/finiteRotation/finiteRotation.H @@ -26,7 +26,8 @@ Class Author Dubravko Matijasevic, FSB Zagreb. All rights reserved. - Updated by Hrvoje Jasak + Hrvoje Jasak, FSB Zagreb. All rights reserved. + Vuko Vukcevic, FSB Zagreb. All rights reserved. SourceFiles finiteRotation.C @@ -89,6 +90,9 @@ public: const scalar& angle ); + //- Construct from rotation tensor + explicit finiteRotation(const tensor& R); + // Destructor @@ -97,9 +101,15 @@ public: // Member Functions - //- Update rotation + //- Update rotation given rotation tensor + void updateRotation(const tensor& R); + + //- Update rotation given HamiltonRodriguezRot (quaternions) void updateRotation(const HamiltonRodriguezRot& rot); + //- Update rotation given increment rotation tensor + void updateRotationGivenIncrement(const tensor& incR); + //- Return initial quaternions const HamiltonRodriguezRot& eInitial() const; diff --git a/src/ODE/sixDOF/quaternions/HamiltonRodriguezRot.H b/src/ODE/sixDOF/quaternions/HamiltonRodriguezRot.H index a09ca6616..55f7f129b 100644 --- a/src/ODE/sixDOF/quaternions/HamiltonRodriguezRot.H +++ b/src/ODE/sixDOF/quaternions/HamiltonRodriguezRot.H @@ -26,7 +26,8 @@ Class Author Dubravko Matijasevic, FSB Zagreb. All rights reserved. - Updated by Hrvoje Jasak + Hrvoje Jasak, FSB Zagreb. All rights reserved. + Vuko Vukcevic, FSB Zagreb. All rights reserved. Description Rotation defined with 4 parameters: quaternions. @@ -66,13 +67,13 @@ class HamiltonRodriguezRot scalarRectangularMatrix Gt_; //- Inertial to rotated coordinate system transformation - mutable tensor R_; + tensor R_; // Private member functions //- Calculate R_ - inertial to body coord. sys. rotation - inline void calculateR() const + inline void calculateR() { R_.xx() = 2*(e1_*e1_ + e0_*e0_ - 0.5); R_.xy() = 2*(e1_*e2_ + e0_*e3_); @@ -133,11 +134,7 @@ public: } //- Construct from rotation vector and angle - explicit HamiltonRodriguezRot - ( - const vector& rVect, - const scalar& rAngle - ) + HamiltonRodriguezRot(const vector& rVect, const scalar& rAngle) : Gt_(4, 3), R_(tensor::zero) @@ -164,6 +161,29 @@ public: calculateGt(); } + //- Construct from rotation tensor + explicit HamiltonRodriguezRot(const tensor& R) + : + Gt_(4, 3), + R_(R) + { + // Calculate Hamilton - Rodriguez (Euler) parameters from rotation + // matrix + + // Note: sign of e0_ assumed positive + e0_ = Foam::sqrt((tr(R) + 1.0)/4.0); + + // Helper variable + const scalar oneByFourEo = 1.0/(4.0*e0_); + + e1_ = oneByFourEo*(R.zy() - R.yz()); + e2_ = oneByFourEo*(R.xz() - R.zx()); + e3_ = oneByFourEo*(R.yx() - R.xy()); + + // Calculate Gt + calculateGt(); + } + // Destructor