Free pascal Oriented bounding box collisions for RayLib

collision.pas

//
// Oriented bounding box collisions
//
// 2023, Original code in c langauge by Jonathan Tainer
// 2023, Pascal translation Vadim Gunko and Jarrod Davis
//
unit collider;

{$mode ObjFPC}{$H+}

interface

uses
  raylib, raymath, math;

const
  COLLIDER_VERTEX_COUNT = 8;
  COLLIDER_NORMAL_COUNT = 3;

type
  PCollider = ^TCollider;
  TCollider = record
  //En: Vertex positions in local (model) space
  //Ru: Положения вершин в локальном (модельном) пространстве
  vertLocal: array[0..COLLIDER_VERTEX_COUNT] of TVector3;

  //En: Vertex positions in global (world) space
  //Ru: Положение вершин в глобальном (мировом) пространстве
  vertGlobal: array[0..COLLIDER_VERTEX_COUNT] of TVector3;

  //En: Rotation about origin in local space
  //Ru: Вращение вокруг начала координат в локальном пространстве
  matRotate: TMatrix;

  //En: Translation applied after rotation
  //Ru: Перевод, примененный после поворота
  matTranslate: TMatrix;
  end;

  //En: Calculate verts, use identity matrix by default
  //Ru: Вычислите вершины, используйте идентификационную матрицу по умолчанию
  function CreateCollider(min, max: TVector3): TCollider;

  //En: Rotate object starting from origin
  //Ru: Повернуть объект, начиная с начала координат
  procedure SetColliderRotation(col: TCollider; axis: TVector3; ang: Single);

  //En: Rotate objects starting from current position
  //Ru: Поворачивать объекты, начиная с текущего положения
  procedure AddColliderRotation(col: TCollider; axis: TVector3; ang: Single);

  //En: Translate object starting from origin
  //Ru: Перевести объект, начиная с исходного
  procedure SetColliderTranslation(col: TCollider; pos: TVector3);

  //En: Translate object starting from current position
  //Ru: Перевести объект, начиная с текущего положения
  procedure AddColliderTranslation(col: TCollider; pos: TVector3);

  //En: Get the transformation matrix
  //Ru: Получить матрицу трансформации
  function GetColliderTransform(col: TCollider): TMatrix;

  //En: Use separating axis theorem to detect overlap
  //Ru: Используйте теорему о разделяющей оси для обнаружения перекрытия
  function TestColliderPair(a, b: TCollider): Boolean;

  //En: Find translation needed to resolve a collision
  //Ru: Найти перевод, необходимый для разрешения коллизии
  function GetCollisionCorrection(a, b: TCollider): TVector3;

implementation

//*******************************************************************
// Various collider transformations. Physics engine should call these
// to apply movement to each collider.
//*******************************************************************

procedure UpdateColliderGlobalVerts(var col: TCollider);
var
  matTemp: TMatrix;
  i: Integer;
begin
  matTemp := MatrixMultiply(col.matRotate, col.matTranslate);
  for i := 0 to COLLIDER_VERTEX_COUNT - 1 do
    col.vertGlobal[i] := Vector3Transform(col.vertLocal[i], matTemp);
end;

function CreateCollider(min, max: TVector3): TCollider;
begin
  Result := Default (TCollider);

  Result.vertLocal[0] := Vector3Create(min.x, min.y, min.z);
  Result.vertLocal[1] := Vector3Create(min.x, min.y, max.z);
  Result.vertLocal[2] := Vector3Create(min.x, max.y, min.z);
  Result.vertLocal[3] := Vector3Create(min.x, max.y, max.z);
  Result.vertLocal[4] := Vector3Create(max.x, min.y, min.z);
  Result.vertLocal[5] := Vector3Create(max.x, min.y, max.z);
  Result.vertLocal[6] := Vector3Create(max.x, max.y, min.z);
  Result.vertLocal[7] := Vector3Create(max.x, max.y, max.z);

  Result.matRotate := MatrixIdentity;
  Result.matTranslate := MatrixIdentity;
  UpdateColliderGlobalVerts(Result);
end;

procedure SetColliderRotation(col: TCollider; axis: TVector3; ang: Single);
begin
  col.matRotate := MatrixRotate(axis, ang);
  UpdateColliderGlobalVerts(col);
end;

procedure AddColliderRotation(col: TCollider; axis: TVector3; ang: Single);
var
  matTemp: TMatrix;
