OpenGLCore/ugluQuaternion.pas

unit ugluQuaternion;

{ Package:      OpenGLCore
  Prefix:       glu - OpenGL Utils
  Beschreibung: diese Unit enthält Quaternion-Typen und Methoden um diese zu erstellen und zu manipulieren }

{$mode objfpc}{$H+}

interface

uses
  Classes, SysUtils, ugluVector, ugluMatrix;

type
  TgluQuaternion = type TgluVector4f; // w,x,y,z

{
   Winkel                           : rad, außer glRotate-Komponenten
   Absolute Werte                   : Orientation
   Relative Werte/"Drehanweisungen" : Rotation
   To/FromVector                    : Position im R^4

   Alle Funktionen nehmen an, dass die Quaternion nur zur Rotation verwendet wird (kein Scale),
   mathematisch also normalisiert ist. Es findet keine Überprüfung statt.
}

  //Quaternion Konstruktoren
  function gluQuaternion(const W, X, Y, Z: Single): TgluQuaternion;
  function gluQuaternionNormalize(const q: TgluQuaternion): TgluQuaternion;
  procedure gluQuaternionNormalizeInplace(var q: TgluQuaternion);
  function gluQuaternionToVector(const q: TgluQuaternion): TgluVector3f;
  function gluVectorToQuaternion(const v: TgluVector3f): TgluQuaternion;

  //Arithmetic
  function gluQuaternionConjugate(const q: TgluQuaternion): TgluQuaternion;
  function gluQuaternionMultiply(const l,r: TgluQuaternion): TgluQuaternion;
  function gluQuaternionAdd(const a,b: TgluQuaternion): TgluQuaternion;
  function gluQuaternionSubtract(const l,r: TgluQuaternion): TgluQuaternion;
  function gluQuaternionScale(const q: TgluQuaternion; const f: Single): TgluQuaternion;


  //To/From RotationMatrix
  function gluQuaternionToMatrix(const q: TgluQuaternion): TgluMatrix4f;
  function gluMatrixToQuaternion(const m: TgluMatrix4f): TgluQuaternion;

  //To/From Axis/Angle {WINKEL IN °DEG}
  function gluQuaternionToRotation(const q: TgluQuaternion; out angle: Single): TgluVector3f;
  function gluRotationToQuaternion(const angle: Single; const axis: TgluVector3f): TgluQuaternion;

  //Transforms
  function gluQuaternionTransformVec(const q: TgluQuaternion; const v: TgluVector3f): TgluVector3f;

  function gluQuaternionLookAt(const Location, Target, UpVector: TgluVector3f): TgluQuaternion;
  //Rotation zw. Richtungen: Wie muss a modifiziert werden um b zu bekommen?
  function gluVectorRotationTo(const a, b: TgluVector3f): TgluQuaternion;

  //Modifying Quaternions
  function gluQuaternionHalfAngle(const q: TgluQuaternion): TgluQuaternion;
  function gluQuaternionAngleBetween(const a, b: TgluQuaternion): double;
  function gluQuaternionSlerpOrientation(const a, b: TgluQuaternion; const t: single): TgluQuaternion;
  function gluQuaternionNlerpOrientation(const a, b: TgluQuaternion; const t: single): TgluQuaternion;

  operator +(const a, b: TgluQuaternion): TgluQuaternion;
  operator -(const l, r: TgluQuaternion): TgluQuaternion;
  operator *(const l, r: TgluQuaternion): TgluQuaternion;
  operator *(const q: TgluQuaternion; const s: Single): TgluQuaternion;

const
  quW = 0;
  quX = 1;
  quY = 2;
  quZ = 3;
  gluQuaternionIdentity: TgluQuaternion = (1,0,0,0);

implementation

uses
  Math;

operator +(const a, b: TgluQuaternion): TgluQuaternion;
begin
  result := gluQuaternionAdd(a, b);
end;

operator -(const l, r: TgluQuaternion): TgluQuaternion;
begin
  result := gluQuaternionSubtract(l, r);
end;

