<commit_before>#include <Game/ECS/GameWorld.h>
#include <Game/ECS/Components/ActionQueueComponent.h>
#include <Game/ECS/Components/SceneComponent.h>
#include <AI/Navigation/NavAgentComponent.h>
#include <AI/AIStateComponent.h>
#include <AI/Movement/SteerAction.h>
#include <Game/Interaction/Ability.h>
#include <Game/Objects/SmartObjectComponent.h>
#include <Game/Objects/SmartObject.h>
#include <DetourCommon.h>

// Ability execution consists of the following steps:
// 1. Approach an interaction point
// 2. Face appropriate direction, if required
// 3. Start ability
// 4. Update ability until done, cancelled or failed
// 5. End ability

//!!!
// TODO: path optimization - optimize only when enter new poly, don't optimize in offmesh polys
// TODO: movement failed - add a timeout before failing, use ElapsedTime with Movement status
// TODO: handle cases without zones, without targets, check all is OK
// TODO: what if ability requires visibility line from actor to target?
// TODO: anim graph override

namespace DEM::Game
{

static bool GetFacingParams(const CAbilityInstance& AbilityInstance, const vector3& ActorPos, vector3& OutFacingDir, float* pOutFacingTolerance)
{
	CFacingParams Facing;
	if (AbilityInstance.Ability.GetFacingParams(Facing))
	{
		if (pOutFacingTolerance) *pOutFacingTolerance = std::max(Facing.Tolerance, AI::Turn::AngularTolerance);

		switch (Facing.Mode)
		{
			case EFacingMode::Direction:
			{
				OutFacingDir = Facing.Dir;
				return true;
			}
			case EFacingMode::Point:
			{
				OutFacingDir = AbilityInstance.TargetToWorld.transform_coord(Facing.Dir) - ActorPos;
				OutFacingDir.y = 0.f;
				OutFacingDir.norm();
				return true;
			}
		}
	}

	OutFacingDir = vector3::Zero;
	if (pOutFacingTolerance) *pOutFacingTolerance = AI::Turn::AngularTolerance;
	return false;
}
//---------------------------------------------------------------------

// When enter new navigation poly, check if it intersects an available zone. If so, move to it instead of original target zone.
static void OptimizePath(CAbilityInstance& AbilityInstance, CGameWorld& World, HEntity EntityID, HAction ChildAction)
{
	auto pNavAction = ChildAction.As<AI::Navigate>();
	if (!pNavAction) return;

	const auto* pNavAgent = World.FindComponent<AI::CNavAgentComponent>(EntityID);
	if (!pNavAgent || pNavAgent->Corridor.getFirstPoly() == AbilityInstance.CheckedPoly) return;

	AbilityInstance.CheckedPoly = pNavAgent->Corridor.getFirstPoly();
	if (!AbilityInstance.CheckedPoly) return;

	matrix44 WorldToTarget;
	AbilityInstance.TargetToWorld.invert_simple(WorldToTarget);
	const vector3 ActorPosInTargetSpace = WorldToTarget.transform_coord(pNavAgent->Corridor.getPos());

	const dtMeshTile* pTile = nullptr;
	const dtPoly* pPoly = nullptr;
	pNavAgent->pNavQuery->getAttachedNavMesh()->getTileAndPolyByRefUnsafe(AbilityInstance.CheckedPoly, &pTile, &pPoly);

	float Verts[DT_VERTS_PER_POLYGON * 3];
	const int PolyVertCount = pPoly->vertCount;
	for (int i = 0; i < PolyVertCount; ++i)
		dtVcopy(&Verts[i * 3], &pTile->verts[pPoly->verts[i] * 3]);

	// FIXME: how to find closest point of intersection of nav. poly and zone? Now wrong!
	float MinDistance = std::numeric_limits<float>().max();
	for (UPTR ZoneIdx = 0; ZoneIdx < AbilityInstance.AvailableZones.size(); ++ZoneIdx)
	{
		const auto& Zone = *AbilityInstance.AvailableZones[ZoneIdx];

		if (!Zone.IntersectsPoly(AbilityInstance.TargetToWorld, Verts, PolyVertCount)) continue;

		//???adjust for actor radius? for now 0.f
		vector3 Point;
		const float Distance = Zone.FindClosestPoint(ActorPosInTargetSpace, 0.f, Point);
		if (Distance < MinDistance)
		{
			MinDistance = Distance;
			pNavAction->_Destination = Point; // Remember local for now, will convert once at the end
			AbilityInstance.CurrZoneIndex = ZoneIdx;
		}
	}

	if (MinDistance != std::numeric_limits<float>().max())
	{
		pNavAction->_Destination = AbilityInstance.TargetToWorld.transform_coord(pNavAction->_Destination);
		GetFacingParams(AbilityInstance, pNavAction->_Destination, pNavAction->_FinalFacing, nullptr);
	}
}
//---------------------------------------------------------------------

static EActionStatus MoveToTarget(CAbilityInstance& AbilityInstance, CGameWorld& World, HEntity EntityID, CActionQueueComponent& Queue, HAction Action,
	HAction ChildAction, bool TransformChanged)
{
	// When no zones available for an interaction, run ability from the current position
	//if (AbilityInstance.InitialZones.empty()) return EActionStatus::Succeeded;

	if (TransformChanged)
	{
		// Target transform changed, therefore all zones might have changed, must update action point
		AbilityInstance.AvailableZones = AbilityInstance.InitialZones; // Intended copy
		AbilityInstance.CheckedPoly = 0;
	}
	else if (AbilityInstance.Stage > EAbilityExecutionStage::Movement)
	{
		// Already at position, skip movement
		return EActionStatus::Succeeded;
	}
	else if (ChildAction)
	{
		// Process movement sub-action
		const auto ChildActionStatus = Queue.GetStatus(ChildAction);
		if (ChildActionStatus != EActionStatus::Failed)
		{
			// Check if another available zone is closer along the way than our target zone
			if (ChildActionStatus == EActionStatus::Active)<commit_msg>Ability system: poly-by-poly path optimization must work, though it's far from perfect<commit_after>#include <Game/ECS/GameWorld.h>
#include <Game/ECS/Components/ActionQueueComponent.h>
#include <Game/ECS/Components/SceneComponent.h>
#include <AI/Navigation/NavAgentComponent.h>
#include <AI/AIStateComponent.h>
#include <AI/Movement/SteerAction.h>
#include <Game/Interaction/Ability.h>
#include <Game/Objects/SmartObjectComponent.h>
#include <Game/Objects/SmartObject.h>
#include <DetourCommon.h>

// Ability execution consists of the following steps:
// 1. Approach an interaction point
// 2. Face appropriate direction, if required
// 3. Start ability
// 4. Update ability until done, cancelled or failed
// 5. End ability

//!!!
// TODO: path optimization - optimize only when enter new poly, don't optimize in offmesh polys
// TODO: movement failed - add a timeout before failing, use ElapsedTime with Movement status
// TODO: handle cases without zones, without targets, check all is OK
// TODO: what if ability requires visibility line from actor to target?
// TODO: anim graph override

namespace DEM::Game
{

static bool GetFacingParams(const CAbilityInstance& AbilityInstance, const vector3& ActorPos, vector3& OutFacingDir, float* pOutFacingTolerance)
{
	CFacingParams Facing;
	if (AbilityInstance.Ability.GetFacingParams(Facing))
	{
		if (pOutFacingTolerance) *pOutFacingTolerance = std::max(Facing.Tolerance, AI::Turn::AngularTolerance);

		switch (Facing.Mode)
		{
			case EFacingMode::Direction:
			{
				OutFacingDir = Facing.Dir;
				return true;
			}
			case EFacingMode::Point:
			{
				OutFacingDir = AbilityInstance.TargetToWorld.transform_coord(Facing.Dir) - ActorPos;
				OutFacingDir.y = 0.f;
				OutFacingDir.norm();
				return true;
			}
		}
	}

	OutFacingDir = vector3::Zero;
	if (pOutFacingTolerance) *pOutFacingTolerance = AI::Turn::AngularTolerance;
	return false;
}
//---------------------------------------------------------------------

// When enter new navigation poly, check if it intersects an available zone. If so, move to it instead of original target zone.
static void OptimizePath(CAbilityInstance& AbilityInstance, CGameWorld& World, HEntity EntityID, HAction ChildAction)
{
	auto pNavAction = ChildAction.As<AI::Navigate>();
	if (!pNavAction) return;

	const auto* pNavAgent = World.FindComponent<AI::CNavAgentComponent>(EntityID);
	if (!pNavAgent || pNavAgent->Corridor.getFirstPoly() == AbilityInstance.CheckedPoly) return;

	AbilityInstance.CheckedPoly = pNavAgent->Corridor.getFirstPoly();
	if (!AbilityInstance.CheckedPoly) return;

	matrix44 WorldToTarget;
	AbilityInstance.TargetToWorld.invert_simple(WorldToTarget);
	const vector3 ActorPosInTargetSpace = WorldToTarget.transform_coord(pNavAgent->Corridor.getPos());

	const dtMeshTile* pTile = nullptr;
	const dtPoly* pPoly = nullptr;
	pNavAgent->pNavQuery->getAttachedNavMesh()->getTileAndPolyByRefUnsafe(AbilityInstance.CheckedPoly, &pTile, &pPoly);

	float Verts[DT_VERTS_PER_POLYGON * 3];
	const int PolyVertCount = pPoly->vertCount;
	for (int i = 0; i < PolyVertCount; ++i)
		dtVcopy(&Verts[i * 3], &pTile->verts[pPoly->verts[i] * 3]);

	float MinDistance = std::numeric_limits<float>().max();
	for (UPTR ZoneIdx = 0; ZoneIdx < AbilityInstance.AvailableZones.size(); ++ZoneIdx)
	{
		const auto& Zone = *AbilityInstance.AvailableZones[ZoneIdx];

		// FIXME: need to find an union of zone and curent poly. Then must find a point in that union
		// closest to the actor position. Now the logic is bad and misses some opportunities.

		if (!Zone.IntersectsPoly(AbilityInstance.TargetToWorld, Verts, PolyVertCount)) continue;

		//???adjust for actor radius? for now 0.f
		vector3 Point;
		const float Distance = Zone.FindClosestPoint(ActorPosInTargetSpace, 0.f, Point);
		if (MinDistance <= Distance) continue;

		Point = AbilityInstance.TargetToWorld.transform_coord(Point);

		float NavigablePos[3];
		const float Extents[3] = { Zone.Radius, pNavAgent->Height, Zone.Radius };
		dtPolyRef ObjPolyRef = 0;
		pNavAgent->pNavQuery->findNearestPoly(Point.v, Extents, pNavAgent->Settings->GetQueryFilter(), &ObjPolyRef, NavigablePos);
		if (!ObjPolyRef || dtVdist2DSqr(Point.v, NavigablePos) > Zone.Radius * Zone.Radius) continue;

		MinDistance = Distance;
		pNavAction->_Destination = Point; // Remember local for now, will convert once at the end
		AbilityInstance.CurrZoneIndex = ZoneIdx;
	}

	// If destination changed, update arrival facing
	if (MinDistance != std::numeric_limits<float>().max())
		GetFacingParams(AbilityInstance, pNavAction->_Destination, pNavAction->_FinalFacing, nullptr);
}
//---------------------------------------------------------------------

static EActionStatus MoveToTarget(CAbilityInstance& AbilityInstance, CGameWorld& World, HEntity EntityID, CActionQueueComponent& Queue, HAction Action,
	HAction ChildAction, bool TransformChanged)
{
	// When no zones available for an interaction, run ability from the current position
	//if (AbilityInstance.InitialZones.empty()) return EActionStatus::Succeeded;

	if (TransformChanged)
	{
		// Target transform changed, therefore all zones might have changed, must update action point
		AbilityInstance.AvailableZones = AbilityInstance.InitialZones; // Intended copy
		AbilityInstance.CheckedPoly = 0;
	}
	else if (AbilityInstance.Stage > EAbilityExecutionStage::Movement)
	{
		// Already at position, skip movement
		return EActionStatus::Succeeded;
	}
	else if (ChildAction)
	{
		// Process movement sub-action
		const auto ChildActionStatus = Queue.GetStatus(ChildAction);
		if (ChildActionStatus != EActionStatus::Failed)
		{
			// Check if another available zone is closer along the way than our target zone
			if (ChildActionStatus == EActionStatus::Active)
{'fraction_non_alphanumeric': 0.12844874840124246, 'fraction_numerical': 0.0042024483829709485, 'mean_word_length': 4.487218045112782, 'pattern_counts': {'":': 0, '<': 37, '<?xml version=': 0, '>': 65, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '6436412', 'n_tokens_mistral': 3509, 'n_tokens_neox': 3262, 'n_words': 886}