diff --git a/src/OpenFOAM/primitives/quaternion/quaternion.C b/src/OpenFOAM/primitives/quaternion/quaternion.C index f7ff1904c90fb31ad33b72541ed17ee9a25014a7..0e7b1b8c3a115ec115922556387f5b217a4b80df 100644 --- a/src/OpenFOAM/primitives/quaternion/quaternion.C +++ b/src/OpenFOAM/primitives/quaternion/quaternion.C @@ -51,6 +51,31 @@ Foam::word Foam::name(const quaternion& q) } +Foam::quaternion Foam::slerp +( + const quaternion& qa, + const quaternion& qb, + const scalar t +) +{ + // Calculate angle between the quaternions + scalar cosHalfTheta = qa & qb; + + if (mag(cosHalfTheta) >= 1) + { + return qa; + } + + scalar halfTheta = acos(cosHalfTheta); + scalar sinHalfTheta = sqrt(1.0 - sqr(cosHalfTheta)); + + scalar wa = sin((1 - t)*halfTheta)/sinHalfTheta; + scalar wb = sin(t*halfTheta)/sinHalfTheta; + + return wa*qa + wb*qb; +} + + // * * * * * * * * * * * * * * * IOstream Operators * * * * * * * * * * * * // Foam::Istream& Foam::operator>>(Istream& is, quaternion& q) diff --git a/src/OpenFOAM/primitives/quaternion/quaternion.H b/src/OpenFOAM/primitives/quaternion/quaternion.H index 730e86d68df316b0a31fb5848d5d4534f54818f3..dbe7ad15b524edd0b36fcf1ba0953734887e21e0 100644 --- a/src/OpenFOAM/primitives/quaternion/quaternion.H +++ b/src/OpenFOAM/primitives/quaternion/quaternion.H @@ -106,6 +106,15 @@ public: // and angle theta inline quaternion(const vector& d, const scalar theta); + //- Construct a rotation quaternion given the direction d + // and cosine angle cosTheta and a if d is normalized + inline quaternion + ( + const vector& d, + const scalar cosTheta, + const bool normalized + ); + //- Construct given scalar part, the vector part = vector::zero inline explicit quaternion(const scalar w); @@ -209,6 +218,14 @@ inline quaternion inv(const quaternion& q); //- Return a string representation of a quaternion word name(const quaternion&); +//- Spherical linear interpolation of quaternions +quaternion slerp +( + const quaternion& qa, + const quaternion& qb, + const scalar t +); + //- Data associated with quaternion type are contiguous template<> inline bool contiguous<quaternion>() {return true;} diff --git a/src/OpenFOAM/primitives/quaternion/quaternionI.H b/src/OpenFOAM/primitives/quaternion/quaternionI.H index 709ee52b6d9ac7ddcdad5cf6a82f2d2ed65fd082..1582f5c7337abc70d46777dc0241b0614ed94606 100644 --- a/src/OpenFOAM/primitives/quaternion/quaternionI.H +++ b/src/OpenFOAM/primitives/quaternion/quaternionI.H @@ -40,6 +40,26 @@ inline Foam::quaternion::quaternion(const vector& d, const scalar theta) v_((sin(0.5*theta)/mag(d))*d) {} +inline Foam::quaternion::quaternion +( + const vector& d, + const scalar cosTheta, + const bool normalized +) +{ + scalar cosHalfTheta2 = 0.5*(cosTheta + 1); + w_ = sqrt(cosHalfTheta2); + + if (normalized) + { + v_ = sqrt(1 - cosHalfTheta2)*d; + } + else + { + v_ = (sqrt(1 - cosHalfTheta2)/mag(d))*d; + } +} + inline Foam::quaternion::quaternion(const scalar w) : w_(w), @@ -69,9 +89,10 @@ inline Foam::quaternion::quaternion const tensor& rotationTensor ) { - scalar trace = rotationTensor.xx() - + rotationTensor.yy() - + rotationTensor.zz(); + scalar trace = + rotationTensor.xx() + + rotationTensor.yy() + + rotationTensor.zz(); if (trace > 0) {