Changeset 15 in flair-src for trunk/lib/FlairCore/src/Quaternion.cpp

Timestamp:

04/08/16 15:40:57 (8 years ago)

Author:

Bayard Gildas

Message:

sources reformatted with flair-format-dir script

File:

• trunk/lib/FlairCore/src/Quaternion.cpp (modified) (1 diff)

trunk/lib/FlairCore/src/Quaternion.cpp

@@ -26 +26 @@
 namespace core {
 
-Quaternion::Quaternion(float inQ0,float inQ1,float inQ2,float inQ3):q0(inQ0),q1(inQ1),q2(inQ2),q3(inQ3) {
-}
+Quaternion::Quaternion(float inQ0, float inQ1, float inQ2, float inQ3)
+    : q0(inQ0), q1(inQ1), q2(inQ2), q3(inQ3) {}
 
-Quaternion::~Quaternion() {
-}
+Quaternion::~Quaternion() {}
 
-Quaternion& Quaternion::operator=(const Quaternion &quaternion) {
-    q0=quaternion.q0;
-    q1=quaternion.q1;
-    q2=quaternion.q2;
-    q3=quaternion.q3;
-    return (*this);
+Quaternion &Quaternion::operator=(const Quaternion &quaternion) {
+  q0 = quaternion.q0;
+  q1 = quaternion.q1;
+  q2 = quaternion.q2;
+  q3 = quaternion.q3;
+  return (*this);
 }
 
 float Quaternion::GetNorm(void) const {
-    return sqrt(q0*q0+q1*q1+q2*q2+q3*q3);
+  return sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
 }
 
 void Quaternion::Normalize(void) {
-    float n=GetNorm();
-    if(n!=0) {
-        q0=q0/n;
-        q1=q1/n;
-        q2=q2/n;
-        q3=q3/n;
-    }
+  float n = GetNorm();
+  if (n != 0) {
+    q0 = q0 / n;
+    q1 = q1 / n;
+    q2 = q2 / n;
+    q3 = q3 / n;
+  }
 }
 
 void Quaternion::Conjugate(void) {
-    q1=-q1;
-    q2=-q2;
-    q3=-q3;
+  q1 = -q1;
+  q2 = -q2;
+  q3 = -q3;
 }
 
 Quaternion Quaternion::GetConjugate(void) {
-    return Quaternion(q0,-q1,-q2,-q3);
+  return Quaternion(q0, -q1, -q2, -q3);
 }
 
 void Quaternion::GetLogarithm(Vector3D &logarithm) {
-    Normalize();
-    float v_norm=sqrtf(q1*q1+q2*q2+q3*q3);
+  Normalize();
+  float v_norm = sqrtf(q1 * q1 + q2 * q2 + q3 * q3);
 
-    if(v_norm!=0) {
-        float v_arccos = acosf(q0);
-        logarithm.x=(q1*v_arccos)/v_norm;
-        logarithm.y=(q2*v_arccos)/v_norm;
-        logarithm.z=(q3*v_arccos)/v_norm;
-    } else {
-        logarithm.x=0;
-        logarithm.y=0;
-        logarithm.z=0;
-    }
+  if (v_norm != 0) {
+    float v_arccos = acosf(q0);
+    logarithm.x = (q1 * v_arccos) / v_norm;
+    logarithm.y = (q2 * v_arccos) / v_norm;
+    logarithm.z = (q3 * v_arccos) / v_norm;
+  } else {
+    logarithm.x = 0;
+    logarithm.y = 0;
+    logarithm.z = 0;
+  }
 }
 
 Vector3D Quaternion::GetLogarithm(void) {
-    Vector3D vector;
-    GetLogarithm(vector);
-    return vector;
+  Vector3D vector;
+  GetLogarithm(vector);
+  return vector;
 }
 
 Quaternion Quaternion::GetDerivative(const Vector3D &angularSpeed) const {
-    const Quaternion Qw(0,angularSpeed.x,angularSpeed.y,angularSpeed.z);
-    return 0.5*(*this)*Qw;
+  const Quaternion Qw(0, angularSpeed.x, angularSpeed.y, angularSpeed.z);
+  return 0.5 * (*this) * Qw;
 }
 
 void Quaternion::Derivate(const Vector3D &angularSpeed) {
-    Quaternion Q=GetDerivative(angularSpeed);
-    (*this)=Q;
+  Quaternion Q = GetDerivative(angularSpeed);
+  (*this) = Q;
 }
 
 void Quaternion::ToEuler(Euler &euler) const {
-    euler.roll=atan2(2*(q0*q1+q2*q3), 1-2*(q1*q1+q2*q2));
-    euler.pitch=asin(2*(q0*q2-q1*q3));
-    euler.yaw = atan2(2*(q0*q3+q1*q2), 1-2*(q2*q2+q3*q3));
+  euler.roll = atan2(2 * (q0 * q1 + q2 * q3), 1 - 2 * (q1 * q1 + q2 * q2));
+  euler.pitch = asin(2 * (q0 * q2 - q1 * q3));
+  euler.yaw = atan2(2 * (q0 * q3 + q1 * q2), 1 - 2 * (q2 * q2 + q3 * q3));
 }
 
 Euler Quaternion::ToEuler(void) const {
-    Euler euler;
-    ToEuler(euler);
-    return euler;
+  Euler euler;
+  ToEuler(euler);
+  return euler;
 }
 
 void Quaternion::ToRotationMatrix(RotationMatrix &matrix) const {
-    float aSq = q0 * q0;
-    float bSq = q1 * q1;
-    float cSq = q2 * q2;
-    float dSq = q3 * q3;
-    matrix.m[0][0] = aSq + bSq - cSq - dSq;
-    matrix.m[0][1] = 2.0f * (q1 * q2 - q0 * q3);
-    matrix.m[0][2] = 2.0f * (q0 * q2 + q1 * q3);
-    matrix.m[1][0] = 2.0f * (q1 * q2 + q0 * q3);
-    matrix.m[1][1] = aSq - bSq + cSq - dSq;
-    matrix.m[1][2] = 2.0f * (q2 * q3 - q0 * q1);
-    matrix.m[2][0] = 2.0f * (q1 * q3 - q0 * q2);
-    matrix.m[2][1] = 2.0f * (q0 * q1 + q2 * q3);
-    matrix.m[2][2] = aSq - bSq - cSq + dSq;
+  float aSq = q0 * q0;
+  float bSq = q1 * q1;
+  float cSq = q2 * q2;
+  float dSq = q3 * q3;
+  matrix.m[0][0] = aSq + bSq - cSq - dSq;
+  matrix.m[0][1] = 2.0f * (q1 * q2 - q0 * q3);
+  matrix.m[0][2] = 2.0f * (q0 * q2 + q1 * q3);
+  matrix.m[1][0] = 2.0f * (q1 * q2 + q0 * q3);
+  matrix.m[1][1] = aSq - bSq + cSq - dSq;
+  matrix.m[1][2] = 2.0f * (q2 * q3 - q0 * q1);
+  matrix.m[2][0] = 2.0f * (q1 * q3 - q0 * q2);
+  matrix.m[2][1] = 2.0f * (q0 * q1 + q2 * q3);
+  matrix.m[2][2] = aSq - bSq - cSq + dSq;
 }
 
 Quaternion &Quaternion::operator+=(const Quaternion &quaternion) {
-    q0+=quaternion.q0;
-    q1+=quaternion.q1;
-    q2+=quaternion.q2;
-    q3+=quaternion.q3;
-    return (*this);
+  q0 += quaternion.q0;
+  q1 += quaternion.q1;
+  q2 += quaternion.q2;
+  q3 += quaternion.q3;
+  return (*this);
 }
 
 Quaternion &Quaternion::operator-=(const Quaternion &quaternion) {
-    q0-=quaternion.q0;
-    q1-=quaternion.q1;
-    q2-=quaternion.q2;
-    q3-=quaternion.q3;
-    return (*this);
+  q0 -= quaternion.q0;
+  q1 -= quaternion.q1;
+  q2 -= quaternion.q2;
+  q3 -= quaternion.q3;
+  return (*this);
 }
 
-Quaternion operator + (const Quaternion &quaternionA,const Quaternion &quaterniontB) {
-    return Quaternion(
-               quaternionA.q0 + quaterniontB.q0,
-               quaternionA.q1 + quaterniontB.q1,
-               quaternionA.q2 + quaterniontB.q2,
-               quaternionA.q3 + quaterniontB.q3);
+Quaternion operator+(const Quaternion &quaternionA,
+                     const Quaternion &quaterniontB) {
+  return Quaternion(
+      quaternionA.q0 + quaterniontB.q0, quaternionA.q1 + quaterniontB.q1,
+      quaternionA.q2 + quaterniontB.q2, quaternionA.q3 + quaterniontB.q3);
 }
 
-Quaternion operator- (const Quaternion &quaterniontA,const Quaternion &quaterniontB) {
-   return Quaternion(
-               quaterniontA.q0 - quaterniontB.q0,
-               quaterniontA.q1 - quaterniontB.q1,
-               quaterniontA.q2 - quaterniontB.q2,
-               quaterniontA.q3 - quaterniontB.q3);
+Quaternion operator-(const Quaternion &quaterniontA,
+                     const Quaternion &quaterniontB) {
+  return Quaternion(
+      quaterniontA.q0 - quaterniontB.q0, quaterniontA.q1 - quaterniontB.q1,
+      quaterniontA.q2 - quaterniontB.q2, quaterniontA.q3 - quaterniontB.q3);
 }
 
 Quaternion operator-(const Quaternion &quaternion) {
-    return Quaternion(-quaternion.q0,-quaternion.q1,-quaternion.q2,-quaternion.q3);
+  return Quaternion(-quaternion.q0, -quaternion.q1, -quaternion.q2,
+                    -quaternion.q3);
 }
 
-Quaternion operator * (const Quaternion &quaterniontA,const Quaternion &quaterniontB) {
-    return Quaternion(
-               quaterniontA.q0 * quaterniontB.q0 - quaterniontA.q1 * quaterniontB.q1 - quaterniontA.q2 * quaterniontB.q2 - quaterniontA.q3 * quaterniontB.q3,
-               quaterniontA.q0 * quaterniontB.q1 + quaterniontA.q1 * quaterniontB.q0 + quaterniontA.q2 * quaterniontB.q3 - quaterniontA.q3 * quaterniontB.q2,
-               quaterniontA.q0 * quaterniontB.q2 - quaterniontA.q1 * quaterniontB.q3 + quaterniontA.q2 * quaterniontB.q0 + quaterniontA.q3 * quaterniontB.q1,
-               quaterniontA.q0 * quaterniontB.q3 + quaterniontA.q1 * quaterniontB.q2 - quaterniontA.q2 * quaterniontB.q1 + quaterniontA.q3 * quaterniontB.q0);
+Quaternion operator*(const Quaternion &quaterniontA,
+                     const Quaternion &quaterniontB) {
+  return Quaternion(
+      quaterniontA.q0 * quaterniontB.q0 - quaterniontA.q1 * quaterniontB.q1 -
+          quaterniontA.q2 * quaterniontB.q2 - quaterniontA.q3 * quaterniontB.q3,
+      quaterniontA.q0 * quaterniontB.q1 + quaterniontA.q1 * quaterniontB.q0 +
+          quaterniontA.q2 * quaterniontB.q3 - quaterniontA.q3 * quaterniontB.q2,
+      quaterniontA.q0 * quaterniontB.q2 - quaterniontA.q1 * quaterniontB.q3 +
+          quaterniontA.q2 * quaterniontB.q0 + quaterniontA.q3 * quaterniontB.q1,
+      quaterniontA.q0 * quaterniontB.q3 + quaterniontA.q1 * quaterniontB.q2 -
+          quaterniontA.q2 * quaterniontB.q1 +
+          quaterniontA.q3 * quaterniontB.q0);
 }
 
-Quaternion operator * (float coeff,const Quaternion &quaternion) {
-    return Quaternion(
-               coeff*quaternion.q0,
-               coeff*quaternion.q1,
-               coeff*quaternion.q2,
-               coeff*quaternion.q3);
+Quaternion operator*(float coeff, const Quaternion &quaternion) {
+  return Quaternion(coeff * quaternion.q0, coeff * quaternion.q1,
+                    coeff * quaternion.q2, coeff * quaternion.q3);
 }
 
-
-Quaternion operator * (const Quaternion &quaternion,float coeff) {
-    return Quaternion(
-               coeff*quaternion.q0,
-               coeff*quaternion.q1,
-               coeff*quaternion.q2,
-               coeff*quaternion.q3);
+Quaternion operator*(const Quaternion &quaternion, float coeff) {
+  return Quaternion(coeff * quaternion.q0, coeff * quaternion.q1,
+                    coeff * quaternion.q2, coeff * quaternion.q3);
 }