OpenGL quaternions

unit Quaternions;

interface

type
 QuatPtr=^Quat;
 Quat=RECORD
  w,x,y,z:single;
 end;

 VectPtr=^Vect;
 Vect=record
  x,y,z:single;
 end;

 PGLMatrixf=^GLMatrixf;
 //GLMatrixf=array[1..16]of single;  // column major matrix 4x4, down first
 GlMatrixf=array[1..4,1..4]of single; // please check
//  [1] [5] [ 9] [13]
//  [2] [6] [10] [14]
//  [3] [7] [11] [15]
//  [4] [8] [12] [16]

// basic quaternion operations
procedure Q_Add(a,b:QuatPtr);   // a:=a+b;
function Q_AddN(a,b:QuatPtr):QuatPtr;   // result:=a+b;
procedure Q_Mult(var a,b:QuatPtr);   // a:=a*b;
function Q_MultN(a,b:QuatPtr):QuatPtr;   // result:=a*b;
procedure Q_Conj(a:QuatPtr);            // a:=conj(a);
function Q_ConjN(a:QuatPtr):QuatPtr;   // result:=conj(a);
function Q_norm(a:QuatPtr):single;      // result:=norm(a);
procedure Q_Inv(a:QuatPtr);            // a:=inv(a);
function Q_InvN(a:QuatPtr):QuatPtr;   // result:=inv(a);
procedure Q_normed(a:QuatPtr);          // normalize(a);

// rotate a vector
function rotateN(a:QuatPtr;v:VectPtr):VectPtr; // result:= v rotate by a
procedure rotate(a:QuatPtr;v:VectPtr); // a:= v rotate by a

function Q2M4(a:QuatPtr):PGLMatrixf;   // quaternion to matrix 4x4
function M42Q(m:PGLMatrixf):QuatPtr;    // matrix 4x4 to quaternion


implementation

procedure Q_Add(a,b:QuatPtr);   // a:=a+b;
begin
 a.w:=a.w+b.w;
 a.x:=a.x+b.x; a.y:=a.y+b.y; a.z:=a.z+b.z;
end;

function Q_AddN(a,b:QuatPtr):QuatPtr;   // result:=a+b;
begin
 result:=new(QuatPtr);
 result.w:=a.w+b.w;
 result.x:=a.x+b.x; result.y:=a.y+b.y; result.z:=a.z+b.z;
end;

procedure Q_Mult(VAR a,b:QuatPtr);   // a:=a*b;
var h:Quatptr;
begin
 h:=Q_MultN(a,b);
 dispose(a);
 a:=h;
end;

function Q_MultN(a,b:QuatPtr):QuatPtr;   // result:=a*b;
begin
 result:=new(QuatPtr);
 result.w:=a.w*b.w-a.x*b.x-a.y*b.y-a.z*b.z;
 result.x:=a.y*b.z-a.z*b.y+a.w*b.x+b.w*a.x;
 result.y:=a.z*b.x-a.x*b.z+a.w*b.y+b.w*a.y;
 result.z:=a.x*b.y-a.y*b.x+a.w*b.z+b.w*a.z;
end;

procedure Q_Conj(a:QuatPtr);            // a:=conj(a);
begin
 a.x:=-a.x; a.y:=-a.y; a.z:=-a.z;
end;

function Q_ConjN(a:QuatPtr):QuatPtr;   // result:=conj(a);
begin
 result:=new(QuatPtr);
 result.w:=a.w; result.x:=-a.x; result.y:=-a.y; result.z:=-a.z;
end;

function Q_norm(a:QuatPtr):single;      // result:=norm(a);
begin
 result:=sqr(a.w)+sqr(a.x)+sqr(a.y)+sqr(a.z);
end;

procedure Q_Inv(a:QuatPtr);            // a:=inv(a);
begin
 Q_Conj(a);
 Q_normed(a);
end;

