{MSX}
24-01-2005, 07:23 PM
Hi! I've developed this unit that contains Vector and Quaternion algebra.
It's headed more to game developing than to math and geometry, so i found this useful.
Using quaternions and vectors you can build everythig :P Vectors for positions and quats for orientations :P
Feedback appreciated!
PS I've just finished it, maybe i will retouch it a bit (and surely comment it).
(**
This unit contains an implementation for vector and quaternion algebra
designed to be used in 3d video games.
*)
unit msxmath;
(*
This unit was written by Nicola {MSX} Lugato.
http://msx.rules.it
nicola@lugato.net
nicola.lugato@gmail.com
It is released under the term of
The GNU General Public License as published by the Free
Software Foundation.
Thanks to Arash Partow for FastGEO
*)
interface
uses math;
const epsilon = 1e-12;
type
TFloat = single;
TVector = record
case byte of
0: (v:array[0..2] of TFloat);
1: (x, y, z: TFloat);
end;
TQuaternion = record
case byte of
0: (v:array[0..3] of TFloat);
1: (x, y, z, w: TFloat);
end;
{ vectors }
function getVector(vx,vy,vz:TFloat):TVector;
function Add(const Vec1,Vec2:TVector):TVector; overload;
function Sub(const Vec1,Vec2:TVector):TVector;
function Cross(const Vec1,Vec2:TVector):TVector;
function Normalized(const Vec:TVector):TVector; overload;
procedure Normalize(var Vec:TVector); overload;
function Inverted(const Vec:TVector):TVector; overload;
procedure Invert(var Vec:TVector); overload;
function Length(const Vec:TVector):TFloat;overload;
function Dot(const Vec1,Vec2:TVector):TFloat;
function Scaled(const Vec:TVector; const Factor:TFloat):TVector;
procedure Scale(var Vec:TVector; const Factor:TFloat);
function Distance(const x1,y1,z1,x2,y2,z2:TFloat):TFloat; overload;
function Distance(const v1,v2:TVector):TFloat; overload;
function Interpolated(const v1, v2:TVector; value:TFloat):TVector;
{ quaternion }
function Add(const q1,q2:TQuaternion):TQuaternion; overload;
function Mult(const a,b:TQuaternion):TQuaternion;
procedure MultP(var a:TQuaternion; const b:TQuaternion);
function Conjugated(const a:TQuaternion):TQuaternion;
procedure Conjugate(var a:TQuaternion);
function Length(const a:TQuaternion):TFloat; overload;
function Normalized(const a:TQuaternion):TQuaternion; overload;
procedure Normalize(var a:TQuaternion); overload;
function Inverted(const a:TQuaternion):TQuaternion; overload;
procedure Invert(var a:TQuaternion); overload;
function Identity:TQuaternion; overload;
procedure Identity(var q:TQuaternion); overload;
procedure GetAxisAngle(const q:TQuaternion; var axis: TVector; var rotAngle: TFloat); overload;
procedure GetAxisAngle(const q:TQuaternion; var x,y,z,Angle: TFloat); overload;
function SetAxisAngle(const angle:TFloat; const axis:TVector):TQuaternion; overload;
procedure SetAxisAngle(var q:TQuaternion; const angle:TFloat; const axis:TVector); overload;
function SetAxisAngle(const angle, axisX, axisY, axisZ:TFloat):TQuaternion; overload;
procedure SetAxisAngle(var q:TQuaternion; const angle, axisX, axisY, axisZ:TFloat); overload;
procedure SetEuler(var q:TQuaternion; x,y,z:single);
function LookAt(const dir, up:TVector):TQuaternion;
function Slerp(const q1,q2: TQuaternion; value:single):TQuaternion;
{ misc }
function Rotated(const v:TVector; const r:TQuaternion):TVector;
procedure Rotate(var v:TVector; const r:TQuaternion);
function TranslateToFrame(const v:TVector; const Position:TVector; const Orientation:TQuaternion):TVector;
function AddRotated(v:TVector; orientation:TQuaternion; relativeVector:TVector):TVector;overload;
function AddRotated(v:TVector; orientation:TQuaternion; distanceZ:TFloat):TVector; overload;
function DistancePointLine(point,l1,l2:TVector):TFloat;
function Interpolate(a,b:TFloat; value:TFloat):TFloat;
implementation
type TMatrix3=array[0..2,0..2]of TFloat; // i need this for LookAt function
procedure swp(var a,b:single);
var c:single;
begin
c:=a;
a:=b;
b:=c;
end;
procedure Transpose(var m:TMatrix3);
begin
swp(m[1, 0], m[0, 1]);
swp(m[2, 0], m[0, 2]);
swp(m[1, 2], m[2, 1]);
end;
function Add(const Vec1,Vec2:TVector):TVector;
begin
Result.x := Vec1.x + Vec2.x;
Result.y := Vec1.y + Vec2.y;
Result.z := Vec1.z + Vec2.z;
end;
function Sub(const Vec1,Vec2:TVector):TVector;
begin
Result.x := Vec1.x - Vec2.x;
Result.y := Vec1.y - Vec2.y;
Result.z := Vec1.z - Vec2.z;
end;
function Cross(const Vec1,Vec2:TVector):TVector;
begin
Result.x := Vec1.y * Vec2.z - Vec1.z * Vec2.y;
Result.y := Vec1.z * Vec2.x - Vec1.x * Vec2.z;
Result.z := Vec1.x * Vec2.y - Vec1.y * Vec2.x;
end;
function Inverted(const Vec:TVector):TVector;
begin
Result.x := -Vec.x;
Result.y := -Vec.y;
Result.z := -Vec.z;
end;
procedure Invert(var Vec:TVector); overload;
begin
vec.x := -Vec.x;
vec.y := -Vec.y;
vec.z := -Vec.z;
end;
function Normalized(const Vec:TVector):TVector;
var Mag : TFloat;
begin
Mag := Length(Vec);
Result.x := Vec.x / Mag;
Result.y := Vec.y / Mag;
Result.z := Vec.z / Mag;
end;
procedure Normalize(var Vec:TVector); overload;
var Mag : TFloat;
begin
Mag := Length(Vec);
vec.x := Vec.x / Mag;
vec.y := Vec.y / Mag;
vec.z := Vec.z / Mag;
end;
function Length(const Vec:TVector):TFloat;
begin
Result := Sqrt((Vec.x * Vec.x) + (Vec.y * Vec.y) + (Vec.z * Vec.z));
end;
function Dot(const Vec1,Vec2:TVector):TFloat;
begin
Result := Vec1.x * Vec2.x + Vec1.y * Vec2.y + Vec1.z * Vec2.z;
end;
function Scaled(const Vec:TVector; const Factor:TFloat):TVector;
begin
Result.x := Vec.x * Factor;
Result.y := Vec.y * Factor;
Result.z := Vec.z * Factor;
end;
procedure Scale(var Vec:TVector; const Factor:TFloat);
begin
with vec do
begin
x := x * Factor;
y := y * Factor;
z := z * Factor;
end;
end;
function Add(const q1,q2:TQuaternion):TQuaternion;
begin
result.w:=q1.w+q2.w;
result.x:=q1.x+q2.x;
result.y:=q1.y+q2.y;
result.z:=q1.z+q2.z;
end;
function Mult(const a,b:TQuaternion):TQuaternion;
begin
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 MultP(var a:TQuaternion; const b:TQuaternion);
var x:TQuaternion;
begin
x:=Mult(a,b);
a:=x;
end;
function Conjugated(const a:TQuaternion):TQuaternion;
begin
result.w:=a.w;
result.x:=-a.x;
result.y:=-a.y;
result.z:=-a.z;
end;
procedure Conjugate(var a:TQuaternion);
begin
with a do
begin
x:=-x;
y:=-y;
z:=-z;
end;
end;
function Length(const a:TQuaternion):TFloat;
begin
result:=sqrt( sqr(a.w)+sqr(a.x)+sqr(a.y)+sqr(a.z) );
end;
function Normalized(const a:TQuaternion):TQuaternion;
var u:single;
begin
u:=Length(a);
with result do
begin
w:=a.w/u;
x:=a.x/u;
y:=a.y/u;
z:=a.z/u;
end;
end;
procedure Normalize(var a:TQuaternion);
var u:single;
begin
u:=Length(a);
with a do
begin
w:=w/u;
x:=x/u;
y:=y/u;
z:=z/u;
end;
end;
function Inverted(const a:TQuaternion):TQuaternion;
begin
result:=Normalized(Conjugated(a));
end;
procedure Invert(var a:TQuaternion); overload;
begin
Conjugate(a);
Normalize(a);
end;
function Identity:TQuaternion;
begin
result.w:=1;
result.x:=0;
result.y:=0;
result.z:=0;
end;
procedure Identity(var q:TQuaternion);
begin
with q do
begin
w:=1;
x:=0;
y:=0;
z:=0;
end;
end;
function Rotated(const v:TVector; const r:TQuaternion):TVector;
var u,h,t:TQuaternion;
begin
u.w:=0; u.x:=v.x; u.y:=v.y; u.z:=v.z;
h:=Mult(r,u);
t:=Inverted(r);
u:=Mult(h,t);
result.x:=u.x;
result.y:=u.y;
result.z:=u.z;
end;
procedure Rotate(var v:TVector; const r:TQuaternion);
begin
v:=rotated(v,r);
end;
procedure GetAxisAngle(const q:TQuaternion; var axis: TVector; var rotAngle: TFloat);
begin
GetAxisAngle(q,axis.x,axis.y,axis.z, rotAngle);
end;
procedure GetAxisAngle(const q:TQuaternion; var x,y,z,Angle: TFloat);
var lenOfVector,invLen, k:single;
begin
lenOfVector := q.x*q.x+q.y*q.y+q.z*q.z;
if(lenOfVector < EPSILON) then
begin
X := 1.0;
Y := 0.0;
Z := 0.0;
Angle := 0.0;
end
else
begin
invLen := (1/sqrt(lenOfVector));
X := q.x*invLen;
Y := q.y*invLen;
Z := q.z*invLen;
if q.w>1 then
k:=1
else
if q.w<-1 then
k:=-1
else
k:=q.w;
Angle := 2*arccos(k);
end;
end;
function SetAxisAngle(const angle:TFloat; const axis:TVector):TQuaternion;
begin
with axis do
SetAxisAngle(result, angle, x, y, z);
end;
function SetAxisAngle(const angle, axisX, axisY, axisZ:TFloat):TQuaternion;
begin
SetAxisAngle(result, angle, axisx, axisy, axisz);
end;
procedure SetAxisAngle(var q:TQuaternion; const angle:TFloat; const axis:TVector);
begin
with axis do
SetAxisAngle(q, angle, x, y, z);
end;
procedure SetAxisAngle(var q:TQuaternion; const angle, axisX, axisY, axisZ:TFloat);
var factor, scaleBy, sinhalfangle: single;
begin
factor := axisx*axisx+axisy*axisy+axisz*axisz;
if factor=0 then factor := EPSILON;
scaleBy := 1/sqrt(factor);
q.w := cos(angle/2);
sinHalfAngle := sin(angle/2);
q.x := axisx*scaleBy*sinHalfAngle;
q.y := axisy*scaleBy*sinHalfAngle;
q.z := axisz*scaleBy*sinHalfAngle;
end;
procedure SetEuler(var q:TQuaternion; x,y,z:single);
var xQ, yQ, zQ:TQuaternion;
begin
SetAxisAngle(xQ,x,1,0,0);
SetAxisAngle(yQ,y,0,1,0);
SetAxisAngle(zQ,z,0,0,1);
q:=xQ;
q:=Mult(q,yQ);
q:=Mult(q,zQ);
end;
function Matrix3ToQuaternion(const m:TMatrix3):TQuaternion;
var tr,s :single;
i,j,k:integer;
q:array[0..3]of single;
next:array[0..2] of integer;
begin
next[0]:=1;
next[1]:=2;
next[2]:=0;
tr:=m[0,0]+m[1,1]+m[2,2];
if tr>0 then begin
s:=sqrt(tr+1);
result.w:=s/2;
s:=0.5/s;
result.x:=(m[1,2]-m[2,1])*s;
result.y:=(m[2,0]-m[0,2])*s;
result.z:=(m[0,1]-m[1,0])*s;
end
else begin // trace<0
i:=0;
if (m[1,1]>m[0,0]) then i:=1;
if (m[2,2]>m[i,i]) then i:=2;
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[3]:=(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[0];
result.y:=q[1];
result.z:=q[2];
result.w:=q[3];
end;
end;
function MatrixLookAt(const dir, up:TVector):TMatrix3;
var xAxis, normUp, normDir:TVector;
begin
// calculate the x axis, and normalize it
xAxis := Normalized(Cross(up,dir));
// Recalculate up so that it is perpendicular to dir (and normalize it)
normup := Normalized(Cross(dir,xAxis));
// normalize dir
normdir := Normalized(dir);
result[0, 0] := xAxis.x;
result[1, 0] := xAxis.y;
result[2, 0] := xAxis.z;
result[0, 1] := normup.x;
result[1, 1] := normup.y;
result[2, 1] := normup.z;
result[0, 2] := normdir.x;
result[1, 2] := normdir.y;
result[2, 2] := normdir.z;
end;
function LookAt(const dir, up:TVector):TQuaternion;
var m:TMatrix3;
begin
m:=matrixLookAt(dir, up);
transpose(m);
result:=Matrix3ToQuaternion(m);
end;
function Slerp(const q1,q2: TQuaternion; value:single):TQuaternion;
var
omega, cosom, sinom, scale0, scale1:single;
i:integer;
q2o:TQuaternion;
begin
//Calc cosine
cosom := q1.x*q2.x + q1.y*q2.y + q1.z*q2.z + q1.w*q2.w;
//Adjust signs (if necessary)
if cosom < 0 then
begin
cosom := -cosom;
q2o.x := -q2.x;
q2o.y := -q2.y;
q2o.z := -q2.z;
q2o.w := -q2.w;
end
else
begin
q2o := q2;
end;
//Calculate coefficients
if (1-cosom) > EPSILON then
begin
//Standard case (slerp)
omega := arccos(cosom);
sinom := sin(omega);
scale0 := sin( (1-value) * omega) / sinom;
scale1 := sin( value*omega ) / sinom;
end
else
begin
//"from" and "to" quaternions are very close
//... so we can do a linear interpolation
scale0 := 1-value;
scale1 := value;
end;
//Calculate final values
result.x := scale0 * q1.x + scale1 * q2o.x;
result.y := scale0 * q1.y + scale1 * q2o.y;
result.z := scale0 * q1.z + scale1 * q2o.z;
result.w := scale0 * q1.w + scale1 * q2o.w;
end;
function TranslateToFrame(const v:TVector; const Position:TVector; const Orientation:TQuaternion):TVector;
begin
result:=Rotated(sub(v, position), inverted(orientation));
end;
function AddRotated(v:TVector; orientation:TQuaternion; relativeVector:TVector):TVector;
begin
result := add(v, rotated(relativeVector, orientation));
end;
function AddRotated(v:TVector; orientation:TQuaternion; distanceZ:TFloat):TVector;
begin
result:= AddRotated(v, orientation, getVector(0,0,distanceZ));
end;
function Distance(const x1,y1,z1,x2,y2,z2:TFloat):TFloat;
var dx : TFloat;
dy : TFloat;
dz : TFloat;
begin
dx := x2 - x1;
dy := y2 - y1;
dz := z2 - z1;
Result := Sqrt(dx * dx + dy * dy + dz * dz);
end;
function Distance(const v1,v2:TVector):TFloat;
begin
result:=distance(v1.x,v1.y,v1.z, v2.x,v2.y,v2.z);
end;
function getVector(vx,vy,vz:TFloat):TVector;
begin
with result do
begin
x:=vx;
y:=vy;
z:=vz;
end;
end;
function DistancePointLine(point,l1,l2:TVector):TFloat;
var numer, denum:TFloat;
begin
// distance is given by:
// d = |(l2-l1) X (l1-point)| / |l2-l1|
// where X is the cross product.
numer := length( cross( Sub(l2,l1), Sub(l1,point) ) );
denum := length( Sub(l2, l1) );
if denum<>0 then result:= numer / denum else result:=0;
end;
function Interpolated(const v1, v2:TVector; value:TFloat):TVector;
begin
result.x:=Interpolate(v1.x, v2.x, value);
result.y:=Interpolate(v1.y, v2.y, value);
result.z:=Interpolate(v1.z, v2.z, value);
end;
function Interpolate(a,b:TFloat; value:TFloat):TFloat;
begin
result:=a+((b-a) * value);
end;
end.
It's headed more to game developing than to math and geometry, so i found this useful.
Using quaternions and vectors you can build everythig :P Vectors for positions and quats for orientations :P
Feedback appreciated!
PS I've just finished it, maybe i will retouch it a bit (and surely comment it).
(**
This unit contains an implementation for vector and quaternion algebra
designed to be used in 3d video games.
*)
unit msxmath;
(*
This unit was written by Nicola {MSX} Lugato.
http://msx.rules.it
nicola@lugato.net
nicola.lugato@gmail.com
It is released under the term of
The GNU General Public License as published by the Free
Software Foundation.
Thanks to Arash Partow for FastGEO
*)
interface
uses math;
const epsilon = 1e-12;
type
TFloat = single;
TVector = record
case byte of
0: (v:array[0..2] of TFloat);
1: (x, y, z: TFloat);
end;
TQuaternion = record
case byte of
0: (v:array[0..3] of TFloat);
1: (x, y, z, w: TFloat);
end;
{ vectors }
function getVector(vx,vy,vz:TFloat):TVector;
function Add(const Vec1,Vec2:TVector):TVector; overload;
function Sub(const Vec1,Vec2:TVector):TVector;
function Cross(const Vec1,Vec2:TVector):TVector;
function Normalized(const Vec:TVector):TVector; overload;
procedure Normalize(var Vec:TVector); overload;
function Inverted(const Vec:TVector):TVector; overload;
procedure Invert(var Vec:TVector); overload;
function Length(const Vec:TVector):TFloat;overload;
function Dot(const Vec1,Vec2:TVector):TFloat;
function Scaled(const Vec:TVector; const Factor:TFloat):TVector;
procedure Scale(var Vec:TVector; const Factor:TFloat);
function Distance(const x1,y1,z1,x2,y2,z2:TFloat):TFloat; overload;
function Distance(const v1,v2:TVector):TFloat; overload;
function Interpolated(const v1, v2:TVector; value:TFloat):TVector;
{ quaternion }
function Add(const q1,q2:TQuaternion):TQuaternion; overload;
function Mult(const a,b:TQuaternion):TQuaternion;
procedure MultP(var a:TQuaternion; const b:TQuaternion);
function Conjugated(const a:TQuaternion):TQuaternion;
procedure Conjugate(var a:TQuaternion);
function Length(const a:TQuaternion):TFloat; overload;
function Normalized(const a:TQuaternion):TQuaternion; overload;
procedure Normalize(var a:TQuaternion); overload;
function Inverted(const a:TQuaternion):TQuaternion; overload;
procedure Invert(var a:TQuaternion); overload;
function Identity:TQuaternion; overload;
procedure Identity(var q:TQuaternion); overload;
procedure GetAxisAngle(const q:TQuaternion; var axis: TVector; var rotAngle: TFloat); overload;
procedure GetAxisAngle(const q:TQuaternion; var x,y,z,Angle: TFloat); overload;
function SetAxisAngle(const angle:TFloat; const axis:TVector):TQuaternion; overload;
procedure SetAxisAngle(var q:TQuaternion; const angle:TFloat; const axis:TVector); overload;
function SetAxisAngle(const angle, axisX, axisY, axisZ:TFloat):TQuaternion; overload;
procedure SetAxisAngle(var q:TQuaternion; const angle, axisX, axisY, axisZ:TFloat); overload;
procedure SetEuler(var q:TQuaternion; x,y,z:single);
function LookAt(const dir, up:TVector):TQuaternion;
function Slerp(const q1,q2: TQuaternion; value:single):TQuaternion;
{ misc }
function Rotated(const v:TVector; const r:TQuaternion):TVector;
procedure Rotate(var v:TVector; const r:TQuaternion);
function TranslateToFrame(const v:TVector; const Position:TVector; const Orientation:TQuaternion):TVector;
function AddRotated(v:TVector; orientation:TQuaternion; relativeVector:TVector):TVector;overload;
function AddRotated(v:TVector; orientation:TQuaternion; distanceZ:TFloat):TVector; overload;
function DistancePointLine(point,l1,l2:TVector):TFloat;
function Interpolate(a,b:TFloat; value:TFloat):TFloat;
implementation
type TMatrix3=array[0..2,0..2]of TFloat; // i need this for LookAt function
procedure swp(var a,b:single);
var c:single;
begin
c:=a;
a:=b;
b:=c;
end;
procedure Transpose(var m:TMatrix3);
begin
swp(m[1, 0], m[0, 1]);
swp(m[2, 0], m[0, 2]);
swp(m[1, 2], m[2, 1]);
end;
function Add(const Vec1,Vec2:TVector):TVector;
begin
Result.x := Vec1.x + Vec2.x;
Result.y := Vec1.y + Vec2.y;
Result.z := Vec1.z + Vec2.z;
end;
function Sub(const Vec1,Vec2:TVector):TVector;
begin
Result.x := Vec1.x - Vec2.x;
Result.y := Vec1.y - Vec2.y;
Result.z := Vec1.z - Vec2.z;
end;
function Cross(const Vec1,Vec2:TVector):TVector;
begin
Result.x := Vec1.y * Vec2.z - Vec1.z * Vec2.y;
Result.y := Vec1.z * Vec2.x - Vec1.x * Vec2.z;
Result.z := Vec1.x * Vec2.y - Vec1.y * Vec2.x;
end;
function Inverted(const Vec:TVector):TVector;
begin
Result.x := -Vec.x;
Result.y := -Vec.y;
Result.z := -Vec.z;
end;
procedure Invert(var Vec:TVector); overload;
begin
vec.x := -Vec.x;
vec.y := -Vec.y;
vec.z := -Vec.z;
end;
function Normalized(const Vec:TVector):TVector;
var Mag : TFloat;
begin
Mag := Length(Vec);
Result.x := Vec.x / Mag;
Result.y := Vec.y / Mag;
Result.z := Vec.z / Mag;
end;
procedure Normalize(var Vec:TVector); overload;
var Mag : TFloat;
begin
Mag := Length(Vec);
vec.x := Vec.x / Mag;
vec.y := Vec.y / Mag;
vec.z := Vec.z / Mag;
end;
function Length(const Vec:TVector):TFloat;
begin
Result := Sqrt((Vec.x * Vec.x) + (Vec.y * Vec.y) + (Vec.z * Vec.z));
end;
function Dot(const Vec1,Vec2:TVector):TFloat;
begin
Result := Vec1.x * Vec2.x + Vec1.y * Vec2.y + Vec1.z * Vec2.z;
end;
function Scaled(const Vec:TVector; const Factor:TFloat):TVector;
begin
Result.x := Vec.x * Factor;
Result.y := Vec.y * Factor;
Result.z := Vec.z * Factor;
end;
procedure Scale(var Vec:TVector; const Factor:TFloat);
begin
with vec do
begin
x := x * Factor;
y := y * Factor;
z := z * Factor;
end;
end;
function Add(const q1,q2:TQuaternion):TQuaternion;
begin
result.w:=q1.w+q2.w;
result.x:=q1.x+q2.x;
result.y:=q1.y+q2.y;
result.z:=q1.z+q2.z;
end;
function Mult(const a,b:TQuaternion):TQuaternion;
begin
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 MultP(var a:TQuaternion; const b:TQuaternion);
var x:TQuaternion;
begin
x:=Mult(a,b);
a:=x;
end;
function Conjugated(const a:TQuaternion):TQuaternion;
begin
result.w:=a.w;
result.x:=-a.x;
result.y:=-a.y;
result.z:=-a.z;
end;
procedure Conjugate(var a:TQuaternion);
begin
with a do
begin
x:=-x;
y:=-y;
z:=-z;
end;
end;
function Length(const a:TQuaternion):TFloat;
begin
result:=sqrt( sqr(a.w)+sqr(a.x)+sqr(a.y)+sqr(a.z) );
end;
function Normalized(const a:TQuaternion):TQuaternion;
var u:single;
begin
u:=Length(a);
with result do
begin
w:=a.w/u;
x:=a.x/u;
y:=a.y/u;
z:=a.z/u;
end;
end;
procedure Normalize(var a:TQuaternion);
var u:single;
begin
u:=Length(a);
with a do
begin
w:=w/u;
x:=x/u;
y:=y/u;
z:=z/u;
end;
end;
function Inverted(const a:TQuaternion):TQuaternion;
begin
result:=Normalized(Conjugated(a));
end;
procedure Invert(var a:TQuaternion); overload;
begin
Conjugate(a);
Normalize(a);
end;
function Identity:TQuaternion;
begin
result.w:=1;
result.x:=0;
result.y:=0;
result.z:=0;
end;
procedure Identity(var q:TQuaternion);
begin
with q do
begin
w:=1;
x:=0;
y:=0;
z:=0;
end;
end;
function Rotated(const v:TVector; const r:TQuaternion):TVector;
var u,h,t:TQuaternion;
begin
u.w:=0; u.x:=v.x; u.y:=v.y; u.z:=v.z;
h:=Mult(r,u);
t:=Inverted(r);
u:=Mult(h,t);
result.x:=u.x;
result.y:=u.y;
result.z:=u.z;
end;
procedure Rotate(var v:TVector; const r:TQuaternion);
begin
v:=rotated(v,r);
end;
procedure GetAxisAngle(const q:TQuaternion; var axis: TVector; var rotAngle: TFloat);
begin
GetAxisAngle(q,axis.x,axis.y,axis.z, rotAngle);
end;
procedure GetAxisAngle(const q:TQuaternion; var x,y,z,Angle: TFloat);
var lenOfVector,invLen, k:single;
begin
lenOfVector := q.x*q.x+q.y*q.y+q.z*q.z;
if(lenOfVector < EPSILON) then
begin
X := 1.0;
Y := 0.0;
Z := 0.0;
Angle := 0.0;
end
else
begin
invLen := (1/sqrt(lenOfVector));
X := q.x*invLen;
Y := q.y*invLen;
Z := q.z*invLen;
if q.w>1 then
k:=1
else
if q.w<-1 then
k:=-1
else
k:=q.w;
Angle := 2*arccos(k);
end;
end;
function SetAxisAngle(const angle:TFloat; const axis:TVector):TQuaternion;
begin
with axis do
SetAxisAngle(result, angle, x, y, z);
end;
function SetAxisAngle(const angle, axisX, axisY, axisZ:TFloat):TQuaternion;
begin
SetAxisAngle(result, angle, axisx, axisy, axisz);
end;
procedure SetAxisAngle(var q:TQuaternion; const angle:TFloat; const axis:TVector);
begin
with axis do
SetAxisAngle(q, angle, x, y, z);
end;
procedure SetAxisAngle(var q:TQuaternion; const angle, axisX, axisY, axisZ:TFloat);
var factor, scaleBy, sinhalfangle: single;
begin
factor := axisx*axisx+axisy*axisy+axisz*axisz;
if factor=0 then factor := EPSILON;
scaleBy := 1/sqrt(factor);
q.w := cos(angle/2);
sinHalfAngle := sin(angle/2);
q.x := axisx*scaleBy*sinHalfAngle;
q.y := axisy*scaleBy*sinHalfAngle;
q.z := axisz*scaleBy*sinHalfAngle;
end;
procedure SetEuler(var q:TQuaternion; x,y,z:single);
var xQ, yQ, zQ:TQuaternion;
begin
SetAxisAngle(xQ,x,1,0,0);
SetAxisAngle(yQ,y,0,1,0);
SetAxisAngle(zQ,z,0,0,1);
q:=xQ;
q:=Mult(q,yQ);
q:=Mult(q,zQ);
end;
function Matrix3ToQuaternion(const m:TMatrix3):TQuaternion;
var tr,s :single;
i,j,k:integer;
q:array[0..3]of single;
next:array[0..2] of integer;
begin
next[0]:=1;
next[1]:=2;
next[2]:=0;
tr:=m[0,0]+m[1,1]+m[2,2];
if tr>0 then begin
s:=sqrt(tr+1);
result.w:=s/2;
s:=0.5/s;
result.x:=(m[1,2]-m[2,1])*s;
result.y:=(m[2,0]-m[0,2])*s;
result.z:=(m[0,1]-m[1,0])*s;
end
else begin // trace<0
i:=0;
if (m[1,1]>m[0,0]) then i:=1;
if (m[2,2]>m[i,i]) then i:=2;
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[3]:=(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[0];
result.y:=q[1];
result.z:=q[2];
result.w:=q[3];
end;
end;
function MatrixLookAt(const dir, up:TVector):TMatrix3;
var xAxis, normUp, normDir:TVector;
begin
// calculate the x axis, and normalize it
xAxis := Normalized(Cross(up,dir));
// Recalculate up so that it is perpendicular to dir (and normalize it)
normup := Normalized(Cross(dir,xAxis));
// normalize dir
normdir := Normalized(dir);
result[0, 0] := xAxis.x;
result[1, 0] := xAxis.y;
result[2, 0] := xAxis.z;
result[0, 1] := normup.x;
result[1, 1] := normup.y;
result[2, 1] := normup.z;
result[0, 2] := normdir.x;
result[1, 2] := normdir.y;
result[2, 2] := normdir.z;
end;
function LookAt(const dir, up:TVector):TQuaternion;
var m:TMatrix3;
begin
m:=matrixLookAt(dir, up);
transpose(m);
result:=Matrix3ToQuaternion(m);
end;
function Slerp(const q1,q2: TQuaternion; value:single):TQuaternion;
var
omega, cosom, sinom, scale0, scale1:single;
i:integer;
q2o:TQuaternion;
begin
//Calc cosine
cosom := q1.x*q2.x + q1.y*q2.y + q1.z*q2.z + q1.w*q2.w;
//Adjust signs (if necessary)
if cosom < 0 then
begin
cosom := -cosom;
q2o.x := -q2.x;
q2o.y := -q2.y;
q2o.z := -q2.z;
q2o.w := -q2.w;
end
else
begin
q2o := q2;
end;
//Calculate coefficients
if (1-cosom) > EPSILON then
begin
//Standard case (slerp)
omega := arccos(cosom);
sinom := sin(omega);
scale0 := sin( (1-value) * omega) / sinom;
scale1 := sin( value*omega ) / sinom;
end
else
begin
//"from" and "to" quaternions are very close
//... so we can do a linear interpolation
scale0 := 1-value;
scale1 := value;
end;
//Calculate final values
result.x := scale0 * q1.x + scale1 * q2o.x;
result.y := scale0 * q1.y + scale1 * q2o.y;
result.z := scale0 * q1.z + scale1 * q2o.z;
result.w := scale0 * q1.w + scale1 * q2o.w;
end;
function TranslateToFrame(const v:TVector; const Position:TVector; const Orientation:TQuaternion):TVector;
begin
result:=Rotated(sub(v, position), inverted(orientation));
end;
function AddRotated(v:TVector; orientation:TQuaternion; relativeVector:TVector):TVector;
begin
result := add(v, rotated(relativeVector, orientation));
end;
function AddRotated(v:TVector; orientation:TQuaternion; distanceZ:TFloat):TVector;
begin
result:= AddRotated(v, orientation, getVector(0,0,distanceZ));
end;
function Distance(const x1,y1,z1,x2,y2,z2:TFloat):TFloat;
var dx : TFloat;
dy : TFloat;
dz : TFloat;
begin
dx := x2 - x1;
dy := y2 - y1;
dz := z2 - z1;
Result := Sqrt(dx * dx + dy * dy + dz * dz);
end;
function Distance(const v1,v2:TVector):TFloat;
begin
result:=distance(v1.x,v1.y,v1.z, v2.x,v2.y,v2.z);
end;
function getVector(vx,vy,vz:TFloat):TVector;
begin
with result do
begin
x:=vx;
y:=vy;
z:=vz;
end;
end;
function DistancePointLine(point,l1,l2:TVector):TFloat;
var numer, denum:TFloat;
begin
// distance is given by:
// d = |(l2-l1) X (l1-point)| / |l2-l1|
// where X is the cross product.
numer := length( cross( Sub(l2,l1), Sub(l1,point) ) );
denum := length( Sub(l2, l1) );
if denum<>0 then result:= numer / denum else result:=0;
end;
function Interpolated(const v1, v2:TVector; value:TFloat):TVector;
begin
result.x:=Interpolate(v1.x, v2.x, value);
result.y:=Interpolate(v1.y, v2.y, value);
result.z:=Interpolate(v1.z, v2.z, value);
end;
function Interpolate(a,b:TFloat; value:TFloat):TFloat;
begin
result:=a+((b-a) * value);
end;
end.