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


Ignore:
Timestamp:
Apr 8, 2016, 3:40:57 PM (6 years ago)
Author:
Bayard Gildas
Message:

sources reformatted with flair-format-dir script

File:
1 edited

Legend:

Unmodified
Added
Removed
  • trunk/lib/FlairCore/src/Quaternion.cpp

    r2 r15  
    2626namespace core {
    2727
    28 Quaternion::Quaternion(float inQ0,float inQ1,float inQ2,float inQ3):q0(inQ0),q1(inQ1),q2(inQ2),q3(inQ3) {
    29 }
     28Quaternion::Quaternion(float inQ0, float inQ1, float inQ2, float inQ3)
     29    : q0(inQ0), q1(inQ1), q2(inQ2), q3(inQ3) {}
    3030
    31 Quaternion::~Quaternion() {
    32 }
     31Quaternion::~Quaternion() {}
    3332
    34 Quaternion& Quaternion::operator=(const Quaternion &quaternion) {
    35     q0=quaternion.q0;
    36     q1=quaternion.q1;
    37     q2=quaternion.q2;
    38     q3=quaternion.q3;
    39     return (*this);
     33Quaternion &Quaternion::operator=(const Quaternion &quaternion) {
     34  q0 = quaternion.q0;
     35  q1 = quaternion.q1;
     36  q2 = quaternion.q2;
     37  q3 = quaternion.q3;
     38  return (*this);
    4039}
    4140
    4241float Quaternion::GetNorm(void) const {
    43     return sqrt(q0*q0+q1*q1+q2*q2+q3*q3);
     42  return sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
    4443}
    4544
    4645void Quaternion::Normalize(void) {
    47     float n=GetNorm();
    48     if(n!=0) {
    49         q0=q0/n;
    50         q1=q1/n;
    51         q2=q2/n;
    52         q3=q3/n;
    53     }
     46  float n = GetNorm();
     47  if (n != 0) {
     48    q0 = q0 / n;
     49    q1 = q1 / n;
     50    q2 = q2 / n;
     51    q3 = q3 / n;
     52  }
    5453}
    5554
    5655void Quaternion::Conjugate(void) {
    57     q1=-q1;
    58     q2=-q2;
    59     q3=-q3;
     56  q1 = -q1;
     57  q2 = -q2;
     58  q3 = -q3;
    6059}
    6160
    6261Quaternion Quaternion::GetConjugate(void) {
    63     return Quaternion(q0,-q1,-q2,-q3);
     62  return Quaternion(q0, -q1, -q2, -q3);
    6463}
    6564
    6665void Quaternion::GetLogarithm(Vector3D &logarithm) {
    67     Normalize();
    68     float v_norm=sqrtf(q1*q1+q2*q2+q3*q3);
     66  Normalize();
     67  float v_norm = sqrtf(q1 * q1 + q2 * q2 + q3 * q3);
    6968
    70     if(v_norm!=0) {
    71         float v_arccos = acosf(q0);
    72         logarithm.x=(q1*v_arccos)/v_norm;
    73         logarithm.y=(q2*v_arccos)/v_norm;
    74         logarithm.z=(q3*v_arccos)/v_norm;
    75     } else {
    76         logarithm.x=0;
    77         logarithm.y=0;
    78         logarithm.z=0;
    79     }
     69  if (v_norm != 0) {
     70    float v_arccos = acosf(q0);
     71    logarithm.x = (q1 * v_arccos) / v_norm;
     72    logarithm.y = (q2 * v_arccos) / v_norm;
     73    logarithm.z = (q3 * v_arccos) / v_norm;
     74  } else {
     75    logarithm.x = 0;
     76    logarithm.y = 0;
     77    logarithm.z = 0;
     78  }
    8079}
    8180
    8281Vector3D Quaternion::GetLogarithm(void) {
    83     Vector3D vector;
    84     GetLogarithm(vector);
    85     return vector;
     82  Vector3D vector;
     83  GetLogarithm(vector);
     84  return vector;
    8685}
    8786
    8887Quaternion Quaternion::GetDerivative(const Vector3D &angularSpeed) const {
    89     const Quaternion Qw(0,angularSpeed.x,angularSpeed.y,angularSpeed.z);
    90     return 0.5*(*this)*Qw;
     88  const Quaternion Qw(0, angularSpeed.x, angularSpeed.y, angularSpeed.z);
     89  return 0.5 * (*this) * Qw;
    9190}
    9291
    9392void Quaternion::Derivate(const Vector3D &angularSpeed) {
    94     Quaternion Q=GetDerivative(angularSpeed);
    95     (*this)=Q;
     93  Quaternion Q = GetDerivative(angularSpeed);
     94  (*this) = Q;
    9695}
    9796
    9897void Quaternion::ToEuler(Euler &euler) const {
    99     euler.roll=atan2(2*(q0*q1+q2*q3), 1-2*(q1*q1+q2*q2));
    100     euler.pitch=asin(2*(q0*q2-q1*q3));
    101     euler.yaw = atan2(2*(q0*q3+q1*q2), 1-2*(q2*q2+q3*q3));
     98  euler.roll = atan2(2 * (q0 * q1 + q2 * q3), 1 - 2 * (q1 * q1 + q2 * q2));
     99  euler.pitch = asin(2 * (q0 * q2 - q1 * q3));
     100  euler.yaw = atan2(2 * (q0 * q3 + q1 * q2), 1 - 2 * (q2 * q2 + q3 * q3));
    102101}
    103102
    104103Euler Quaternion::ToEuler(void) const {
    105     Euler euler;
    106     ToEuler(euler);
    107     return euler;
     104  Euler euler;
     105  ToEuler(euler);
     106  return euler;
    108107}
    109108
    110109void Quaternion::ToRotationMatrix(RotationMatrix &matrix) const {
    111     float aSq = q0 * q0;
    112     float bSq = q1 * q1;
    113     float cSq = q2 * q2;
    114     float dSq = q3 * q3;
    115     matrix.m[0][0] = aSq + bSq - cSq - dSq;
    116     matrix.m[0][1] = 2.0f * (q1 * q2 - q0 * q3);
    117     matrix.m[0][2] = 2.0f * (q0 * q2 + q1 * q3);
    118     matrix.m[1][0] = 2.0f * (q1 * q2 + q0 * q3);
    119     matrix.m[1][1] = aSq - bSq + cSq - dSq;
    120     matrix.m[1][2] = 2.0f * (q2 * q3 - q0 * q1);
    121     matrix.m[2][0] = 2.0f * (q1 * q3 - q0 * q2);
    122     matrix.m[2][1] = 2.0f * (q0 * q1 + q2 * q3);
    123     matrix.m[2][2] = aSq - bSq - cSq + dSq;
     110  float aSq = q0 * q0;
     111  float bSq = q1 * q1;
     112  float cSq = q2 * q2;
     113  float dSq = q3 * q3;
     114  matrix.m[0][0] = aSq + bSq - cSq - dSq;
     115  matrix.m[0][1] = 2.0f * (q1 * q2 - q0 * q3);
     116  matrix.m[0][2] = 2.0f * (q0 * q2 + q1 * q3);
     117  matrix.m[1][0] = 2.0f * (q1 * q2 + q0 * q3);
     118  matrix.m[1][1] = aSq - bSq + cSq - dSq;
     119  matrix.m[1][2] = 2.0f * (q2 * q3 - q0 * q1);
     120  matrix.m[2][0] = 2.0f * (q1 * q3 - q0 * q2);
     121  matrix.m[2][1] = 2.0f * (q0 * q1 + q2 * q3);
     122  matrix.m[2][2] = aSq - bSq - cSq + dSq;
    124123}
    125124
    126125Quaternion &Quaternion::operator+=(const Quaternion &quaternion) {
    127     q0+=quaternion.q0;
    128     q1+=quaternion.q1;
    129     q2+=quaternion.q2;
    130     q3+=quaternion.q3;
    131     return (*this);
     126  q0 += quaternion.q0;
     127  q1 += quaternion.q1;
     128  q2 += quaternion.q2;
     129  q3 += quaternion.q3;
     130  return (*this);
    132131}
    133132
    134133Quaternion &Quaternion::operator-=(const Quaternion &quaternion) {
    135     q0-=quaternion.q0;
    136     q1-=quaternion.q1;
    137     q2-=quaternion.q2;
    138     q3-=quaternion.q3;
    139     return (*this);
     134  q0 -= quaternion.q0;
     135  q1 -= quaternion.q1;
     136  q2 -= quaternion.q2;
     137  q3 -= quaternion.q3;
     138  return (*this);
    140139}
    141140
    142 Quaternion operator + (const Quaternion &quaternionA,const Quaternion &quaterniontB) {
    143     return Quaternion(
    144                quaternionA.q0 + quaterniontB.q0,
    145                quaternionA.q1 + quaterniontB.q1,
    146                quaternionA.q2 + quaterniontB.q2,
    147                quaternionA.q3 + quaterniontB.q3);
     141Quaternion operator+(const Quaternion &quaternionA,
     142                     const Quaternion &quaterniontB) {
     143  return Quaternion(
     144      quaternionA.q0 + quaterniontB.q0, quaternionA.q1 + quaterniontB.q1,
     145      quaternionA.q2 + quaterniontB.q2, quaternionA.q3 + quaterniontB.q3);
    148146}
    149147
    150 Quaternion operator- (const Quaternion &quaterniontA,const Quaternion &quaterniontB) {
    151    return Quaternion(
    152                quaterniontA.q0 - quaterniontB.q0,
    153                quaterniontA.q1 - quaterniontB.q1,
    154                quaterniontA.q2 - quaterniontB.q2,
    155                quaterniontA.q3 - quaterniontB.q3);
     148Quaternion operator-(const Quaternion &quaterniontA,
     149                     const Quaternion &quaterniontB) {
     150  return Quaternion(
     151      quaterniontA.q0 - quaterniontB.q0, quaterniontA.q1 - quaterniontB.q1,
     152      quaterniontA.q2 - quaterniontB.q2, quaterniontA.q3 - quaterniontB.q3);
    156153}
    157154
    158155Quaternion operator-(const Quaternion &quaternion) {
    159     return Quaternion(-quaternion.q0,-quaternion.q1,-quaternion.q2,-quaternion.q3);
     156  return Quaternion(-quaternion.q0, -quaternion.q1, -quaternion.q2,
     157                    -quaternion.q3);
    160158}
    161159
    162 Quaternion operator * (const Quaternion &quaterniontA,const Quaternion &quaterniontB) {
    163     return Quaternion(
    164                quaterniontA.q0 * quaterniontB.q0 - quaterniontA.q1 * quaterniontB.q1 - quaterniontA.q2 * quaterniontB.q2 - quaterniontA.q3 * quaterniontB.q3,
    165                quaterniontA.q0 * quaterniontB.q1 + quaterniontA.q1 * quaterniontB.q0 + quaterniontA.q2 * quaterniontB.q3 - quaterniontA.q3 * quaterniontB.q2,
    166                quaterniontA.q0 * quaterniontB.q2 - quaterniontA.q1 * quaterniontB.q3 + quaterniontA.q2 * quaterniontB.q0 + quaterniontA.q3 * quaterniontB.q1,
    167                quaterniontA.q0 * quaterniontB.q3 + quaterniontA.q1 * quaterniontB.q2 - quaterniontA.q2 * quaterniontB.q1 + quaterniontA.q3 * quaterniontB.q0);
     160Quaternion operator*(const Quaternion &quaterniontA,
     161                     const Quaternion &quaterniontB) {
     162  return Quaternion(
     163      quaterniontA.q0 * quaterniontB.q0 - quaterniontA.q1 * quaterniontB.q1 -
     164          quaterniontA.q2 * quaterniontB.q2 - quaterniontA.q3 * quaterniontB.q3,
     165      quaterniontA.q0 * quaterniontB.q1 + quaterniontA.q1 * quaterniontB.q0 +
     166          quaterniontA.q2 * quaterniontB.q3 - quaterniontA.q3 * quaterniontB.q2,
     167      quaterniontA.q0 * quaterniontB.q2 - quaterniontA.q1 * quaterniontB.q3 +
     168          quaterniontA.q2 * quaterniontB.q0 + quaterniontA.q3 * quaterniontB.q1,
     169      quaterniontA.q0 * quaterniontB.q3 + quaterniontA.q1 * quaterniontB.q2 -
     170          quaterniontA.q2 * quaterniontB.q1 +
     171          quaterniontA.q3 * quaterniontB.q0);
    168172}
    169173
    170 Quaternion operator * (float coeff,const Quaternion &quaternion) {
    171     return Quaternion(
    172                coeff*quaternion.q0,
    173                coeff*quaternion.q1,
    174                coeff*quaternion.q2,
    175                coeff*quaternion.q3);
     174Quaternion operator*(float coeff, const Quaternion &quaternion) {
     175  return Quaternion(coeff * quaternion.q0, coeff * quaternion.q1,
     176                    coeff * quaternion.q2, coeff * quaternion.q3);
    176177}
    177178
    178 
    179 Quaternion operator * (const Quaternion &quaternion,float coeff) {
    180     return Quaternion(
    181                coeff*quaternion.q0,
    182                coeff*quaternion.q1,
    183                coeff*quaternion.q2,
    184                coeff*quaternion.q3);
     179Quaternion operator*(const Quaternion &quaternion, float coeff) {
     180  return Quaternion(coeff * quaternion.q0, coeff * quaternion.q1,
     181                    coeff * quaternion.q2, coeff * quaternion.q3);
    185182}
    186183
Note: See TracChangeset for help on using the changeset viewer.