test/testpathfind.pas - master - Valhl88/pas_world

test/testpathfind.pas
591 строка16 KB
Vano
28 фев 2026, 17:31
28 фев 2026, 17:315fb9b0a
unit testpathfind;

{$mode objfpc}{$H+}

interface

uses
  Classes, SysUtils, fpcunit, testutils, testregistry,
  pascollision_types, pascollision_simulation, pascollision_pathfinding;

type

  TPathFindTests = class(TTestCase)
  private
    function MakeWorld: TCollisionWorld;
    function MakeWorldWithPlane: TCollisionWorld;
    function MakeStaticSphere(const pos: TVector3; radius: Double): TRigidBody;
    function MakeStaticBox(const pos: TVector3; halfX, halfZ: Double): TRigidBody;
    function MakeStaticCapsule(const pos: TVector3; radius, height: Double): TRigidBody;
    function MakeWorldWithHeightmapRidge(const sizeX, sizeZ, ridgeHeight: Double): TCollisionWorld;
  published
    procedure TestPathfinderNilWorld;
    procedure TestPathfinderEmptyWorldStraightPath;
    procedure TestPathfinderStartEqualsGoal;
    procedure TestPathfinderGetHeightAtNoWorld;
    procedure TestPathfinderGetHeightAtWithPlane;
    procedure TestPathfinderStaticSphereObstacle;
    procedure TestPathfinderStaticBoxObstacle;
    procedure TestPathfinderStaticCapsuleObstacle;
    procedure TestPathfinderWallOfSpheresObstacles;
    procedure TestPathfinderStartBlockedReturnsFalse;
    procedure TestPathfinderGoalBlockedReturnsFalse;
    procedure TestPathfinderWaypointsOrderAndBounds;
    procedure TestPathfinderPathGoesAroundObstacle;
    procedure TestPathfinderSetWorldBoundsCellSize;
    procedure TestPathfinderNoPathWhenFullyBlocked;
    procedure TestPathfinderHillyTerrainPathFollowsSurface;
    procedure TestPathfinderHillyTerrainNoPathThroughGround;
  end;

implementation

function TPathFindTests.MakeWorld: TCollisionWorld;
begin
  Result := TCollisionWorld.Create;
end;

function TPathFindTests.MakeWorldWithPlane: TCollisionWorld;
var
  t: TTerrain;
begin
  Result := TCollisionWorld.Create;
  t.Name := 'plane';
  t.Position := Vector3(0, 0, 0);
  t.Rotation := QuatIdentity;
  t.Scale := Vector3(1, 1, 1);
  t.TerrainType := ttPlane;
  t.Size := Vector3(100, 0, 100);
  t.HeightPoints := nil;
  Result.AddTerrain(t);
end;

function TPathFindTests.MakeStaticSphere(const pos: TVector3; radius: Double): TRigidBody;
begin
  Result.Name := 'sphere';
  Result.Dynamic := False;
  Result.UseGravity := False;
  Result.Position := pos;
  Result.Rotation := QuatIdentity;
  Result.Scale := Vector3(1, 1, 1);
  Result.Dimensions := Vector3(radius * 2, radius * 2, radius * 2);
  Result.ColliderType := ctSphere;
  Result.Height := 0;
  Result.Radius := radius;
  Result.SizeScale := 1;
  Result.Velocity := Vector3(0, 0, 0);
  Result.AngularVelocity := Vector3(0, 0, 0);
end;

function TPathFindTests.MakeStaticBox(const pos: TVector3; halfX, halfZ: Double): TRigidBody;
begin
  Result.Name := 'box';
  Result.Dynamic := False;
  Result.UseGravity := False;
  Result.Position := pos;
  Result.Rotation := QuatIdentity;
  Result.Scale := Vector3(1, 1, 1);
  Result.Dimensions := Vector3(halfX * 2, 1, halfZ * 2);
  Result.ColliderType := ctBox;
  Result.Height := 0;
  Result.Radius := 0;
  Result.SizeScale := 1;
  Result.Velocity := Vector3(0, 0, 0);
  Result.AngularVelocity := Vector3(0, 0, 0);
end;

