Quaternion & Dual Quaternion math

quat.pas

  TQuat = {$IFDEF FPC} object {$ELSE} record {$ENDIF}
    x, y, z, w : Single;
  {$IFNDEF FPC}
    class operator Equal(const q1, q2: TQuat): Boolean;
    class operator Add(const q1, q2: TQuat): TQuat;
    class operator Subtract(const q1, q2: TQuat): TQuat;
    class operator Multiply(const q: TQuat; x: Single): TQuat;
    class operator Multiply(const q1, q2: TQuat): TQuat;
    class operator Multiply(const q: TQuat; const v: TVec3f): TVec3f;
  {$ENDIF}
    function Invert: TQuat; inline;
    function Lerp(const q: TQuat; t: Single): TQuat;
    function Dot(const q: TQuat): Single; inline;
    function Normal: TQuat;
    function Euler: TVec3f;
  end;

  TDualQuat = {$IFDEF FPC} object {$ELSE} record {$ENDIF}
  private
    function GetPos: TVec3f;	// https://gist.github.com/XProger/da9a74ae8b37905b421a
  public
    Real, Dual : TQuat;
  {$IFNDEF FPC}
    class operator Multiply(const dq1, dq2: TDualQuat): TDualQuat;
  {$ENDIF}
    function Lerp(const dq: TDualQuat; t: Single): TDualQuat;
    property Pos: TVec3f read GetPos;
    property Rot: TQuat read Real;
  end;
  
{$IFDEF FPC}
// TQuat
  operator = (const q1, q2: TQuat): Boolean;
  operator + (const q1, q2: TQuat): TQuat;
  operator - (const q1, q2: TQuat): TQuat;
  operator * (const q: TQuat; x: Single): TQuat;
  operator * (const q1, q2: TQuat): TQuat;
  operator * (const q: TQuat; const v: TVec3f): TVec3f;
// TDualQuat
  operator * (const dq1, dq2: TDualQuat): TDualQuat;
{$ENDIF}

{$REGION 'TQuat'}
{$IFDEF FPC}operator = {$ELSE}class operator TQuat.Equal{$ENDIF}
  (const q1, q2: TQuat): Boolean;
begin
  Result := (abs(q1.x - q2.x) <= EPS) and
            (abs(q1.y - q2.y) <= EPS) and
            (abs(q1.z - q2.z) <= EPS) and
            (abs(q1.w - q2.w) <= EPS);
end;

{$IFDEF FPC}operator + {$ELSE}class operator TQuat.Add{$ENDIF}
  (const q1, q2: TQuat): TQuat;
begin
  Result.x := q1.x + q2.x;
  Result.y := q1.y + q2.y;
  Result.z := q1.z + q2.z;
  Result.w := q1.w + q2.w;
end;

{$IFDEF FPC}operator - {$ELSE}class operator TQuat.Subtract{$ENDIF}
  (const q1, q2: TQuat): TQuat;
begin
  Result.x := q1.x - q2.x;
  Result.y := q1.y - q2.y;
  Result.z := q1.z - q2.z;
  Result.w := q1.w - q2.w;
end;

{$IFDEF FPC}operator * {$ELSE}class operator TQuat.Multiply{$ENDIF}
  (const q: TQuat; x: Single): TQuat;
begin
  Result.x := q.x * x;
  Result.y := q.y * x;
  Result.z := q.z * x;
  Result.w := q.w * x;
end;

{$IFDEF FPC}operator * {$ELSE}class operator TQuat.Multiply{$ENDIF}
  (const q1, q2: TQuat): TQuat;
begin
  Result.x := q1.w * q2.x + q1.x * q2.w + q1.y * q2.z - q1.z * q2.y;
  Result.y := q1.w * q2.y + q1.y * q2.w + q1.z * q2.x - q1.x * q2.z;
  Result.z := q1.w * q2.z + q1.z * q2.w + q1.x * q2.y - q1.y * q2.x;
  Result.w := q1.w * q2.w - q1.x * q2.x - q1.y * q2.y - q1.z * q2.z;
end;

{$IFDEF FPC}operator * {$ELSE}class operator TQuat.Multiply{$ENDIF}
  (const q: TQuat; const v: TVec3f): TVec3f;
begin
  with q * Quat(v.x, v.y, v.z, 0) * q.Invert do
    Result := Vec3f(x, y, z);
end;

function TQuat.Invert: TQuat;
begin
  Result := Quat(-x, -y, -z, w);
end;

function TQuat.Lerp(const q: TQuat; t: Single): TQuat;
begin
  if Dot(q) < 0 then
    Result := Self - (q + Self) * t
  else
    Result := Self + (q - Self) * t;
end;

function TQuat.Dot(const q: TQuat): Single;
begin
  Result := x * q.x + y * q.y + z * q.z + w * q.w;
end;

function TQuat.Normal: TQuat;
var
  Len : Single;
begin
  Len := sqrt(sqr(x) + sqr(y) + sqr(z) + sqr(w));
  if Len > 0 then
  begin
    Len := 1 / Len;
    Result.x := x * Len;
    Result.y := y * Len;
    Result.z := z * Len;
    Result.w := w * Len;
  end;
end;

function TQuat.Euler: TVec3f;
var
  D : Single;
begin
  D := 2 * x * z + y * w;
  if abs(D) > 1 - EPS then
  begin
    Result.x := 0;
    if D > 0 then
      Result.y := -pi * 0.5
    else
      Result.y :=  pi * 0.5;
    Result.z := ArcTan2(-2 * (y * z - w * x), 2 * (w * w + y * y) - 1);
  end else
  begin
    Result.x := -ArcTan2(2 * (y * z + w * x), 2 * (w * w + z * z) - 1);
    Result.y := ArcSin(-d);
    Result.z := -ArcTan2(2 * (x * y + w * z), 2 * (w * w + x * x) - 1);
  end;
end;
{$ENDREGION}

{$REGION 'TDualQuat'}
{$IFDEF FPC}operator * {$ELSE}class operator TDualQuat.Multiply{$ENDIF}
  (const dq1, dq2: TDualQuat): TDualQuat;
begin
  Result.Real := dq1.Real * dq2.Real;
  Result.Dual := dq1.Real * dq2.Dual + dq1.Dual * dq2.Real;
end;

function TDualQuat.Lerp(const dq: TDualQuat; t: Single): TDualQuat;
begin
  if Real.Dot(dq.Real) < 0 then
  begin
    Result.Real := Real - (dq.Real + Real) * t;
    Result.Dual := Dual - (dq.Dual + Dual) * t;
  end else
  begin
    Result.Real := Real + (dq.Real - Real) * t;
    Result.Dual := Dual + (dq.Dual - Dual) * t;
  end;
end;

function TDualQuat.GetPos: TVec3f;
begin
  Result.x := (Dual.x * Real.w - Real.x * Dual.w + Real.y * Dual.z - Real.z * Dual.y) * 2;
  Result.y := (Dual.y * Real.w - Real.y * Dual.w + Real.z * Dual.x - Real.x * Dual.z) * 2;
  Result.z := (Dual.z * Real.w - Real.z * Dual.w + Real.x * Dual.y - Real.y * Dual.x) * 2;
end;
{$ENDREGION}