{$MODE OBJFPC}
unit dquat4f;

interface

uses
  dmath,
  dvector3f,
  dvector4f,
  dmatrix4f;

type
TFloat = Single;

TQuat4f = packed record
case Integer of
  0: (X, Y, Z, W: TFloat);
  1: (V: TVec3f; S: TFloat);
  2: (V4f: TVec4f);
end;

const
  IdentityQuat4f: TQuat4f = (X: 0; Y: 0; Z: 0; W: 1);

function Quat4f(X, Y, Z, W: TFloat): TQuat4f; inline;
function Quat4f(const V: TVec3f; Angle: TFloat): TQuat4f; inline;
function Quat4f(const V4f: TVec4f): TQuat4f; inline;
function Quat4f(const Mat: TMat4f): TQuat4f;
function QuatAdd4f(const P, Q: TQuat4f): TQuat4f; overload;
function QuatMul4f(const P, Q: TQuat4f): TQuat4f; overload;
function QuatMul4f(const Q: TQuat4f; S: TFloat): TQuat4f; inline; overload;
function QuatLen4f(const Q: TQuat4f): TFloat; inline;
function QuatNorm4f(const Q: TQuat4f): TQuat4f; inline;
function QuatConjugate4f(const Q: TQuat4f): TQuat4f; inline;
function QuatInv4f(const Q: TQuat4f): TQuat4f;
function QuatRot4f(const Q: TQuat4f; const V: TVec3f): TVec3f;
function QuatAngle4f(const P, Q: TQuat4f): TFloat; inline;
function Matrix4f(const Q: TQuat4f): TMat4f; overload; inline;
function QuatLerp4f(const P, Q: TQuat4f; F: TFloat): TQuat4f; inline;
function QuatSLerp4f(const P, Q: TQuat4f; F: TFloat): TQuat4f; inline;
function QuatFromPitchRollYawf(Pitch, Roll, Yaw: Single): TQuat4f;
// TODO Matrix -> Quat

implementation

function Quat4f(X, Y, Z, W: TFloat): TQuat4f;
begin
  Result.X := X;
  Result.Y := Y;
  Result.Z := Z;
  Result.W := W;
end;

function Quat4f(const V: TVec3f; Angle: TFloat): TQuat4f;
var
  S: TFloat;
begin
  S := Sin(Angle / 2);
  Result.X := S * V.X;
  Result.Y := S * V.Y;
  Result.Z := S * V.Z;
  Result.W := Cos(Angle / 2);
end;

function Quat4f(const V4f: TVec4f): TQuat4f;
begin
  Result.V4f := V4f;
end;

function Quat4f(const Mat: TMat4f): TQuat4f;
var
  W4: Single;
begin
  Result.W := Sqrt(1.0 + Mat[0 + 0 * 4] + Mat[1 + 1 * 4] + Mat[1 + 2 * 4]) / 2;
  W4 := Result.W / 4;
  Result.X := (Mat[2 + 1 * 4] - Mat[1 + 2 * 4]) / 4;
  Result.Y := (Mat[0 + 2 * 4] - Mat[2 + 0 * 4]) / 4;
  Result.Z := (Mat[1 + 0 * 4] - Mat[0 + 1 * 4]) / 4;
end;

function QuatAdd4f(const P, Q: TQuat4f): TQuat4f;
begin
  Result.X := P.X + Q.X;
  Result.Y := P.Y + Q.Y;
  Result.Z := P.Z + Q.Z;
  Result.W := P.W + Q.W;
end;

function QuatMul4f(const P, Q: TQuat4f): TQuat4f;
begin
  Result.V := Add3f(Add3f(Mul3f(Q.V, P.S), Mul3f(P.V, Q.S)), Cross3f(P.V, Q.V));
  Result.S := P.S * Q.S - Dot3f(P.V, Q.V);
end;

function QuatMul4f(const Q: TQuat4f; S: TFloat): TQuat4f;
begin
  Result.X := S * Q.X;
  Result.Y := S * Q.Y;
  Result.Z := S * Q.Z;
  Result.W := S * Q.W;