operator *(const l, r: TgluQuaternion): TgluQuaternion;
begin
  result := gluQuaternionMultiply(l, r);
end;

operator *(const q: TgluQuaternion; const s: Single): TgluQuaternion;
begin
  result := gluQuaternionScale(q, s);
end;

function gluQuaternion(const W, X, Y, Z: Single): TgluQuaternion;
begin
  Result:= gluVector4f(W,X,Y,Z);
end;

function gluQuaternionNormalize(const q: TgluQuaternion): TgluQuaternion;
begin
  Result:= q;
  gluQuaternionNormalizeInplace(Result);
end;

procedure gluQuaternionNormalizeInplace(var q: TgluQuaternion);
var
  s: Double;
begin
  s:= sqr(q[quX])+sqr(q[quY])+sqr(q[quZ])+sqr(q[quW]);
  // already normalized?
  if IsZero(s - 1) then
    exit;
  s:= 1/sqrt(s);
  q[quX]:= q[quX] * s;
  q[quY]:= q[quY] * s;
  q[quZ]:= q[quZ] * s;
  q[quW]:= q[quW] * s;
end;

function gluQuaternionToVector(const q: TgluQuaternion): TgluVector3f;
begin
  Result:= gluVector3f(q[quX], q[quY], q[quZ]);
end;

function gluVectorToQuaternion(const v: TgluVector3f): TgluQuaternion;
begin
  Result:= gluQuaternion(0, v[0], v[1], v[2]);
end;


function gluQuaternionConjugate(const q: TgluQuaternion): TgluQuaternion;
begin
  Result[quW] := q[quW];
  Result[quX] := -q[quX];
  Result[quY] := -q[quY];
  Result[quZ] := -q[quZ];
end;

function gluQuaternionMultiply(const l, r: TgluQuaternion): TgluQuaternion;
begin
  Result[quW] := -l[qux] * r[qux] - l[quy] * r[quy] - l[quz] * r[quz] + l[quw] * r[quw];
  Result[quX] :=  l[qux] * r[quw] + l[quy] * r[quz] - l[quz] * r[quy] + l[quw] * r[qux];
  Result[quY] := -l[qux] * r[quz] + l[quy] * r[quw] + l[quz] * r[qux] + l[quw] * r[quy];
  Result[quZ] :=  l[qux] * r[quy] - l[quy] * r[qux] + l[quz] * r[quw] + l[quw] * r[quz];
end;

function gluQuaternionAdd(const a, b: TgluQuaternion): TgluQuaternion;
begin
  Result[quW] := a[quW] + b[quW];
  Result[quX] := a[quX] + b[quX];
  Result[quY] := a[quY] + b[quY];
  Result[quZ] := a[quZ] + b[quZ];
end;

function gluQuaternionSubtract(const l, r: TgluQuaternion): TgluQuaternion;
begin
  Result[quW] := l[quW] - r[quW];
  Result[quX] := l[quX] - r[quX];
  Result[quY] := l[quY] - r[quY];
  Result[quZ] := l[quZ] - r[quZ];
end;

function gluQuaternionScale(const q: TgluQuaternion; const f: Single): TgluQuaternion;
begin
  Result[quW] := q[quW] * f;
  Result[quX] := q[quX] * f;
  Result[quY] := q[quY] * f;
  Result[quZ] := q[quZ] * f;
end;

// http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToMatrix/index.htm
function gluQuaternionToMatrix(const q: TgluQuaternion): TgluMatrix4f;
var
  qx,qy,qz,qw: Single;
begin
  qw:= q[quW];
  qx:= q[quX];
  qy:= q[quY];
  qz:= q[quZ];
  Result:= gluMatrixIdentity;
  Result[maAxisX] := gluVector4f(
    1 - 2*SQR(qy) - 2*SQR(qz),
    2*qx*qy + 2*qz*qw,
    2*qx*qz - 2*qy*qw,
    0);
  Result[maAxisY] := gluVector4f(
    2*qx*qy - 2*qz*qw,
    1 - 2*SQR(qx) - 2*SQR(qz),
    2*qy*qz + 2*qx*qw,
    0);
  Result[maAxisZ] := gluVector4f(
    2*qx*qz + 2*qy*qw,
    2*qy*qz - 2*qx*qw,
   	1 - 2*SQR(qx) - 2*SQR(qy),
    0);
