ugluQuaternion.pas

   1 unit ugluQuaternion;
   2
   3 { Package:      OpenGLCore
   4   Prefix:       glu - OpenGL Utils
   5   Beschreibung: diese Unit enthält Quaternion-Typen und Methoden um diese zu erstellen und zu manipulieren }
   6
   7 {$mode objfpc}{$H+}
   8
   9 interface
  10
  11 uses
  12   Classes, SysUtils, ugluVector, ugluMatrix;
  13
  14 type
  15   TgluQuaternion = type TgluVector4f; // w,x,y,z
  16
  17 {
  18    Winkel                           : rad, außer glRotate-Komponenten
  19    Absolute Werte                   : Orientation
  20    Relative Werte/"Drehanweisungen" : Rotation
  21    To/FromVector                    : Position im R^4
  22
  23    Alle Funktionen nehmen an, dass die Quaternion nur zur Rotation verwendet wird (kein Scale),
  24    mathematisch also normalisiert ist. Es findet keine Überprüfung statt.
  25 }
  26
  27   //Quaternion Konstruktoren
  28   function gluQuaternion(const W, X, Y, Z: Single): TgluQuaternion;
  29   function gluQuaternionNormalize(const q: TgluQuaternion): TgluQuaternion;
  30   procedure gluQuaternionNormalizeInplace(var q: TgluQuaternion);
  31   function gluQuaternionToVector(const q: TgluQuaternion): TgluVector3f;
  32   function gluVectorToQuaternion(const v: TgluVector3f): TgluQuaternion;
  33
  34   //Arithmetic
  35   function gluQuaternionConjugate(const q: TgluQuaternion): TgluQuaternion;
  36   function gluQuaternionMultiply(const l,r: TgluQuaternion): TgluQuaternion;
  37   function gluQuaternionAdd(const a,b: TgluQuaternion): TgluQuaternion;
  38   function gluQuaternionSubtract(const l,r: TgluQuaternion): TgluQuaternion;
  39   function gluQuaternionScale(const q: TgluQuaternion; const f: Single): TgluQuaternion;
  40
  41
  42   //To/From RotationMatrix
  43   function gluQuaternionToMatrix(const q: TgluQuaternion): TgluMatrix4f;
  44   function gluMatrixToQuaternion(const m: TgluMatrix4f): TgluQuaternion;
  45
  46   //To/From Axis/Angle {WINKEL IN °DEG}
  47   function gluQuaternionToRotation(const q: TgluQuaternion; out angle: Single): TgluVector3f;
  48   function gluRotationToQuaternion(const angle: Single; const axis: TgluVector3f): TgluQuaternion;
  49
  50   //Transforms
  51   function gluQuaternionTransformVec(const q: TgluQuaternion; const v: TgluVector3f): TgluVector3f;
  52
  53   function gluQuaternionLookAt(const Location, Target, UpVector: TgluVector3f): TgluQuaternion;
  54   //Rotation zw. Richtungen: Wie muss a modifiziert werden um b zu bekommen?
  55   function gluVectorRotationTo(const a, b: TgluVector3f): TgluQuaternion;
  56
  57   //Modifying Quaternions
  58   function gluQuaternionHalfAngle(const q: TgluQuaternion): TgluQuaternion;
  59   function gluQuaternionAngleBetween(const a, b: TgluQuaternion): double;
  60   function gluQuaternionSlerpOrientation(const a, b: TgluQuaternion; const t: single): TgluQuaternion;
  61   function gluQuaternionNlerpOrientation(const a, b: TgluQuaternion; const t: single): TgluQuaternion;
  62
  63   operator +(const a, b: TgluQuaternion): TgluQuaternion;
  64   operator -(const l, r: TgluQuaternion): TgluQuaternion;
  65   operator *(const l, r: TgluQuaternion): TgluQuaternion;
  66   operator *(const q: TgluQuaternion; const s: Sigle): TgluQuaternion;
  67
  68 const
  69   quW = 0;
  70   quX = 1;
  71   quY = 2;
  72   quZ = 3;
  73   gluQuaternionIdentity: TgluQuaternion = (1,0,0,0);
  74
  75 implementation
  76
  77 uses
  78   Math;
  79
  80 operator +(const a, b: TgluQuaternion): TgluQuaternion;
  81 begin
  82   result := gluQuaternionAdd(a, b);
  83 end;
  84
  85 operator -(const l, r: TgluQuaternion): TgluQuaternion;
  86 begin
  87   result := gluQuaternionSubtract(l, r);
  88 end;
  89
  90 operator *(const l, r: TgluQuaternion): TgluQuaternion;
  91 begin
  92   result := gluQuaternionMultiply(l, r);
  93 end;
  94
  95 operator *(const q: TgluQuaternion; const s: Sigle): TgluQuaternion;
  96 begin
  97   result := gluQuaternionScale(q, s);
  98 end;
  99
 100 function gluQuaternion(const W, X, Y, Z: Single): TgluQuaternion;
 101 begin
 102   Result:= gluVector4f(W,X,Y,Z);
 103 end;
 104
 105 function gluQuaternionNormalize(const q: TgluQuaternion): TgluQuaternion;
 106 begin
 107   Result:= q;
 108   gluQuaternionNormalizeInplace(Result);
 109 end;
 110
 111 procedure gluQuaternionNormalizeInplace(var q: TgluQuaternion);
 112 var
 113   s: Double;
 114 begin
 115   s:= sqr(q[quX])+sqr(q[quY])+sqr(q[quZ])+sqr(q[quW]);
 116   // already normalized?
 117   if IsZero(s - 1) then
 118     exit;
 119   s:= 1/sqrt(s);
 120   q[quX]:= q[quX] * s;
 121   q[quY]:= q[quY] * s;
 122   q[quZ]:= q[quZ] * s;
 123   q[quW]:= q[quW] * s;
 124 end;
 125
 126 function gluQuaternionToVector(const q: TgluQuaternion): TgluVector3f;
 127 begin
 128   Result:= gluVector3f(q[quX], q[quY], q[quZ]);
 129 end;
 130
 131 function gluVectorToQuaternion(const v: TgluVector3f): TgluQuaternion;
 132 begin
 133   Result:= gluQuaternion(0, v[0], v[1], v[2]);
 134 end;
 135
 136
 137 function gluQuaternionConjugate(const q: TgluQuaternion): TgluQuaternion;
 138 begin
 139   Result[quW] := q[quW];
 140   Result[quX] := -q[quX];
 141   Result[quY] := -q[quY];
 142   Result[quZ] := -q[quZ];
 143 end;
 144
 145 function gluQuaternionMultiply(const l, r: TgluQuaternion): TgluQuaternion;
 146 begin
 147   Result[quW] := -l[qux] * r[qux] - l[quy] * r[quy] - l[quz] * r[quz] + l[quw] * r[quw];
 148   Result[quX] :=  l[qux] * r[quw] + l[quy] * r[quz] - l[quz] * r[quy] + l[quw] * r[qux];
 149   Result[quY] := -l[qux] * r[quz] + l[quy] * r[quw] + l[quz] * r[qux] + l[quw] * r[quy];
 150   Result[quZ] :=  l[qux] * r[quy] - l[quy] * r[qux] + l[quz] * r[quw] + l[quw] * r[quz];
 151 end;
 152
 153 function gluQuaternionAdd(const a, b: TgluQuaternion): TgluQuaternion;
 154 begin
 155   Result[quW] := a[quW] + b[quW];
 156   Result[quX] := a[quX] + b[quX];
 157   Result[quY] := a[quY] + b[quY];
 158   Result[quZ] := a[quZ] + b[quZ];
 159 end;
 160
 161 function gluQuaternionSubtract(const l, r: TgluQuaternion): TgluQuaternion;
 162 begin
 163   Result[quW] := l[quW] - r[quW];
 164   Result[quX] := l[quX] - r[quX];
 165   Result[quY] := l[quY] - r[quY];
 166   Result[quZ] := l[quZ] - r[quZ];
 167 end;
 168
 169 function gluQuaternionScale(const q: TgluQuaternion; const f: Single): TgluQuaternion;
 170 begin
 171   Result[quW] := q[quW] * f;
 172   Result[quX] := q[quX] * f;
 173   Result[quY] := q[quY] * f;
 174   Result[quZ] := q[quZ] * f;
 175 end;
 176
 177 // http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToMatrix/index.htm
 178 function gluQuaternionToMatrix(const q: TgluQuaternion): TgluMatrix4f;
 179 var
 180   qx,qy,qz,qw: Single;
 181 begin
 182   qw:= q[quW];
 183   qx:= q[quX];
 184   qy:= q[quY];
 185   qz:= q[quZ];
 186   Result:= gluMatrixIdentity;
 187   Result[maAxisX] := gluVector4f(
 188     1 - 2*SQR(qy) - 2*SQR(qz),
 189     2*qx*qy + 2*qz*qw,
 190     2*qx*qz - 2*qy*qw,
 191     0);
 192   Result[maAxisY] := gluVector4f(
 193     2*qx*qy - 2*qz*qw,
 194     1 - 2*SQR(qx) - 2*SQR(qz),
 195     2*qy*qz + 2*qx*qw,
 196     0);
 197   Result[maAxisZ] := gluVector4f(
 198     2*qx*qz + 2*qy*qw,
 199     2*qy*qz - 2*qx*qw,
 200         1 - 2*SQR(qx) - 2*SQR(qy),
 201     0);
 202 end;
 203
 204 // http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/index.htm
 205 function gluMatrixToQuaternion(const m: TgluMatrix4f): TgluQuaternion;
 206 var
 207   trace, s: double;
 208   q: TgluQuaternion;
 209
 210 begin
 211   trace := m[0][0] + m[1][1] + m[2][2]; // I removed + 1.0f; see discussion with Ethan
 212   if( trace > 0 ) then begin// I changed M_EPSILON to 0
 213     s := 0.5 / SQRT(trace+ 1.0);
 214     q[quW] := 0.25 / s;
 215     q[quX] := ( m[2][1] - m[1][2] ) * s;
 216     q[quY] := ( m[0][2] - m[2][0] ) * s;
 217     q[quZ] := ( m[1][0] - m[0][1] ) * s;
 218   end else begin
 219     if ( m[0][0] > m[1][1]) and (m[0][0] > m[2][2] ) then begin
 220       s := 2.0 * SQRT( 1.0 + m[0][0] - m[1][1] - m[2][2]);
 221       q[quW] := (m[2][1] - m[1][2] ) / s;
 222       q[quX] := 0.25 * s;
 223       q[quY] := (m[0][1] + m[1][0] ) / s;
 224       q[quZ] := (m[0][2] + m[2][0] ) / s;
 225     end else if (m[1][1] > m[2][2]) then begin
 226       s := 2.0 * SQRT( 1.0 + m[1][1] - m[0][0] - m[2][2]);
 227       q[quW] := (m[0][2] - m[2][0] ) / s;
 228       q[quX] := (m[0][1] + m[1][0] ) / s;
 229       q[quY] := 0.25 * s;
 230       q[quZ] := (m[1][2] + m[2][1] ) / s;
 231     end else begin
 232       s := 2.0 * SQRT( 1.0 + m[2][2] - m[0][0] - m[1][1] );
 233       q[quW] := (m[1][0] - m[0][1] ) / s;
 234       q[quX] := (m[0][2] + m[2][0] ) / s;
 235       q[quY] := (m[1][2] + m[2][1] ) / s;
 236       q[quZ] := 0.25 * s;
 237     end;
 238   end;
 239   Result:= q;
 240 end;
 241
 242 // http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToAngle/index.htm
 243 function gluQuaternionToRotation(const q: TgluQuaternion; out angle: Single): TgluVector3f;
 244 var
 245   s: double;
 246 begin
 247   angle := radtodeg(2 * arccos(q[quW]));
 248   s := sqrt(1-q[quW]*q[quW]); // assuming quaternion normalised then w is less than 1, so term always positive.
 249   if (s < 0.001) then begin // test to avoid divide by zero, s is always positive due to sqrt
 250     // if s close to zero then direction of axis not important
 251     Result[0] := q[quX]; // if it is important that axis is normalised then replace with x=1; y=z=0;
 252     Result[1] := q[quY];
 253     Result[2] := q[quZ];
 254   end else begin
 255     Result[0] := q[quX] / s; // normalise axis
 256     Result[1] := q[quY] / s;
 257     Result[2] := q[quZ] / s;
 258   end;
 259 end;
 260
 261 function gluRotationToQuaternion(const angle: Single; const axis: TgluVector3f): TgluQuaternion;
 262 var
 263   a: single;
 264 begin
 265   a:= degtorad(angle) / 2;
 266   Result:= gluQuaternion(
 267     cos(a),
 268     sin(a) * axis[0],
 269     sin(a) * axis[1],
 270     sin(a) * axis[2]);
 271 end;
 272
 273 // http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/transforms/index.htm
 274 function gluQuaternionTransformVec(const q: TgluQuaternion; const v: TgluVector3f): TgluVector3f;
 275 var
 276   p: TgluQuaternion;
 277 begin
 278   //Pout = q * Pin * q'
 279   p:= gluQuaternionMultiply(q, gluVectorToQuaternion(v));
 280   p:= gluQuaternionMultiply(p, gluQuaternionConjugate(q));
 281   Result:= gluQuaternionToVector(p);
 282 end;
 283
 284 // http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/transforms/halfAngle.htm
 285 function gluQuaternionHalfAngle(const q: TgluQuaternion): TgluQuaternion;
 286 begin
 287   Result:= q;
 288   Result[quW]:= Result[quW] + 1;
 289   gluQuaternionNormalizeInplace(Result);
 290 end;
 291
 292 function gluQuaternionAngleBetween(const a, b: TgluQuaternion): double;
 293 var
 294   cosHalfTheta: double;
 295 begin
 296   cosHalfTheta:= a[quW] * b[quW] + a[quX] * b[quX] + a[quY] * b[quY] + a[quZ] * b[quZ];
 297   Result:= arccos(cosHalfTheta) * 2;
 298 end;
 299
 300 // http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/slerp/index.htm
 301 function gluQuaternionSlerpOrientation(const a, b: TgluQuaternion; const t: single): TgluQuaternion;
 302 var
 303   qa,qb: TgluQuaternion;
 304   cosHalfTheta, sinHalfTheta,
 305     halfTheta,
 306     ratioA, ratioB: double;
 307 begin
 308   qa:= a;
 309   qb:= b;
 310         // Calculate angle between them.
 311   cosHalfTheta:= a[quW] * b[quW] + a[quX] * b[quX] + a[quY] * b[quY] + a[quZ] * b[quZ];
 312   if (cosHalfTheta < 0) then begin
 313     qb:= gluQuaternion(
 314       -b[quW],
 315       -b[quX],
 316       -b[quY],
 317        b[quZ]
 318     );
 319     cosHalfTheta:= -cosHalfTheta;
 320   end;
 321
 322   // if qa=qb or qa=-qb then theta = 0 and we can return qa
 323         if abs(cosHalfTheta) >= 1.0 then begin
 324                 Result:= qa;
 325     Exit;
 326         end;
 327
 328         // Calculate temporary values.
 329         halfTheta := arccos(cosHalfTheta);
 330         sinHalfTheta := sqrt(1.0 - sqr(cosHalfTheta));
 331         // if theta = 180 degrees then result is not fully defined
 332         // we could rotate around any axis normal to qa or qb
 333         if (abs(sinHalfTheta) < 0.001) then begin
 334     Result:= gluQuaternionAdd(gluQuaternionScale(qa, 0.5), gluQuaternionScale(qb, 0.5));
 335     exit
 336   end;
 337         ratioA := sin((1 - t) * halfTheta) / sinHalfTheta;
 338         ratioB := sin(t * halfTheta) / sinHalfTheta;
 339         //calculate Quaternion.
 340   Result:= gluQuaternionAdd(gluQuaternionScale(qa, ratioA), gluQuaternionScale(qb, ratioB));
 341 end;
 342
 343 function gluQuaternionNlerpOrientation(const a, b: TgluQuaternion; const t: single): TgluQuaternion;
 344 begin
 345   Result:= gluQuaternionAdd(a, gluQuaternionScale(gluQuaternionSubtract(b,a), t));
 346   gluQuaternionNormalizeInplace(Result);
 347 end;
 348
 349 function gluQuaternionLookAt(const Location, Target, UpVector: TgluVector3f): TgluQuaternion;
 350 var
 351   front, up, right: TgluVector3f;
 352   w4_recip: Single;
 353 begin
 354   front:= gluVectorSubtract(Location, Target); // eigentlich falschrum. don't ask.
 355   up:= UpVector;
 356   gluVectorOrthoNormalize(front, up);
 357   right:= gluVectorProduct(up, front);
 358
 359   Result[quW]:= SQRT(1 + right[0] + up[1] + front[2]) * 0.5;
 360   w4_recip:= 1 / (4 * Result[quW]);
 361
 362   Result[quX]:= (front[1] - up[2]) * w4_recip;
 363         Result[quY]:= (right[2] - front[0]) * w4_recip;
 364         Result[quZ]:= (up[0] - right[1]) * w4_recip;
 365 end;
 366
 367 function gluVectorRotationTo(const a, b: TgluVector3f): TgluQuaternion;
 368 var
 369   d, qw: single;
 370   ax: TgluVector3f;
 371 begin
 372   d:=gluVectorScalar(a, b);
 373   ax:= gluVectorProduct(a, b);
 374   qw:= gluVectorLength(a) * gluVectorLength(b) + d;
 375         if (qw < 0.0001) then begin // vectors are 180 degrees apart
 376     Result:= gluQuaternion(0, -a[2],a[1],a[0]);
 377   end else begin
 378     Result:= gluQuaternion(qw, ax[0],ax[1],ax[2]);
 379   end;
 380   gluQuaternionNormalizeInplace(Result);
 381 end;
 382
 383 end.
 384