EscapistsReintroduced/tests/test_convoy_road_plan.py

"""Execute installed road geometry methods; native road/terrain queries remain engine work."""
import os,re
from policy_harness import ROOT, body, run
from pathlib import Path
p=Path(os.environ.get('ERR_CONVOY_ROAD_PLAN',str(ROOT/'addons/core/Scripts/Game/ERR/10_AI/ERR_ConvoyRoadPlan.c')))
code='\n'.join(body(p,s) for s in ('static bool Sample(', 'static bool CandidateStart(', 'static bool FromPolyline(', 'static bool Prefer('))
code=re.sub(r'//[^\n]*','',code)
code=code.replace('out array<vector>','Array<vector>&').replace('array<vector>','Array<vector>')
code=code.replace('out vector','vector&').replace('out float','float&').replace('out ERR_ForceRoute','ERR_ForceRoute*&')
code=re.sub(r'ERR_ForceRoute (\w+)',r'ERR_ForceRoute* \1',code)
code=code.replace('foreach (','for (').replace('null','nullptr')
for cls in ('vector','ERR_ForceRules','Math'):code=code.replace(cls+'.',cls+'::')
for obj in ('candidate','route','bestRoute'):code=code.replace(obj+'.',obj+'->')
code=code.replace('float length, projected, nearest', 'float length=0, projected=0, nearest').replace('float traversed;', 'float traversed=0;').replace('float offset;', 'float offset=0;')
run(r'''
#include <cassert>
#include <cmath>
#include <algorithm>
#include <vector>
#include <limits>
struct vector{float x=0,y=0,z=0;float& operator[](int i){return i==0?x:i==1?y:z;}vector operator+(vector b)const{return{x+b.x,y+b.y,z+b.z};}vector operator-(vector b)const{return{x-b.x,y-b.y,z-b.z};}vector operator*(float n)const{return{x*n,y*n,z*n};}float Length()const{return std::sqrt(x*x+y*y+z*z);}void Normalize(){float n=Length();x/=n;y/=n;z/=n;}static float DistanceXZ(vector a,vector b){return std::hypot(a.x-b.x,a.z-b.z);}static vector Lerp(vector a,vector b,float n){return a+(b-a)*n;}static float Dot(vector a,vector b){return a.x*b.x+a.y*b.y+a.z*b.z;}};
vector Vector(float x,float y,float z){return{x,y,z};}
template<class T>struct Array:std::vector<T>{using std::vector<T>::vector;int Count()const{return this->size();}bool IsEmpty()const{return this->empty();}void Insert(T v){this->push_back(v);}explicit operator bool()const{return true;}};
namespace Math{float Clamp(float x,float a,float b){return std::clamp(x,a,b);}int Min(int a,int b){return std::min(a,b);}}
namespace ERR_ForceRules{bool Finite(float n){return std::isfinite(n);}bool FiniteVector(vector v){return Finite(v.x)&&Finite(v.y)&&Finite(v.z);}float Heading(vector a,vector b,float){return std::atan2(b.x-a.x,b.z-a.z)*180/3.141592653589793;}}
struct ERR_ForceRoute{Array<vector> points;bool sealed=false;float length=0;bool Add(vector v){if(sealed||points.Count()>=64||!ERR_ForceRules::FiniteVector(v))return false;points.Insert(v);return true;}bool Seal(){if(points.IsEmpty())return false;length=0;for(int i=1;i<points.Count();i++)length+=vector::DistanceXZ(points[i-1],points[i]);sealed=true;return true;}int Count(){return points.Count();}float Length(){return length;}vector At(int i){return points.at(i);}};
struct Plan{'''+code+r'''};
bool near(float a,float b){return std::abs(a-b)<.02;}
int main(){
 Array<vector> positions,angles;ERR_ForceRoute* route=nullptr;float proximity;vector pos,dir;
 assert(Plan::Sample({{0,0,0},{0,0,0},{100,0,0},{100,0,100}},150,pos,dir)&&near(pos.x,100)&&near(pos.z,50)&&near(dir.z,1));
 assert(!Plan::Sample({{0,0,0},{100,0,0}},101,pos,dir));assert(!Plan::Sample({{0,0,0},{100,0,0}},-1,pos,dir));
 assert(!Plan::Sample({{0,0,0},{100,0,0}},NAN,pos,dir));
 assert(Plan::FromPolyline({{0,10,0},{390,10,0}},{100,10,20},positions,angles,route,proximity));
 assert(positions.Count()==6&&angles.Count()==6&&route&&near(proximity,std::hypot(2.5f,20.0f)));assert(near(positions[0].x,190)&&near(positions[5].x,15));assert(near(route->Length(),200));
 for(int i=1;i<6;i++)assert(near(vector::DistanceXZ(positions[i-1],positions[i]),35));
 assert(Plan::FromPolyline({{0,10,0},{390,10,0}},{380,10,0},positions,angles,route,proximity));assert(near(positions[0].x,200)&&near(positions[5].x,375)&&near(angles[0].x,-90));
 assert(Plan::FromPolyline({{0,10,0},{200,10,0},{200,10,200},{500,10,200}},{10,10,0},positions,angles,route,proximity));
 assert(route->Count()==3&&near(route->At(1).x,200)&&near(route->At(1).z,0)&&near(route->At(2).z,200));
 assert(Plan::FromPolyline({{0,10,0},{0,10,0},{600,10,0}},{102.5,10,490},positions,angles,route,proximity));
 assert(!Plan::FromPolyline({{0,10,0},{600,10,0}},{30,10,500.1},positions,angles,route,proximity)&&route==nullptr);
 assert(!Plan::FromPolyline({{0,10,0},{389.9,10,0}},{0,10,0},positions,angles,route,proximity));
 assert(!Plan::FromPolyline({{0,10,0},{NAN,10,0}},{0,10,0},positions,angles,route,proximity));
 assert(!Plan::FromPolyline({{0,10,0},{0,10,0}},{0,10,0},positions,angles,route,proximity));
 Array<vector> dense;for(int i=0;i<=600;i++)dense.Insert({float(i),10,0});assert(!Plan::FromPolyline(dense,{0,10,0},positions,angles,route,proximity));
 Array<vector> tooMany;for(int i=0;i<1025;i++)tooMany.Insert({float(i),10,0});assert(!Plan::FromPolyline(tooMany,{0,10,0},positions,angles,route,proximity));
 // Tight hairpin folds the six intended starts onto each other: never accept it.
 assert(!Plan::FromPolyline({{0,10,0},{100,10,0},{100,10,1},{0,10,1},{0,10,2},{500,10,2}},{0,10,0},positions,angles,route,proximity));

 float start;
 assert(Plan::CandidateStart(1200,500,0,start)&&near(start,412.5));
 assert(Plan::CandidateStart(1200,500,1,start)&&near(start,447.5));
 assert(Plan::CandidateStart(1200,500,2,start)&&near(start,377.5));
 assert(!Plan::CandidateStart(390,100,1,start)&&!Plan::CandidateStart(390,100,2,start));
 assert(!Plan::CandidateStart(1200,500,33,start)&&!Plan::CandidateStart(1200,500,-1,start));
 assert(!Plan::CandidateStart(NAN,500,0,start)&&!Plan::CandidateStart(1200,NAN,0,start));
 // All six chosen poses, not merely some remote part of the same road, stay near seed.
 assert(!Plan::FromPolyline({{0,10,0},{2000,10,0}},{10,10,490},positions,angles,route,proximity));
 assert(Plan::FromPolyline({{0,10,0},{2000,10,0}},{500,10,0},positions,angles,route,proximity,1));
 assert(near(positions[0].x,622.5)&&near(positions[5].x,447.5)&&near(proximity,35));
 assert(Plan::FromPolyline({{0,10,0},{2000,10,0}},{500,10,0},positions,angles,route,proximity,2));
 assert(near(positions[0].x,552.5)&&near(positions[5].x,377.5)&&near(proximity,35));
 assert(!Plan::FromPolyline({{0,10,0},{2000,10,0}},{500,10,0},positions,angles,route,proximity,31));
 // Simulated native obstruction occupies initial lead around587.5; the first shifted
 // formation is clear. A failed initial candidate must not discard the native road.
 bool selected=false;int considered=0;Array<vector> first;
 for(int index=0;index<33;index++){
  if(!Plan::FromPolyline({{0,10,0},{2000,10,0}},{500,10,0},positions,angles,route,proximity,index))continue;
  considered++;bool clear=true;for(auto point:positions)if(std::abs(point.x-587.5)<10)clear=false;
  if(clear){selected=true;first=positions;break;}
 }
 assert(selected&&considered>1&&first.Count()==6);

 ERR_ForceRoute a,b;a.Add({0,0,0});a.Add({1,0,0});a.Seal();b.Add({0,0,0});b.Add({2,0,0});b.Seal();
 assert(Plan::Prefer(4,{{0,0,0}},&a,5,{{0,0,0}},&b));assert(!Plan::Prefer(6,{{0,0,0}},&a,5,{{0,0,0}},&b));
 assert(Plan::Prefer(5,{{0,0,0}},&a,5,{{1,0,0}},&b));assert(Plan::Prefer(5,{{0,0,0}},&a,5,{{0,0,0}},&b));assert(!Plan::Prefer(5,{{0,0,0}},&b,5,{{0,0,0}},&a));
}
''')
build=body(p,'static bool Build(')
assert 'candidateIndex < 33' in build
assert build.index('Prefer(distance,') < build.index('ERR_ConvoyRules.GroundClear(')
print('PASS: actual road interpolation/projection/orientation, six35m starts,200m forward tail, preserved bends, short/far/degenerate/nonfinite/overdense/hairpin refusal and deterministic candidate ties')