end;

function QuatLen4f(const Q: TQuat4f): TFloat;
begin
  Result := Len4f(Q.V4f);
end;

function QuatNorm4f(const Q: TQuat4f): TQuat4f;
begin
  Result.V4f := Norm4f(Q.V4f);
end;

function QuatConjugate4f(const Q: TQuat4f): TQuat4f;
begin
  Result.X := - Q.X;
  Result.Y := - Q.Y;
  Result.Z := - Q.Z;
  Result.W := Q.W;
end;

function QuatInv4f(const Q: TQuat4f): TQuat4f;
var
  QLen: TFloat;
begin
  QLen := QLen4f(Q.V4f);
  Result.X := - Q.X / QLen;
  Result.Y := - Q.Y / QLen;
  Result.Z := - Q.Z / QLen;
  Result.W := Q.W / QLen;
end;

function QuatRot4f(const Q: TQuat4f; const V: TVec3f): TVec3f;
var
  P: TQuat4f;
begin
  P := Quat4f(V.X, V.Y, V.Z, 0);
  Result := QuatMul4f(Q, QuatMul4f(P, QuatConjugate4f(Q))).V;
end;

function QuatAngle4f(const P, Q: TQuat4f): TFloat;
begin
  Result := ArcCos(Dot4f(P.V4F, Q.V4F));
end;

function Matrix4f(const Q: TQuat4f): TMat4f;
var
  X2, Y2, Z2, XY, XZ, XW, YZ, YW, ZW: TFloat;
begin
  with Q do begin
    X2 := 2 * Sqr(X);
    Y2 := 2 * Sqr(Y);
    Z2 := 2 * Sqr(Z);
    XY := 2 * X * Y;
    XZ := 2 * X * Z;
    XW := 2 * X * W;
    YZ := 2 * Y * Z;
    YW := 2 * Y * W;
    ZW := 2 * Z * W;
    // 0 1 2 3
    // 4 5 6 7
    // 8 9 10 11
    // 12 13 14 15
    Result[0] := 1 - Y2 - Z2;
    Result[1] := XY + ZW;
    Result[2] := XZ - YW;
    Result[3] := 0;
    Result[4] := XY - ZW;
    Result[5] := 1 - X2 - Z2;
    Result[6] := YZ + XW;
    Result[7] := 0;
    Result[8] := XZ + YW;
    Result[9] := YZ - XW;
    Result[10] := 1 - X2 - Y2;
    Result[11] := 0;
    Result[12] := 0;
    Result[13] := 0;
    Result[14] := 0;
    Result[15] := 1;
  end;
end;

function QuatLerp4f(const P, Q: TQuat4f; F: TFloat): TQuat4f;
begin
  Result.V4f := Norm4f(Add4f(Mul4f(P.V4f, 1 - F), Mul4f(Q.V4f, F)));
end;

function QuatSLerp4f(const P, Q: TQuat4f; F: TFloat): TQuat4f;
var
  C, S, Angle: TFloat;
  wp, wq: TFloat;
begin
  C := Dot4f(P.V4F, Q.V4F);
  Angle := ArcCos(C);
  S := sin(Angle);
  wp := sin((1 - F) * Angle) / S;
  wq := sin(F * Angle) / S;
  Result.V4F := Add4f(Mul4f(P.V4f, wp), Mul4f(Q.V4f, wq));
end;

function QuatFromPitchRollYawf(Pitch, Roll, Yaw: Single): TQuat4f;
var
  cy, sy, cr, sr, cp, sp: Single;
begin
  cy := Cos(Yaw * 0.5);
  sy := Sin(Yaw * 0.5);
  cr := Cos(Roll * 0.5);
  sr := Sin(Roll * 0.5);
  cp := Cos(Pitch * 0.5);
  sp := Sin(Pitch * 0.5);

  Result.W := cy * cr * cp + sy * sr * sp;
  Result.X := cy * sr * cp - sy * cr * sp;
  Result.Y := cy * cr * sp + sy * sr * cp;
  Result.Z := sy * cr * cp - cy * sr * sp;
end;

end.
