example/PathFind.lpr - master - Valhl88/pas_world

example/PathFind.lpr
69 строк2 KB
Vano
28 фев 2026, 17:31
28 фев 2026, 17:315fb9b0a
program PathFind;

{$mode objfpc}{$H+}

uses
  SysUtils,
  pascollision_types,
  pascollision_json,
  pascollision_simulation,
  pascollision_pathfinding;

var
  dataPath: string;
  data: TCollisionData;
  world: TCollisionWorld;
  pathfinder: TPathfinder;
  startPt, goalPt: TVector3;
  waypoints: array of TVector3;
  i: Integer;
  dataDir: string;
begin
  dataDir := ExtractFilePath(ParamStr(0));
  if dataDir <> '' then
    dataPath := ExpandFileName(dataDir + '../data/collision-data.json')
  else
    dataPath := 'data/collision-data.json';
  if ParamCount > 0 then
    dataPath := ParamStr(1);
  if not FileExists(dataPath) then
  begin
    WriteLn('Usage: ', ExtractFileName(ParamStr(0)), ' [path-to-collision-data.json]');
    WriteLn('Default path: ', dataPath);
    WriteLn('File not found: ', dataPath);
    Halt(1);
  end;

  WriteLn('Loading: ', dataPath);
  data := LoadCollisionDataFromFile(dataPath);
  world := TCollisionWorld.Create;
  pathfinder := TPathfinder.Create;
  try
    world.LoadFromData(data);
    pathfinder.World := world;
    pathfinder.SetBounds(0, 20, 0, 20);
    pathfinder.CellSize := 0.5;
    pathfinder.ObstacleMargin := 0.3;

    startPt := Vector3(2, 0, 2);
    goalPt := Vector3(18, 0, 18);
    WriteLn;
    WriteLn('FindPath from (', startPt.x:0:2, ', ', startPt.y:0:2, ', ', startPt.z:0:2, ')');
    WriteLn('         to   (', goalPt.x:0:2, ', ', goalPt.y:0:2, ', ', goalPt.z:0:2, ')');

    if pathfinder.FindPath(startPt, goalPt, waypoints) then
    begin
      WriteLn('Path found: ', Length(waypoints), ' waypoints');
      for i := 0 to High(waypoints) do
        WriteLn('  [', i, '] (', waypoints[i].x:8:3, ', ', waypoints[i].y:8:3, ', ', waypoints[i].z:8:3, ')');
    end
    else
      WriteLn('Path not found (blocked or out of bounds).');

    WriteLn;
    WriteLn('Done.');
  finally
    pathfinder.Free;
    world.Free;
  end;
end.