end;

// http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/index.htm
function gluMatrixToQuaternion(const m: TgluMatrix4f): TgluQuaternion;
var
  trace, s: double;
  q: TgluQuaternion;

begin
  trace := m[0][0] + m[1][1] + m[2][2]; // I removed + 1.0f; see discussion with Ethan
  if( trace > 0 ) then begin// I changed M_EPSILON to 0
    s := 0.5 / SQRT(trace+ 1.0);
    q[quW] := 0.25 / s;
    q[quX] := ( m[2][1] - m[1][2] ) * s;
    q[quY] := ( m[0][2] - m[2][0] ) * s;
    q[quZ] := ( m[1][0] - m[0][1] ) * s;
  end else begin
    if ( m[0][0] > m[1][1]) and (m[0][0] > m[2][2] ) then begin
      s := 2.0 * SQRT( 1.0 + m[0][0] - m[1][1] - m[2][2]);
      q[quW] := (m[2][1] - m[1][2] ) / s;
      q[quX] := 0.25 * s;
      q[quY] := (m[0][1] + m[1][0] ) / s;
      q[quZ] := (m[0][2] + m[2][0] ) / s;
    end else if (m[1][1] > m[2][2]) then begin
      s := 2.0 * SQRT( 1.0 + m[1][1] - m[0][0] - m[2][2]);
      q[quW] := (m[0][2] - m[2][0] ) / s;
      q[quX] := (m[0][1] + m[1][0] ) / s;
      q[quY] := 0.25 * s;
      q[quZ] := (m[1][2] + m[2][1] ) / s;
    end else begin
      s := 2.0 * SQRT( 1.0 + m[2][2] - m[0][0] - m[1][1] );
      q[quW] := (m[1][0] - m[0][1] ) / s;
      q[quX] := (m[0][2] + m[2][0] ) / s;
      q[quY] := (m[1][2] + m[2][1] ) / s;
      q[quZ] := 0.25 * s;
    end;
  end;
  Result:= q;
end;

// http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToAngle/index.htm
function gluQuaternionToRotation(const q: TgluQuaternion; out angle: Single): TgluVector3f;
var
  s: double;
begin
  angle := radtodeg(2 * arccos(q[quW]));
  s := sqrt(1-q[quW]*q[quW]); // assuming quaternion normalised then w is less than 1, so term always positive.
  if (s < 0.001) then begin // test to avoid divide by zero, s is always positive due to sqrt
    // if s close to zero then direction of axis not important
    Result[0] := q[quX]; // if it is important that axis is normalised then replace with x=1; y=z=0;
    Result[1] := q[quY];
    Result[2] := q[quZ];
  end else begin
    Result[0] := q[quX] / s; // normalise axis
    Result[1] := q[quY] / s;
    Result[2] := q[quZ] / s;
  end;
end;

function gluRotationToQuaternion(const angle: Single; const axis: TgluVector3f): TgluQuaternion;
var
  a: single;
begin
  a:= degtorad(angle) / 2;
  Result:= gluQuaternion(
    cos(a),
    sin(a) * axis[0],
    sin(a) * axis[1],
    sin(a) * axis[2]);
end;

// http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/transforms/index.htm
function gluQuaternionTransformVec(const q: TgluQuaternion; const v: TgluVector3f): TgluVector3f;
var
  p: TgluQuaternion;
begin
  //Pout = q * Pin * q'
  p:= gluQuaternionMultiply(q, gluVectorToQuaternion(v));
  p:= gluQuaternionMultiply(p, gluQuaternionConjugate(q));
  Result:= gluQuaternionToVector(p);
end;

