From 261ff062aebb53d8edbe0f1379f1de31ce7baa4f Mon Sep 17 00:00:00 2001
From: Henry <Henry>
Date: Thu, 19 Jul 2012 17:55:43 +0100
Subject: [PATCH] quaternion: added constructor from angle cosine and slerp
---
.../primitives/quaternion/quaternion.C | 25 +++++++++++++++++
.../primitives/quaternion/quaternion.H | 17 ++++++++++++
.../primitives/quaternion/quaternionI.H | 27 ++++++++++++++++---
3 files changed, 66 insertions(+), 3 deletions(-)
diff --git a/src/OpenFOAM/primitives/quaternion/quaternion.C b/src/OpenFOAM/primitives/quaternion/quaternion.C
index f7ff1904c90..0e7b1b8c3a1 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 730e86d68df..dbe7ad15b52 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 709ee52b6d9..1582f5c7337 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)
{
--
GitLab