function TPathFindTests.MakeStaticCapsule(const pos: TVector3; radius, height: Double): TRigidBody;
begin
  Result.Name := 'capsule';
  Result.Dynamic := False;
  Result.UseGravity := False;
  Result.Position := pos;
  Result.Rotation := QuatIdentity;
  Result.Scale := Vector3(1, 1, 1);
  Result.Dimensions := Vector3(radius * 2, height, radius * 2);
  Result.ColliderType := ctCapsule;
  Result.Height := height;
  Result.Radius := radius;
  Result.SizeScale := 1;
  Result.Velocity := Vector3(0, 0, 0);
  Result.AngularVelocity := Vector3(0, 0, 0);
end;

function TPathFindTests.MakeWorldWithHeightmapRidge(const sizeX, sizeZ, ridgeHeight: Double): TCollisionWorld;
var
  t: TTerrain;
  i, j, nx, nz, idx: Integer;
begin
  Result := TCollisionWorld.Create;
  nx := 5;
  nz := 5;
  SetLength(t.HeightPoints, nx * nz);
  for j := 0 to nz - 1 do
    for i := 0 to nx - 1 do
    begin
      idx := j * nx + i;
      t.HeightPoints[idx].x := i * (sizeX / (nx - 1));
      t.HeightPoints[idx].y := 0;
      t.HeightPoints[idx].z := j * (sizeZ / (nz - 1));
    end;
  t.HeightPoints[2 + 2 * nx].y := ridgeHeight;
  t.Name := 'ridge';
  t.Position := Vector3(0, 0, 0);
  t.Rotation := QuatIdentity;
  t.Scale := Vector3(1, 1, 1);
  t.TerrainType := ttHeightmap;
  t.Size := Vector3(sizeX, 0, sizeZ);
  Result.AddTerrain(t);
end;

procedure TPathFindTests.TestPathfinderNilWorld;
var
  pf: TPathfinder;
  wp: TVector3Array;
begin
  pf := TPathfinder.Create;
  try
    AssertTrue('FindPath with nil world must return False', not pf.FindPath(Vector3(0, 0, 0), Vector3(5, 0, 5), wp));
    AssertEquals('Waypoints must be empty', 0, Length(wp));
    AssertEquals('GetHeightAt without world must be 0', 0, pf.GetHeightAt(1, 1), 1e-6);
  finally
    pf.Free;
  end;
end;

procedure TPathFindTests.TestPathfinderEmptyWorldStraightPath;
var
  world: TCollisionWorld;
  pf: TPathfinder;
  wp: TVector3Array;
  start, goal: TVector3;
begin
  world := MakeWorldWithPlane;
  pf := TPathfinder.Create;
  try
    pf.World := world;
    pf.SetBounds(0, 20, 0, 20);
    pf.CellSize := 0.5;
    start := Vector3(1, 0, 1);
    goal := Vector3(19, 0, 19);
    AssertTrue('Path in empty world must be found', pf.FindPath(start, goal, wp));
    AssertTrue('Waypoints must contain at least 2 points', Length(wp) >= 2);
    AssertTrue('First waypoint near start', (Abs(wp[0].x - start.x) < 1) and (Abs(wp[0].z - start.z) < 1));
    AssertTrue('Last waypoint near goal', (Abs(wp[High(wp)].x - goal.x) < 1) and (Abs(wp[High(wp)].z - goal.z) < 1));
  finally
    pf.Free;
    world.Free;
  end;
end;

procedure TPathFindTests.TestPathfinderStartEqualsGoal;
var
  world: TCollisionWorld;
  pf: TPathfinder;
  wp: TVector3Array;
  p: TVector3;
begin
  world := MakeWorldWithPlane;
  pf := TPathfinder.Create;
  try
    pf.World := world;
    pf.SetBounds(0, 20, 0, 20);
    pf.CellSize := 0.5;
    p := Vector3(10, 0, 10);
    AssertTrue('Path start=goal must be found', pf.FindPath(p, p, wp));
    AssertTrue('Waypoints for start=goal', Length(wp) >= 1);
  finally
    pf.Free;
    world.Free;
  end;
end;

procedure TPathFindTests.TestPathfinderGetHeightAtNoWorld;
var
  pf: TPathfinder;
begin
  pf := TPathfinder.Create;
  try
    AssertEquals('GetHeightAt without world', 0, pf.GetHeightAt(0, 0), 1e-6);
  finally
    pf.Free;
  end;
end;

procedure TPathFindTests.TestPathfinderGetHeightAtWithPlane;
var
  world: TCollisionWorld;
  pf: TPathfinder;
begin
  world := MakeWorldWithPlane;
  pf := TPathfinder.Create;
  try
    pf.World := world;
    pf.SetBounds(0, 10, 0, 10);
    AssertEquals('GetHeightAt on plane at y=0', 0, pf.GetHeightAt(5, 5), 1e-6);
  finally
    pf.Free;
    world.Free;
  end;