function Q_InvN(a:QuatPtr):QuatPtr;   // result:=inv(a);
begin
 result:=Q_ConjN(a);
 Q_normed(result);
end;

procedure Q_normed(a:QuatPtr);          // normalize(a);
var u:single;
begin
 u:=Q_norm(a);
 a.w:=a.w/u; a.x:=a.x/u; a.y:=a.y/u; a.z:=a.z/u;
end;

function rotateN(a:QuatPtr;v:VectPtr):VectPtr; // result:= v rotate by a
var u,h,t:QuatPtr;
begin
 u:=new(QuatPtr);
 u.w:=0; u.x:=v.x; u.y:=v.y; u.z:=v.z;
 h:=Q_MultN(a,u);
 dispose(u);
 t:=Q_invN(a);
 u:=Q_multN(h,t);
 result:=new(VectPtr);
 result.x:=u.x; result.y:=u.y; result.z:=u.z;
 dispose(t);
 dispose(u);
end;

procedure rotate(a:QuatPtr;v:VectPtr); // a:= v rotate by a
var u,h,t:QuatPtr;
begin
 u:=new(QuatPtr);
 u.w:=0; u.x:=v.x; u.y:=v.y; u.z:=v.z;
 h:=Q_MultN(a,u);
 dispose(u);
 t:=Q_invN(a);
 u:=Q_multN(h,t);
 v.x:=u.x; v.y:=u.y; v.z:=u.z;
 dispose(t);
 dispose(u);
end;

function Q2M4(a:QuatPtr):PGLMatrixf;   // quaternion to matrix 4x4
var x2,y2,z2,xy,wx,wy,wz,xz,yz:single;
begin
 result:=new(PGLMatrixf);
 x2:=2*sqr(a.x); y2:=2*sqr(a.y); z2:=2*sqr(a.z);
 xy:=2*a.x*a.y;  wz:=2*a.w*a.z;  yz:=2*a.y*a.z;
 xz:=2*a.x*a.z;  wy:=2*a.w*a.y;  wx:=2*a.w*a.x;
 result[1,1]:=1-y2-z2; result[2,1]:=xy+wz;   result[3,1]:=xz-wz;   result[4,1]:=0;
 result[1,2]:=xy-wz;   result[2,2]:=1-x2-z2; result[3,2]:=yz-wx;   result[4,2]:=0;
 result[1,3]:=xz-wy;   result[2,3]:=yz-wx;   result[3,3]:=1-x2-y2; result[4,3]:=0;
 result[1,4]:=0;       result[2,4]:=0;       result[3,4]:=0;       result[4,4]:=1;
end;

function M42Q(m:PGLMatrixf):QuatPtr;    // matrix 4x4 to quaternion
var tr,s :single;
    i,j,k:integer;
    q:array[1..4]of single;
 next:array[1..3] of integer;
begin
 next[1]:=2; next[2]:=3; next[3]:=1;
 result:=new(QuatPtr);
 tr:=m[1,1]+m[2,2]+m[3,3];
 if tr>0 then begin
  s:=sqrt(tr+1);
  result.w:=s/2;
  s:=0.5/s;
  result.x:=(m[2,3]-m[3,2])*s;
  result.y:=(m[3,1]-m[1,3])*s;
  result.z:=(m[1,2]-m[2,1])*s;
 end
 else begin // trace<0
  i:=1;
  if (m[2,2]>m[1,1]) then i:=2;
  if (m[3,3]>m[i,i]) then i:=3;
  j:=next[i];
  k:=next[j];
  s:=sqrt((m[i,i]-m[j,j]+m[k,k]))+1;
  q[i]:=s*0.5;
  if (s<>0)then s:=0.5/s;
  q[4]:=(m[j,k]-m[k,j])*s;
  q[j]:=(m[i,j]-m[j,i])*s;
  q[k]:=(m[i,k]-m[k,i])*s;
  result.x:=q[1];
  result.y:=q[2];
  result.z:=q[3];
  result.w:=q[4];
 end;
end;


end.