Bo Carøe
Test1 of cmsis and IMU/AHRS (sensor BMA180,HMC5883,ITG3200) IMU/AHRS is not ok
GTMath/Quaternion.cpp@0:cb04b53e6f9b, 2012-06-11 (annotated)
- Committer:
- caroe
- Date:
- Mon Jun 11 12:02:30 2012 +0000
- Revision:
- 0:cb04b53e6f9b
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| caroe | 0:cb04b53e6f9b | 1 | #include "GTMath.h" |
| caroe | 0:cb04b53e6f9b | 2 | |
| caroe | 0:cb04b53e6f9b | 3 | /////////////////////////////////////////////////////////////////// |
| caroe | 0:cb04b53e6f9b | 4 | //Konstruktoren |
| caroe | 0:cb04b53e6f9b | 5 | Quaternion::Quaternion() |
| caroe | 0:cb04b53e6f9b | 6 | {} |
| caroe | 0:cb04b53e6f9b | 7 | Quaternion::Quaternion(const Quaternion& q) : |
| caroe | 0:cb04b53e6f9b | 8 | w(q.w), |
| caroe | 0:cb04b53e6f9b | 9 | v(q.v.x, q.v.y, q.v.z) |
| caroe | 0:cb04b53e6f9b | 10 | {} |
| caroe | 0:cb04b53e6f9b | 11 | Quaternion::Quaternion(float _w, const Vector3& _v) : |
| caroe | 0:cb04b53e6f9b | 12 | w(_w), |
| caroe | 0:cb04b53e6f9b | 13 | v(_v) |
| caroe | 0:cb04b53e6f9b | 14 | {} |
| caroe | 0:cb04b53e6f9b | 15 | Quaternion::Quaternion(float _w, float _x= 0.0F, float _y= 0.0F, float _z= 0.0F) : |
| caroe | 0:cb04b53e6f9b | 16 | w(_w), |
| caroe | 0:cb04b53e6f9b | 17 | v(_x, _y, _z) |
| caroe | 0:cb04b53e6f9b | 18 | {} |
| caroe | 0:cb04b53e6f9b | 19 | |
| caroe | 0:cb04b53e6f9b | 20 | |
| caroe | 0:cb04b53e6f9b | 21 | ////////////////////////////////////////////////////////////////// |
| caroe | 0:cb04b53e6f9b | 22 | //Casting |
| caroe | 0:cb04b53e6f9b | 23 | //Direkter Zugriff auf die Daten |
| caroe | 0:cb04b53e6f9b | 24 | Quaternion::operator float* () |
| caroe | 0:cb04b53e6f9b | 25 | { |
| caroe | 0:cb04b53e6f9b | 26 | return(&w); |
| caroe | 0:cb04b53e6f9b | 27 | } |
| caroe | 0:cb04b53e6f9b | 28 | |
| caroe | 0:cb04b53e6f9b | 29 | |
| caroe | 0:cb04b53e6f9b | 30 | /////////////////////////////////////////////////////////////////// |
| caroe | 0:cb04b53e6f9b | 31 | //Zuweisungen |
| caroe | 0:cb04b53e6f9b | 32 | Quaternion& Quaternion::operator =(const Quaternion& q) |
| caroe | 0:cb04b53e6f9b | 33 | { |
| caroe | 0:cb04b53e6f9b | 34 | w= q.w; |
| caroe | 0:cb04b53e6f9b | 35 | v= q.v; |
| caroe | 0:cb04b53e6f9b | 36 | return(*this); |
| caroe | 0:cb04b53e6f9b | 37 | } |
| caroe | 0:cb04b53e6f9b | 38 | Quaternion& Quaternion::operator+=(const Quaternion& q) |
| caroe | 0:cb04b53e6f9b | 39 | { |
| caroe | 0:cb04b53e6f9b | 40 | w+= q.w; |
| caroe | 0:cb04b53e6f9b | 41 | v+= q.v; |
| caroe | 0:cb04b53e6f9b | 42 | return(*this); |
| caroe | 0:cb04b53e6f9b | 43 | } |
| caroe | 0:cb04b53e6f9b | 44 | Quaternion& Quaternion::operator-=(const Quaternion& q) |
| caroe | 0:cb04b53e6f9b | 45 | { |
| caroe | 0:cb04b53e6f9b | 46 | w-= q.w; |
| caroe | 0:cb04b53e6f9b | 47 | v-= q.v; |
| caroe | 0:cb04b53e6f9b | 48 | return(*this); |
| caroe | 0:cb04b53e6f9b | 49 | } |
| caroe | 0:cb04b53e6f9b | 50 | Quaternion& Quaternion::operator*=(const Quaternion& q) |
| caroe | 0:cb04b53e6f9b | 51 | { |
| caroe | 0:cb04b53e6f9b | 52 | const float a= (v.z - v.y) * (q.v.y - q.v.z), |
| caroe | 0:cb04b53e6f9b | 53 | b= ( w + v.x) * (q. w + q.v.x), |
| caroe | 0:cb04b53e6f9b | 54 | c= ( w - v.x) * (q.v.y + q.v.z), |
| caroe | 0:cb04b53e6f9b | 55 | d= (v.y + v.z) * (q. w - q.v.x), |
| caroe | 0:cb04b53e6f9b | 56 | e= (v.z - v.x) * (q.v.x - q.v.y), |
| caroe | 0:cb04b53e6f9b | 57 | f= (v.z + v.x) * (q.v.x + q.v.y), |
| caroe | 0:cb04b53e6f9b | 58 | g= ( w + v.y) * (q. w - q.v.z), |
| caroe | 0:cb04b53e6f9b | 59 | h= ( w - v.y) * (q. w + q.v.z), |
| caroe | 0:cb04b53e6f9b | 60 | i= f + g + h, |
| caroe | 0:cb04b53e6f9b | 61 | j= 0.5F * (e + i); |
| caroe | 0:cb04b53e6f9b | 62 | |
| caroe | 0:cb04b53e6f9b | 63 | return(*this= Quaternion(a + j - f, |
| caroe | 0:cb04b53e6f9b | 64 | b + j - i, |
| caroe | 0:cb04b53e6f9b | 65 | c + j - h, |
| caroe | 0:cb04b53e6f9b | 66 | d + j - g)); |
| caroe | 0:cb04b53e6f9b | 67 | } |
| caroe | 0:cb04b53e6f9b | 68 | Quaternion& Quaternion::operator/=(const Quaternion& q) |
| caroe | 0:cb04b53e6f9b | 69 | { |
| caroe | 0:cb04b53e6f9b | 70 | return(*this*= q.Invert()); |
| caroe | 0:cb04b53e6f9b | 71 | } |
| caroe | 0:cb04b53e6f9b | 72 | Quaternion& Quaternion::operator*=(float f) |
| caroe | 0:cb04b53e6f9b | 73 | { |
| caroe | 0:cb04b53e6f9b | 74 | w*= f; |
| caroe | 0:cb04b53e6f9b | 75 | v*= f; |
| caroe | 0:cb04b53e6f9b | 76 | return(*this); |
| caroe | 0:cb04b53e6f9b | 77 | } |
| caroe | 0:cb04b53e6f9b | 78 | Quaternion& Quaternion::operator/=(float f) |
| caroe | 0:cb04b53e6f9b | 79 | { |
| caroe | 0:cb04b53e6f9b | 80 | f= 1.0F/f; |
| caroe | 0:cb04b53e6f9b | 81 | w*= f; |
| caroe | 0:cb04b53e6f9b | 82 | v*= f; |
| caroe | 0:cb04b53e6f9b | 83 | return(*this); |
| caroe | 0:cb04b53e6f9b | 84 | } |
| caroe | 0:cb04b53e6f9b | 85 | |
| caroe | 0:cb04b53e6f9b | 86 | //////////////////////////////////////////////////////////// |
| caroe | 0:cb04b53e6f9b | 87 | //Arithmetik |
| caroe | 0:cb04b53e6f9b | 88 | const Quaternion Quaternion::operator+(const Quaternion& q) const |
| caroe | 0:cb04b53e6f9b | 89 | { |
| caroe | 0:cb04b53e6f9b | 90 | return(Quaternion(w+q.w, v.x+q.v.x, v.y+q.v.y, v.z+q.v.z)); |
| caroe | 0:cb04b53e6f9b | 91 | } |
| caroe | 0:cb04b53e6f9b | 92 | const Quaternion Quaternion::operator-(const Quaternion& q) const |
| caroe | 0:cb04b53e6f9b | 93 | { |
| caroe | 0:cb04b53e6f9b | 94 | return(Quaternion(w-q.w, v.x-q.v.x, v.y-q.v.y, v.z-q.v.z)); |
| caroe | 0:cb04b53e6f9b | 95 | } |
| caroe | 0:cb04b53e6f9b | 96 | const Quaternion Quaternion::operator*(const Quaternion& q) const |
| caroe | 0:cb04b53e6f9b | 97 | { |
| caroe | 0:cb04b53e6f9b | 98 | const float a= (v.z - v.y) * (q.v.y - q.v.z), |
| caroe | 0:cb04b53e6f9b | 99 | b= ( w + v.x) * (q. w + q.v.x), |
| caroe | 0:cb04b53e6f9b | 100 | c= ( w - v.x) * (q.v.y + q.v.z), |
| caroe | 0:cb04b53e6f9b | 101 | d= (v.y + v.z) * (q. w - q.v.x), |
| caroe | 0:cb04b53e6f9b | 102 | e= (v.z - v.x) * (q.v.x - q.v.y), |
| caroe | 0:cb04b53e6f9b | 103 | f= (v.z + v.x) * (q.v.x + q.v.y), |
| caroe | 0:cb04b53e6f9b | 104 | g= ( w + v.y) * (q. w - q.v.z), |
| caroe | 0:cb04b53e6f9b | 105 | h= ( w - v.y) * (q. w + q.v.z), |
| caroe | 0:cb04b53e6f9b | 106 | i= f + g + h, |
| caroe | 0:cb04b53e6f9b | 107 | j= 0.5F * (e + i); |
| caroe | 0:cb04b53e6f9b | 108 | |
| caroe | 0:cb04b53e6f9b | 109 | return(Quaternion(a + j - f, |
| caroe | 0:cb04b53e6f9b | 110 | b + j - i, |
| caroe | 0:cb04b53e6f9b | 111 | c + j - h, |
| caroe | 0:cb04b53e6f9b | 112 | d + j - g)); |
| caroe | 0:cb04b53e6f9b | 113 | } |
| caroe | 0:cb04b53e6f9b | 114 | const Quaternion Quaternion::operator/(const Quaternion& q) const |
| caroe | 0:cb04b53e6f9b | 115 | { |
| caroe | 0:cb04b53e6f9b | 116 | return(*this * q.Invert()); |
| caroe | 0:cb04b53e6f9b | 117 | } |
| caroe | 0:cb04b53e6f9b | 118 | const Quaternion Quaternion::operator*(float f) const |
| caroe | 0:cb04b53e6f9b | 119 | { |
| caroe | 0:cb04b53e6f9b | 120 | return(Quaternion(w*f, v*f)); |
| caroe | 0:cb04b53e6f9b | 121 | } |
| caroe | 0:cb04b53e6f9b | 122 | const Quaternion Quaternion::operator/(float f) const |
| caroe | 0:cb04b53e6f9b | 123 | { |
| caroe | 0:cb04b53e6f9b | 124 | f= 1.0F/f; return(Quaternion(w*f, v*f)); |
| caroe | 0:cb04b53e6f9b | 125 | } |
| caroe | 0:cb04b53e6f9b | 126 | const Quaternion Quaternion::operator-() const |
| caroe | 0:cb04b53e6f9b | 127 | { |
| caroe | 0:cb04b53e6f9b | 128 | return(Quaternion(-w, -v.x, -v.y, -v.z)); |
| caroe | 0:cb04b53e6f9b | 129 | } |
| caroe | 0:cb04b53e6f9b | 130 | |
| caroe | 0:cb04b53e6f9b | 131 | //////////////////////////////////////////////////////////// |
| caroe | 0:cb04b53e6f9b | 132 | //Identit�quaternion |
| caroe | 0:cb04b53e6f9b | 133 | const Quaternion Quaternion::MulIdentity() |
| caroe | 0:cb04b53e6f9b | 134 | { |
| caroe | 0:cb04b53e6f9b | 135 | return(Quaternion(1.0F, 0.0F, 0.0F, 0.0F)); |
| caroe | 0:cb04b53e6f9b | 136 | } |
| caroe | 0:cb04b53e6f9b | 137 | const Quaternion Quaternion::AddIdentity() |
| caroe | 0:cb04b53e6f9b | 138 | { |
| caroe | 0:cb04b53e6f9b | 139 | return(Quaternion(0.0F, 0.0F, 0.0F, 0.0F)); |
| caroe | 0:cb04b53e6f9b | 140 | } |
| caroe | 0:cb04b53e6f9b | 141 | |
| caroe | 0:cb04b53e6f9b | 142 | //////////////////////////////////////////////////////////// |
| caroe | 0:cb04b53e6f9b | 143 | //Betrag (=L�e), Normalisieren |
| caroe | 0:cb04b53e6f9b | 144 | float Quaternion::Length() const |
| caroe | 0:cb04b53e6f9b | 145 | { |
| caroe | 0:cb04b53e6f9b | 146 | return(sqrtf(w*w + v.x*v.x + v.y*v.y + v.z*v.z)); |
| caroe | 0:cb04b53e6f9b | 147 | } |
| caroe | 0:cb04b53e6f9b | 148 | float Quaternion::LengthSq() const |
| caroe | 0:cb04b53e6f9b | 149 | { |
| caroe | 0:cb04b53e6f9b | 150 | return(w*w + v.x*v.x + v.y*v.y + v.z*v.z); |
| caroe | 0:cb04b53e6f9b | 151 | } |
| caroe | 0:cb04b53e6f9b | 152 | const Quaternion Quaternion::Normalize() const |
| caroe | 0:cb04b53e6f9b | 153 | { |
| caroe | 0:cb04b53e6f9b | 154 | return(*this / Length()); |
| caroe | 0:cb04b53e6f9b | 155 | } |
| caroe | 0:cb04b53e6f9b | 156 | |
| caroe | 0:cb04b53e6f9b | 157 | |
| caroe | 0:cb04b53e6f9b | 158 | //////////////////////////////////////////////////////////// |
| caroe | 0:cb04b53e6f9b | 159 | //Konjugieren , Invertieren |
| caroe | 0:cb04b53e6f9b | 160 | const Quaternion Quaternion::Conjugate() const |
| caroe | 0:cb04b53e6f9b | 161 | { |
| caroe | 0:cb04b53e6f9b | 162 | return(Quaternion(w, -v.x, -v.y, -v.z)); |
| caroe | 0:cb04b53e6f9b | 163 | } |
| caroe | 0:cb04b53e6f9b | 164 | const Quaternion Quaternion::Invert() const |
| caroe | 0:cb04b53e6f9b | 165 | { |
| caroe | 0:cb04b53e6f9b | 166 | const float fLengthSqInv= 1.0F / (w*w + v.x*v.x + v.y*v.y + v.z*v.z); |
| caroe | 0:cb04b53e6f9b | 167 | return(Quaternion( w * fLengthSqInv, |
| caroe | 0:cb04b53e6f9b | 168 | -v.x * fLengthSqInv, |
| caroe | 0:cb04b53e6f9b | 169 | -v.y * fLengthSqInv, |
| caroe | 0:cb04b53e6f9b | 170 | -v.z * fLengthSqInv)); |
| caroe | 0:cb04b53e6f9b | 171 | } |
| caroe | 0:cb04b53e6f9b | 172 | |
| caroe | 0:cb04b53e6f9b | 173 | //////////////////////////////////////////////////////////// |
| caroe | 0:cb04b53e6f9b | 174 | //Punktprodukt berechnen |
| caroe | 0:cb04b53e6f9b | 175 | float Quaternion::DotP(const Quaternion& q) const |
| caroe | 0:cb04b53e6f9b | 176 | { |
| caroe | 0:cb04b53e6f9b | 177 | return(w*q.w + v.x*q.v.x + v.y*q.v.y + v.z*q.v.z); |
| caroe | 0:cb04b53e6f9b | 178 | } |
| caroe | 0:cb04b53e6f9b | 179 | |
| caroe | 0:cb04b53e6f9b | 180 | //////////////////////////////////////////////////////////// |
| caroe | 0:cb04b53e6f9b | 181 | //Rotation von Vektoren |
| caroe | 0:cb04b53e6f9b | 182 | const Vector3 Quaternion::RotateVector(const Vector3 &v) const |
| caroe | 0:cb04b53e6f9b | 183 | { |
| caroe | 0:cb04b53e6f9b | 184 | //nach der Formel: x'= q * x * qk |
| caroe | 0:cb04b53e6f9b | 185 | return((*this * Quaternion(0, v) * Conjugate()).v); |
| caroe | 0:cb04b53e6f9b | 186 | } |
| caroe | 0:cb04b53e6f9b | 187 | |
| caroe | 0:cb04b53e6f9b | 188 | |
| caroe | 0:cb04b53e6f9b | 189 | //////////////////////////////////////////////////////////// |
| caroe | 0:cb04b53e6f9b | 190 | //Konstruieren |
| caroe | 0:cb04b53e6f9b | 191 | //aus: Normale und Winkel(Radiant!) |
| caroe | 0:cb04b53e6f9b | 192 | const Quaternion Quaternion::CreateFromAxisAndAngle(const Vector3& vAxis, float fAngle) |
| caroe | 0:cb04b53e6f9b | 193 | { |
| caroe | 0:cb04b53e6f9b | 194 | fAngle*= 0.5F; //Halbieren, da immer nur die H�te des Winkels ben�t wird |
| caroe | 0:cb04b53e6f9b | 195 | return(Quaternion(cosf(fAngle), |
| caroe | 0:cb04b53e6f9b | 196 | vAxis * sinf(fAngle))); |
| caroe | 0:cb04b53e6f9b | 197 | } |
| caroe | 0:cb04b53e6f9b | 198 | //aus: Euler-Rotation (=3 Winkel als Vektor3) |
| caroe | 0:cb04b53e6f9b | 199 | const Quaternion Quaternion::CreateFromEuler(Vector3 vEulerAngles) |
| caroe | 0:cb04b53e6f9b | 200 | { |
| caroe | 0:cb04b53e6f9b | 201 | vEulerAngles*= 0.5F; //Halbieren, da immer nur die H�te der Winkel ben�t wird |
| caroe | 0:cb04b53e6f9b | 202 | |
| caroe | 0:cb04b53e6f9b | 203 | return( Quaternion(cosf(vEulerAngles.x), sinf(vEulerAngles.x), 0.0F, 0.0F) * |
| caroe | 0:cb04b53e6f9b | 204 | Quaternion(cosf(vEulerAngles.y), 0.0F, sinf(vEulerAngles.y), 0.0F) * |
| caroe | 0:cb04b53e6f9b | 205 | Quaternion(cosf(vEulerAngles.z), 0.0F, 0.0F, sinf(vEulerAngles.z)) ); |
| caroe | 0:cb04b53e6f9b | 206 | } |
| caroe | 0:cb04b53e6f9b | 207 | //aus: Matrix3x3 |
| caroe | 0:cb04b53e6f9b | 208 | const Quaternion Quaternion::CreateFromMatrix(const Matrix3x3 & m) |
| caroe | 0:cb04b53e6f9b | 209 | { |
| caroe | 0:cb04b53e6f9b | 210 | float qw, qx, qy, qz; |
| caroe | 0:cb04b53e6f9b | 211 | float tr= m.m11 + m.m22 + m.m33; |
| caroe | 0:cb04b53e6f9b | 212 | |
| caroe | 0:cb04b53e6f9b | 213 | if(tr > 0) |
| caroe | 0:cb04b53e6f9b | 214 | { |
| caroe | 0:cb04b53e6f9b | 215 | float S = sqrt(tr+1.0) * 2; // S=4*qw |
| caroe | 0:cb04b53e6f9b | 216 | qw = 0.25 * S; |
| caroe | 0:cb04b53e6f9b | 217 | qx = (m.m32 - m.m23) / S; |
| caroe | 0:cb04b53e6f9b | 218 | qy = (m.m13 - m.m31) / S; |
| caroe | 0:cb04b53e6f9b | 219 | qz = (m.m21 - m.m12) / S; |
| caroe | 0:cb04b53e6f9b | 220 | } |
| caroe | 0:cb04b53e6f9b | 221 | else if((m.m11 > m.m22)&(m.m11 > m.m33)) |
| caroe | 0:cb04b53e6f9b | 222 | { |
| caroe | 0:cb04b53e6f9b | 223 | float S = sqrt(1.0 + m.m11 - m.m22 - m.m33) * 2; // S=4*qx |
| caroe | 0:cb04b53e6f9b | 224 | qw = (m.m32 - m.m23) / S; |
| caroe | 0:cb04b53e6f9b | 225 | qx = 0.25 * S; |
| caroe | 0:cb04b53e6f9b | 226 | qy = (m.m12 + m.m21) / S; |
| caroe | 0:cb04b53e6f9b | 227 | qz = (m.m13 + m.m31) / S; |
| caroe | 0:cb04b53e6f9b | 228 | } |
| caroe | 0:cb04b53e6f9b | 229 | else if(m.m22 > m.m33) |
| caroe | 0:cb04b53e6f9b | 230 | { |
| caroe | 0:cb04b53e6f9b | 231 | float S = sqrt(1.0 + m.m22 - m.m11 - m.m33) * 2; // S=4*qy |
| caroe | 0:cb04b53e6f9b | 232 | qw = (m.m13 - m.m31) / S; |
| caroe | 0:cb04b53e6f9b | 233 | qx = (m.m12 + m.m21) / S; |
| caroe | 0:cb04b53e6f9b | 234 | qy = 0.25 * S; |
| caroe | 0:cb04b53e6f9b | 235 | qz = (m.m23 + m.m32) / S; |
| caroe | 0:cb04b53e6f9b | 236 | } |
| caroe | 0:cb04b53e6f9b | 237 | else |
| caroe | 0:cb04b53e6f9b | 238 | { |
| caroe | 0:cb04b53e6f9b | 239 | float S = sqrt(1.0 + m.m33 - m.m11 - m.m22) * 2; // S=4*qz |
| caroe | 0:cb04b53e6f9b | 240 | qw = (m.m21 - m.m12) / S; |
| caroe | 0:cb04b53e6f9b | 241 | qx = (m.m13 + m.m31) / S; |
| caroe | 0:cb04b53e6f9b | 242 | qy = (m.m23 + m.m32) / S; |
| caroe | 0:cb04b53e6f9b | 243 | qz = 0.25 * S; |
| caroe | 0:cb04b53e6f9b | 244 | } |
| caroe | 0:cb04b53e6f9b | 245 | return(Quaternion(qw, qx, qy, qz)); |
| caroe | 0:cb04b53e6f9b | 246 | } |
| caroe | 0:cb04b53e6f9b | 247 | |
| caroe | 0:cb04b53e6f9b | 248 | //Quaternion in Euler-Winkel umwandeln |
| caroe | 0:cb04b53e6f9b | 249 | const Vector3 Quaternion::CalcEulerAngles() const |
| caroe | 0:cb04b53e6f9b | 250 | { |
| caroe | 0:cb04b53e6f9b | 251 | return(Vector3( asin(-2.0 * (w*v.x + v.y*v.z)), |
| caroe | 0:cb04b53e6f9b | 252 | atan2(-2.0 * (w*v.y - v.x*v.z), 1.0 - 2.0 * (v.y*v.y + v.x*v.x)), |
| caroe | 0:cb04b53e6f9b | 253 | atan2(-2.0 * (w*v.z - v.x*v.y), 1.0 - 2.0 * (v.x*v.x + v.z*v.z)) )); |
| caroe | 0:cb04b53e6f9b | 254 | } |
| caroe | 0:cb04b53e6f9b | 255 | |
| caroe | 0:cb04b53e6f9b | 256 | //////////////////////////////////////////////////////////// |
| caroe | 0:cb04b53e6f9b | 257 | //Lineare Interpolation |
| caroe | 0:cb04b53e6f9b | 258 | const Quaternion Quaternion::Lerp(Quaternion q, float f) const |
| caroe | 0:cb04b53e6f9b | 259 | { |
| caroe | 0:cb04b53e6f9b | 260 | return((*this*f + q*(1.0F-f)).Normalize()); |
| caroe | 0:cb04b53e6f9b | 261 | } |
| caroe | 0:cb04b53e6f9b | 262 | //Sphaerische lineare Interpolation |
| caroe | 0:cb04b53e6f9b | 263 | const Quaternion Quaternion::Slerp(Quaternion q, float f) const |
| caroe | 0:cb04b53e6f9b | 264 | { |
| caroe | 0:cb04b53e6f9b | 265 | //den kuerzeren weg gehen |
| caroe | 0:cb04b53e6f9b | 266 | if(DotP(q) < 0) |
| caroe | 0:cb04b53e6f9b | 267 | q= -q; |
| caroe | 0:cb04b53e6f9b | 268 | const float fAngle= v.Angle(q.v); |
| caroe | 0:cb04b53e6f9b | 269 | if(fAngle < 0.1F) |
| caroe | 0:cb04b53e6f9b | 270 | return((*this*f + q*(1.0F-f)).Normalize()); |
| caroe | 0:cb04b53e6f9b | 271 | |
| caroe | 0:cb04b53e6f9b | 272 | const float fInvSinAngle= 1.0F / sinf(fAngle); |
| caroe | 0:cb04b53e6f9b | 273 | return(*this * sinf( f * fAngle) * fInvSinAngle |
| caroe | 0:cb04b53e6f9b | 274 | + q * sinf((1.0F-f) * fAngle) * fInvSinAngle); |
| caroe | 0:cb04b53e6f9b | 275 | } |