end;

procedure TPathFindTests.TestPathfinderStaticSphereObstacle;
var
  world: TCollisionWorld;
  pf: TPathfinder;
  wp: TVector3Array;
  start, goal: TVector3;
  obstacle: TRigidBody;
  i: Integer;
  distToObstacle: Double;
begin
  world := MakeWorldWithPlane;
  obstacle := MakeStaticSphere(Vector3(10, 0, 10), 2);
  world.AddBody(obstacle);
  pf := TPathfinder.Create;
  try
    pf.World := world;
    pf.SetBounds(0, 20, 0, 20);
    pf.CellSize := 0.4;
    pf.ObstacleMargin := 0.2;
    start := Vector3(2, 0, 10);
    goal := Vector3(18, 0, 10);
    AssertTrue('Path must be found around sphere', pf.FindPath(start, goal, wp));
    AssertTrue('Path must have several waypoints', Length(wp) >= 2);
    for i := 0 to High(wp) do
    begin
      distToObstacle := Sqrt(Sqr(wp[i].x - 10) + Sqr(wp[i].z - 10));
      AssertTrue(Format('Waypoint %d must not be inside obstacle (dist=%f)', [i, distToObstacle]), distToObstacle >= 2 - 0.1);
    end;
  finally
    pf.Free;
    world.Free;
  end;
end;

procedure TPathFindTests.TestPathfinderStaticBoxObstacle;
var
  world: TCollisionWorld;
  pf: TPathfinder;
  wp: TVector3Array;
  start, goal: TVector3;
  obstacle: TRigidBody;
  i: Integer;
  ox, oz: Double;
begin
  world := MakeWorldWithPlane;
  obstacle := MakeStaticBox(Vector3(10, 0, 10), 1.5, 1.5);
  world.AddBody(obstacle);
  pf := TPathfinder.Create;
  try
    pf.World := world;
    pf.SetBounds(0, 20, 0, 20);
    pf.CellSize := 0.4;
    pf.ObstacleMargin := 0.2;
    start := Vector3(2, 0, 10);
    goal := Vector3(18, 0, 10);
    AssertTrue('Path must be found around box', pf.FindPath(start, goal, wp));
    AssertTrue('Path must have waypoints', Length(wp) >= 2);
    for i := 0 to High(wp) do
    begin
      ox := Abs(wp[i].x - 10);
      oz := Abs(wp[i].z - 10);
      AssertTrue(Format('Waypoint %d must not be inside box', [i]), (ox > 1.5 + 0.2) or (oz > 1.5 + 0.2));
    end;
  finally
    pf.Free;
    world.Free;
  end;
end;

procedure TPathFindTests.TestPathfinderStaticCapsuleObstacle;
var
  world: TCollisionWorld;
  pf: TPathfinder;
  wp: TVector3Array;
  start, goal: TVector3;
  obstacle: TRigidBody;
  i: Integer;
  distToCenter: Double;
begin
  world := MakeWorldWithPlane;
  obstacle := MakeStaticCapsule(Vector3(10, 0, 10), 1.2, 2);
  world.AddBody(obstacle);
  pf := TPathfinder.Create;
  try
    pf.World := world;
    pf.SetBounds(0, 20, 0, 20);
    pf.CellSize := 0.4;
    pf.ObstacleMargin := 0.2;
    start := Vector3(2, 0, 10);
    goal := Vector3(18, 0, 10);
    AssertTrue('Path must be found around capsule', pf.FindPath(start, goal, wp));
    AssertTrue('Path must have waypoints', Length(wp) >= 2);
    for i := 0 to High(wp) do
    begin
      distToCenter := Sqrt(Sqr(wp[i].x - 10) + Sqr(wp[i].z - 10));
      AssertTrue(Format('Waypoint %d must not intersect capsule', [i]), distToCenter >= 1.2 - 0.1);
    end;
  finally
    pf.Free;
    world.Free;
  end;
end;

procedure TPathFindTests.TestPathfinderWallOfSpheresObstacles;
var
  world: TCollisionWorld;
  pf: TPathfinder;
  wp: TVector3Array;
  start, goal: TVector3;
  i: Integer;
  obs: TRigidBody;
