4 Prefix: glu - OpenGL Utils
5 Beschreibung: diese Unit enthält Quaternion-Typen und Methoden um diese zu erstellen und zu manipulieren }
12 Classes, SysUtils, ugluVector, ugluMatrix;
15 TgluQuaternion = type TgluVector4f; // w,x,y,z
18 Winkel : rad, außer glRotate-Komponenten
19 Absolute Werte : Orientation
20 Relative Werte/"Drehanweisungen" : Rotation
21 To/FromVector : Position im R^4
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.
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;
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;
42 //To/From RotationMatrix
43 function gluQuaternionToMatrix(const q: TgluQuaternion): TgluMatrix4f;
44 function gluMatrixToQuaternion(const m: TgluMatrix4f): TgluQuaternion;
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;
51 function gluQuaternionTransformVec(const q: TgluQuaternion; const v: TgluVector3f): TgluVector3f;
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;
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;
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: Single): TgluQuaternion;
73 gluQuaternionIdentity: TgluQuaternion = (1,0,0,0);
80 operator +(const a, b: TgluQuaternion): TgluQuaternion;
82 result := gluQuaternionAdd(a, b);
85 operator -(const l, r: TgluQuaternion): TgluQuaternion;
87 result := gluQuaternionSubtract(l, r);
90 operator *(const l, r: TgluQuaternion): TgluQuaternion;
92 result := gluQuaternionMultiply(l, r);
95 operator *(const q: TgluQuaternion; const s: Single): TgluQuaternion;
97 result := gluQuaternionScale(q, s);
100 function gluQuaternion(const W, X, Y, Z: Single): TgluQuaternion;
102 Result:= gluVector4f(W,X,Y,Z);
105 function gluQuaternionNormalize(const q: TgluQuaternion): TgluQuaternion;
108 gluQuaternionNormalizeInplace(Result);
111 procedure gluQuaternionNormalizeInplace(var q: TgluQuaternion);
115 s:= sqr(q[quX])+sqr(q[quY])+sqr(q[quZ])+sqr(q[quW]);
116 // already normalized?
117 if IsZero(s - 1) then
126 function gluQuaternionToVector(const q: TgluQuaternion): TgluVector3f;
128 Result:= gluVector3f(q[quX], q[quY], q[quZ]);
131 function gluVectorToQuaternion(const v: TgluVector3f): TgluQuaternion;
133 Result:= gluQuaternion(0, v[0], v[1], v[2]);
137 function gluQuaternionConjugate(const q: TgluQuaternion): TgluQuaternion;
139 Result[quW] := q[quW];
140 Result[quX] := -q[quX];
141 Result[quY] := -q[quY];
142 Result[quZ] := -q[quZ];
145 function gluQuaternionMultiply(const l, r: TgluQuaternion): TgluQuaternion;
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];
153 function gluQuaternionAdd(const a, b: TgluQuaternion): TgluQuaternion;
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];
161 function gluQuaternionSubtract(const l, r: TgluQuaternion): TgluQuaternion;
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];
169 function gluQuaternionScale(const q: TgluQuaternion; const f: Single): TgluQuaternion;
171 Result[quW] := q[quW] * f;
172 Result[quX] := q[quX] * f;
173 Result[quY] := q[quY] * f;
174 Result[quZ] := q[quZ] * f;
177 // http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToMatrix/index.htm
178 function gluQuaternionToMatrix(const q: TgluQuaternion): TgluMatrix4f;
186 Result:= gluMatrixIdentity;
187 Result[maAxisX] := gluVector4f(
188 1 - 2*SQR(qy) - 2*SQR(qz),
192 Result[maAxisY] := gluVector4f(
194 1 - 2*SQR(qx) - 2*SQR(qz),
197 Result[maAxisZ] := gluVector4f(
200 1 - 2*SQR(qx) - 2*SQR(qy),
204 // http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/index.htm
205 function gluMatrixToQuaternion(const m: TgluMatrix4f): TgluQuaternion;
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);
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;
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;
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;
230 q[quZ] := (m[1][2] + m[2][1] ) / s;
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;
242 // http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToAngle/index.htm
243 function gluQuaternionToRotation(const q: TgluQuaternion; out angle: Single): TgluVector3f;
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;
255 Result[0] := q[quX] / s; // normalise axis
256 Result[1] := q[quY] / s;
257 Result[2] := q[quZ] / s;
261 function gluRotationToQuaternion(const angle: Single; const axis: TgluVector3f): TgluQuaternion;
265 a:= degtorad(angle) / 2;
266 Result:= gluQuaternion(
273 // http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/transforms/index.htm
274 function gluQuaternionTransformVec(const q: TgluQuaternion; const v: TgluVector3f): TgluVector3f;
278 //Pout = q * Pin * q'
279 p:= gluQuaternionMultiply(q, gluVectorToQuaternion(v));
280 p:= gluQuaternionMultiply(p, gluQuaternionConjugate(q));
281 Result:= gluQuaternionToVector(p);
284 // http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/transforms/halfAngle.htm
285 function gluQuaternionHalfAngle(const q: TgluQuaternion): TgluQuaternion;
288 Result[quW]:= Result[quW] + 1;
289 gluQuaternionNormalizeInplace(Result);
292 function gluQuaternionAngleBetween(const a, b: TgluQuaternion): double;
294 cosHalfTheta: double;
296 cosHalfTheta:= a[quW] * b[quW] + a[quX] * b[quX] + a[quY] * b[quY] + a[quZ] * b[quZ];
297 Result:= arccos(cosHalfTheta) * 2;
300 // http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/slerp/index.htm
301 function gluQuaternionSlerpOrientation(const a, b: TgluQuaternion; const t: single): TgluQuaternion;
303 qa,qb: TgluQuaternion;
304 cosHalfTheta, sinHalfTheta,
306 ratioA, ratioB: double;
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
319 cosHalfTheta:= -cosHalfTheta;
322 // if qa=qb or qa=-qb then theta = 0 and we can return qa
323 if abs(cosHalfTheta) >= 1.0 then begin
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));
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));
343 function gluQuaternionNlerpOrientation(const a, b: TgluQuaternion; const t: single): TgluQuaternion;
345 Result:= gluQuaternionAdd(a, gluQuaternionScale(gluQuaternionSubtract(b,a), t));
346 gluQuaternionNormalizeInplace(Result);
349 function gluQuaternionLookAt(const Location, Target, UpVector: TgluVector3f): TgluQuaternion;
351 front, up, right: TgluVector3f;
354 front:= gluVectorSubtract(Location, Target); // eigentlich falschrum. don't ask.
356 gluVectorOrthoNormalize(front, up);
357 right:= gluVectorProduct(up, front);
359 Result[quW]:= SQRT(1 + right[0] + up[1] + front[2]) * 0.5;
360 w4_recip:= 1 / (4 * Result[quW]);
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;
367 function gluVectorRotationTo(const a, b: TgluVector3f): TgluQuaternion;
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]);
378 Result:= gluQuaternion(qw, ax[0],ax[1],ax[2]);
380 gluQuaternionNormalizeInplace(Result);