// http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/transforms/halfAngle.htm
function gluQuaternionHalfAngle(const q: TgluQuaternion): TgluQuaternion;
begin
  Result:= q;
  Result[quW]:= Result[quW] + 1;
  gluQuaternionNormalizeInplace(Result);
end;

function gluQuaternionAngleBetween(const a, b: TgluQuaternion): double;
var
  cosHalfTheta: double;
begin
  cosHalfTheta:= a[quW] * b[quW] + a[quX] * b[quX] + a[quY] * b[quY] + a[quZ] * b[quZ];
  Result:= arccos(cosHalfTheta) * 2;
end;

// http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/slerp/index.htm
function gluQuaternionSlerpOrientation(const a, b: TgluQuaternion; const t: single): TgluQuaternion;
var
  qa,qb: TgluQuaternion;
  cosHalfTheta, sinHalfTheta,
    halfTheta,
    ratioA, ratioB: double;
begin
  qa:= a;
  qb:= b;
	// Calculate angle between them.
  cosHalfTheta:= a[quW] * b[quW] + a[quX] * b[quX] + a[quY] * b[quY] + a[quZ] * b[quZ];
  if (cosHalfTheta < 0) then begin
    qb:= gluQuaternion(
      -b[quW],
      -b[quX],
      -b[quY],
       b[quZ]
    );
    cosHalfTheta:= -cosHalfTheta;
  end;

  // if qa=qb or qa=-qb then theta = 0 and we can return qa
	if abs(cosHalfTheta) >= 1.0 then begin
		Result:= qa;
    Exit;
	end;

	// Calculate temporary values.
	halfTheta := arccos(cosHalfTheta);
	sinHalfTheta := sqrt(1.0 - sqr(cosHalfTheta));
	// if theta = 180 degrees then result is not fully defined
	// we could rotate around any axis normal to qa or qb
	if (abs(sinHalfTheta) < 0.001) then begin
    Result:= gluQuaternionAdd(gluQuaternionScale(qa, 0.5), gluQuaternionScale(qb, 0.5));
    exit
  end;
	ratioA := sin((1 - t) * halfTheta) / sinHalfTheta;
	ratioB := sin(t * halfTheta) / sinHalfTheta;
	//calculate Quaternion.
  Result:= gluQuaternionAdd(gluQuaternionScale(qa, ratioA), gluQuaternionScale(qb, ratioB));
end;

function gluQuaternionNlerpOrientation(const a, b: TgluQuaternion; const t: single): TgluQuaternion;
begin
  Result:= gluQuaternionAdd(a, gluQuaternionScale(gluQuaternionSubtract(b,a), t));
  gluQuaternionNormalizeInplace(Result);
end;

function gluQuaternionLookAt(const Location, Target, UpVector: TgluVector3f): TgluQuaternion;
var
  front, up, right: TgluVector3f;
  w4_recip: Single;
begin
  front:= gluVectorSubtract(Location, Target); // eigentlich falschrum. don't ask.
  up:= UpVector;
  gluVectorOrthoNormalize(front, up);
  right:= gluVectorProduct(up, front);

  Result[quW]:= SQRT(1 + right[0] + up[1] + front[2]) * 0.5;
  w4_recip:= 1 / (4 * Result[quW]);

  Result[quX]:= (front[1] - up[2]) * w4_recip;
	Result[quY]:= (right[2] - front[0]) * w4_recip;
	Result[quZ]:= (up[0] - right[1]) * w4_recip;
end;

function gluVectorRotationTo(const a, b: TgluVector3f): TgluQuaternion;
var
  d, qw: single;
  ax: TgluVector3f;
begin
  d:=gluVectorScalar(a, b);
  ax:= gluVectorProduct(a, b);
  qw:= gluVectorLength(a) * gluVectorLength(b) + d;
	if (qw < 0.0001) then begin // vectors are 180 degrees apart
    Result:= gluQuaternion(0, -a[2],a[1],a[0]);
  end else begin
    Result:= gluQuaternion(qw, ax[0],ax[1],ax[2]);
  end;
  gluQuaternionNormalizeInplace(Result);
end;

end.