begin
  world := MakeWorldWithPlane;
  for i := 0 to 4 do
  begin
    obs := MakeStaticSphere(Vector3(10, 0, 4 + i * 2.5), 0.8);
    world.AddBody(obs);
  end;
  pf := TPathfinder.Create;
  try
    pf.World := world;
    pf.SetBounds(0, 20, 0, 20);
    pf.CellSize := 0.3;
    pf.ObstacleMargin := 0.2;
    start := Vector3(5, 0, 10);
    goal := Vector3(15, 0, 10);
    AssertTrue('Path must be found around wall of spheres', pf.FindPath(start, goal, wp));
    AssertTrue('Path must go around (many waypoints)', Length(wp) >= 3);
  finally
    pf.Free;
    world.Free;
  end;
end;

procedure TPathFindTests.TestPathfinderStartBlockedReturnsFalse;
var
  world: TCollisionWorld;
  pf: TPathfinder;
  wp: TVector3Array;
  obstacle: TRigidBody;
begin
  world := MakeWorldWithPlane;
  obstacle := MakeStaticSphere(Vector3(2, 0, 2), 1.5);
  world.AddBody(obstacle);
  pf := TPathfinder.Create;
  try
    pf.World := world;
    pf.SetBounds(0, 20, 0, 20);
    pf.CellSize := 0.5;
    AssertFalse('FindPath must return False when start is blocked', pf.FindPath(Vector3(2, 0, 2), Vector3(15, 0, 15), wp));
    AssertEquals('Waypoints must be empty', 0, Length(wp));
  finally
    pf.Free;
    world.Free;
  end;
end;

procedure TPathFindTests.TestPathfinderGoalBlockedReturnsFalse;
var
  world: TCollisionWorld;
  pf: TPathfinder;
  wp: TVector3Array;
  obstacle: TRigidBody;
begin
  world := MakeWorldWithPlane;
  obstacle := MakeStaticSphere(Vector3(18, 0, 18), 1.5);
  world.AddBody(obstacle);
  pf := TPathfinder.Create;
  try
    pf.World := world;
    pf.SetBounds(0, 20, 0, 20);
    pf.CellSize := 0.5;
    AssertFalse('FindPath must return False when goal is blocked', pf.FindPath(Vector3(2, 0, 2), Vector3(18, 0, 18), wp));
    AssertEquals('Waypoints must be empty', 0, Length(wp));
  finally
    pf.Free;
    world.Free;
  end;
end;

procedure TPathFindTests.TestPathfinderWaypointsOrderAndBounds;
var
  world: TCollisionWorld;
  pf: TPathfinder;
  wp: TVector3Array;
  start, goal: TVector3;
  i: Integer;
  prevDist, currDist: Double;
begin
  world := MakeWorldWithPlane;
  pf := TPathfinder.Create;
  try
    pf.World := world;
    pf.SetBounds(0, 20, 0, 20);
    pf.CellSize := 0.5;
    start := Vector3(1, 0, 1);
    goal := Vector3(19, 0, 19);
    AssertTrue(pf.FindPath(start, goal, wp));
    AssertTrue(Length(wp) >= 2);
    prevDist := 0;
    for i := 0 to High(wp) do
    begin
      currDist := Sqrt(Sqr(wp[i].x - goal.x) + Sqr(wp[i].z - goal.z));
      AssertTrue(Format('Waypoint %d must be within bounds', [i]), (wp[i].x >= -0.1) and (wp[i].x <= 20.1) and (wp[i].z >= -0.1) and (wp[i].z <= 20.1));
      if i > 0 then
        AssertTrue(Format('Waypoints should progress toward goal (or stay)', [i]), currDist <= prevDist + 2);
      prevDist := currDist;
    end;
  finally
    pf.Free;
    world.Free;
  end;
end;

procedure TPathFindTests.TestPathfinderPathGoesAroundObstacle;
var
  world: TCollisionWorld;
  pf: TPathfinder;
  wp: TVector3Array;
  start, goal: TVector3;
  obstacle: TRigidBody;
  i: Integer;
  dist: Double;
begin
  world := MakeWorldWithPlane;
  obstacle := MakeStaticSphere(Vector3(10, 0, 10), 2.5);
  world.AddBody(obstacle);
  pf := TPathfinder.Create;
  try
    pf.World := world;
    pf.SetBounds(0, 20, 0, 20);
    pf.CellSize := 0.4;
    pf.ObstacleMargin := 0.3;
    start := Vector3(5, 0, 5);
    goal := Vector3(15, 0, 15);
    AssertTrue('Path must exist', pf.FindPath(start, goal, wp));
    for i := 0 to High(wp) do
    begin
      dist := Sqrt(Sqr(wp[i].x - 10) + Sqr(wp[i].z - 10));
      AssertTrue(Format('Waypoint %d must not be inside obstacle radius 2.5', [i]), dist >= 2.5 - 0.2);
    end;
  finally
    pf.Free;
    world.Free;
  end;
