Ви не можете вибрати більше 25 тем Теми мають розпочинатися з літери або цифри, можуть містити дефіси (-) і не повинні перевищувати 35 символів.

385 рядки
12 KiB

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