begin
  matTemp := MatrixRotate(axis, ang);
  col.matRotate := MatrixMultiply(col.matRotate, matTemp);
  UpdateColliderGlobalVerts(col);
end;

procedure SetColliderTranslation(col: TCollider; pos: TVector3);
begin
  col.matTranslate := MatrixTranslate(pos.x, pos.y, pos.z);
  UpdateColliderGlobalVerts(col);
end;

procedure AddColliderTranslation(col: TCollider; pos: TVector3);
var
  matTemp: TMatrix;
begin
  matTemp := MatrixTranslate(pos.x, pos.y, pos.z);
  col.matTranslate := MatrixMultiply(col.matTranslate, matTemp);
  UpdateColliderGlobalVerts(col);
end;

function GetColliderTransform(col: TCollider): TMatrix;
begin
  Result := MatrixMultiply(col.matRotate, col.matTranslate);
end;

procedure GetCollisionVectors(a, b: TCollider; vec: array of TVector3);
var
  x, y, z: TVector3;
  i, j, k: Integer;
begin
  x := Vector3Create(1.0, 0.0, 0.0);
  y := Vector3Create(0.0, 1.0, 0.0);
  z := Vector3Create(0.0, 0.0, 1.0);

  vec[0] := Vector3Transform(x, a.matRotate);
  vec[1] := Vector3Transform(y, a.matRotate);
  vec[2] := Vector3Transform(z, a.matRotate);

  vec[3] := Vector3Transform(x, b.matRotate);
  vec[4] := Vector3Transform(y, b.matRotate);
  vec[5] := Vector3Transform(z, b.matRotate);

  i := 6;
  for j := 0 to 2 do
  begin
    for k := 3 to 5 do
    begin
      if Vector3Equals(vec[j], vec[k]) > 0 then
        vec[i] := x
      else
        vec[i] := Vector3Normalize(Vector3CrossProduct(vec[j], vec[k]));
      Inc(i);
    end;
  end;
end;

function GetColliderProjectionBounds(col: TCollider; vec: TVector3): TVector2;
var
  bounds: TVector2;
  proj: Single;
  i: Integer;
begin
  bounds := Vector2Zero;
  proj := Vector3DotProduct(col.vertGlobal[0], vec);
  bounds.x := proj;
  bounds.y := proj;
  for i := 1 to COLLIDER_VERTEX_COUNT - 1 do
  begin
    proj := Vector3DotProduct(col.vertGlobal[i], vec);
    bounds.x := min(bounds.x, proj);
    bounds.y := max(bounds.y, proj);
  end;
  Result := bounds;
end;

function BoundsOverlap(a, b: TVector2): Boolean;
begin
  Result := (a.x <= b.y) and (b.x <= a.y);
end;

function GetOverlap(a, b: TVector2): Single;
begin
  if a.x > b.y then
    Result := 0.0
  else if b.x > a.y then
    Result := 0.0
  else if a.x > b.x then
    Result := b.y - a.x
  else
    Result := b.x - a.y;
end;

function TestColliderPair(a, b: TCollider): Boolean;
var
  testVec: array [0 .. 14] of TVector3;
  i: Integer;
  apro, bpro: TVector2;
begin
  GetCollisionVectors(a, b, testVec);
  for i := 0 to 14 do
  begin
    apro := GetColliderProjectionBounds(a, testVec[i]);
    bpro := GetColliderProjectionBounds(b, testVec[i]);
    if not BoundsOverlap(apro, bpro) then
      Exit(False);
  end;
  Result := True;
end;

function GetCollisionCorrection(a, b: TCollider): TVector3;
var
  overlapMin, overlap: Single;
  overlapDir: TVector3;
  testVec: array [0..14] of TVector3;
  i: Integer;
  apro, bpro: TVector2;
begin
  overlapMin := 100.0;
  overlapDir := Vector3Zero;
  GetCollisionVectors(a, b, testVec);
  for i := 0 to 14 do
  begin
    apro := GetColliderProjectionBounds(a, testVec[i]);
    bpro := GetColliderProjectionBounds(b, testVec[i]);

    overlap := GetOverlap(apro, bpro);
    if overlap = 0.0 then
      Exit(Vector3Zero);
    if Abs(overlap) < Abs(overlapMin) then
    begin
      overlapMin := overlap;
      overlapDir := testVec[i];
    end;
  end;

  Result := Vector3Scale(overlapDir, overlapMin);
end;

end.