end;

procedure TPathFindTests.TestPathfinderSetWorldBoundsCellSize;
var
  world: TCollisionWorld;
  pf: TPathfinder;
  wp: TVector3Array;
begin
  world := MakeWorldWithPlane;
  pf := TPathfinder.Create;
  try
    AssertTrue(pf.World = nil);
    pf.World := world;
    AssertTrue(pf.World = world);
    pf.SetBounds(0, 10, 0, 10);
    pf.CellSize := 0.25;
    pf.ObstacleMargin := 0.1;
    pf.World := world;
    AssertTrue(pf.FindPath(Vector3(1, 0, 1), Vector3(9, 0, 9), wp));
    AssertTrue(Length(wp) >= 2);
  finally
    pf.Free;
    world.Free;
  end;
end;

procedure TPathFindTests.TestPathfinderNoPathWhenFullyBlocked;
var
  world: TCollisionWorld;
  pf: TPathfinder;
  wp: TVector3Array;
  i: Integer;
  obs: TRigidBody;
begin
  world := MakeWorldWithPlane;
  for i := 0 to 9 do
  begin
    obs := MakeStaticSphere(Vector3(10, 0, 2 + i * 2), 1.2);
    world.AddBody(obs);
  end;
  pf := TPathfinder.Create;
  try
    pf.World := world;
    pf.SetBounds(0, 20, 0, 20);
    pf.CellSize := 0.5;
    pf.ObstacleMargin := 0.2;
    AssertFalse('No path when corridor is fully blocked', pf.FindPath(Vector3(5, 0, 10), Vector3(15, 0, 10), wp));
    AssertEquals(0, Length(wp));
  finally
    pf.Free;
    world.Free;
  end;
end;

procedure TPathFindTests.TestPathfinderHillyTerrainPathFollowsSurface;
var
  world: TCollisionWorld;
  pf: TPathfinder;
  wp: TVector3Array;
  i: Integer;
  terrainH, waypointH: Double;
  tol: Double;
begin
  world := MakeWorldWithHeightmapRidge(20, 20, 6);
  pf := TPathfinder.Create;
  try
    pf.World := world;
    pf.SetBounds(0, 20, 0, 20);
    pf.CellSize := 0.5;
    AssertTrue('Path on hilly terrain must be found', pf.FindPath(Vector3(2, 0, 2), Vector3(18, 0, 18), wp));
    AssertTrue('Path must have waypoints', Length(wp) >= 2);
    tol := 0.15;
    for i := 0 to High(wp) do
    begin
      terrainH := pf.GetHeightAt(wp[i].x, wp[i].z);
      waypointH := wp[i].y;
      AssertTrue(Format('Waypoint %d must follow surface: Y=%.3f should match terrain %.3f at (%.2f, %.2f)', [i, waypointH, terrainH, wp[i].x, wp[i].z]), Abs(waypointH - terrainH) <= tol);
    end;
  finally
    pf.Free;
    world.Free;
  end;
end;

procedure TPathFindTests.TestPathfinderHillyTerrainNoPathThroughGround;
var
  world: TCollisionWorld;
  pf: TPathfinder;
  wp: TVector3Array;
  i: Integer;
  terrainH: Double;
begin
  world := MakeWorldWithHeightmapRidge(20, 20, 8);
  pf := TPathfinder.Create;
  try
    pf.World := world;
    pf.SetBounds(0, 20, 0, 20);
    pf.CellSize := 0.5;
    AssertTrue('Path over ridge must be found', pf.FindPath(Vector3(2, 0, 2), Vector3(18, 0, 18), wp));
    for i := 0 to High(wp) do
    begin
      terrainH := pf.GetHeightAt(wp[i].x, wp[i].z);
      AssertTrue(Format('Waypoint %d must not be below terrain (no going through): Y=%.3f >= terrain %.3f - 0.01', [i, wp[i].y, terrainH]), wp[i].y >= terrainH - 0.01);
    end;
  finally
    pf.Free;
    world.Free;
  end;
end;

initialization

  RegisterTest(TPathFindTests);

end.