ECSでNavMesh
今回はUnityでNavMeshを使用した経路探索機能を作成します。
UnityのNavMeshの機能を使えるので簡単に地形に沿った動きができるようになるのはいいですね。
※今回使用するUnityECSのNavMeshAPI("NavmeshQuery")は現在非推奨で削除される可能性があるため注意
現在Unity6ではまだ動きます。
ECS用のNavigationSystem早く公式で出してくれるとありがたいですね。
NavMeshを取得する方法
UnityECSでは以下の文で現在のワールドに作成してあるNavMeshを取得できます。
NavMeshWorld.GetDefaultWorld();
これでNavMeshを取得し、関連する非推奨のAPIを使用してNavMeshNavigationが実現されます。
また、以下に示すNavMeshPathFinding用の便利関数がオープンソースで公開されているため、これを使ってNavigationを行います。
//
// Copyright (c) 2009-2010 Mikko Mononen memon@inside.org
//
// This software is provided 'as-is', without any express or implied
// warranty. In no event will the authors be held liable for any damages
// arising from the use of this software.
// Permission is granted to anyone to use this software for any purpose,
// including commercial applications, and to alter it and redistribute it
// freely, subject to the following restrictions:
// 1. The origin of this software must not be misrepresented; you must not
// claim that you wrote the original software. If you use this software
// in a product, an acknowledgment in the product documentation would be
// appreciated but is not required.
// 2. Altered source versions must be plainly marked as such, and must not be
// misrepresented as being the original software.
// 3. This notice may not be removed or altered from any source distribution.
//
// The original source code has been modified by Unity Technologies.
using System;
using Unity.Collections;
using Unity.Mathematics;
using UnityEngine;
using UnityEngine.Assertions;
using UnityEngine.Experimental.AI;
[Flags]
public enum StraightPathFlags
{
Start = 0x01, // The vertex is the start position.
End = 0x02, // The vertex is the end position.
OffMeshConnection = 0x04 // The vertex is start of an off-mesh link.
}
public class PathUtils
{
public static float Perp2D(Vector3 u, Vector3 v)
{
return u.z * v.x - u.x * v.z;
}
public static void Swap(ref Vector3 a, ref Vector3 b)
{
var temp = a;
a = b;
b = temp;
}
// Retrace portals between corners and register if type of polygon changes
public static int RetracePortals(NavMeshQuery query, int startIndex, int endIndex
, NativeSlice<PolygonId> path, int n, Vector3 termPos
, ref NativeArray<NavMeshLocation> straightPath
, ref NativeArray<StraightPathFlags> straightPathFlags
, int maxStraightPath)
{
#if DEBUG_CROWDSYSTEM_ASSERTS
Assert.IsTrue(n < maxStraightPath);
Assert.IsTrue(startIndex <= endIndex);
#endif
for (var k = startIndex; k < endIndex - 1; ++k)
{
var type1 = query.GetPolygonType(path[k]);
var type2 = query.GetPolygonType(path[k + 1]);
if (type1 != type2)
{
Vector3 l, r;
var status = query.GetPortalPoints(path[k], path[k + 1], out l, out r);
#if DEBUG_CROWDSYSTEM_ASSERTS
Assert.IsTrue(status); // Expect path elements k, k+1 to be verified
#endif
float3 cpa1, cpa2;
GeometryUtils.SegmentSegmentCPA(out cpa1, out cpa2, l, r, straightPath[n - 1].position, termPos);
straightPath[n] = query.CreateLocation(cpa1, path[k + 1]);
straightPathFlags[n] = (type2 == NavMeshPolyTypes.OffMeshConnection) ? StraightPathFlags.OffMeshConnection : 0;
if (++n == maxStraightPath)
{
return maxStraightPath;
}
}
}
straightPath[n] = query.CreateLocation(termPos, path[endIndex]);
straightPathFlags[n] = query.GetPolygonType(path[endIndex]) == NavMeshPolyTypes.OffMeshConnection ? StraightPathFlags.OffMeshConnection : 0;
return ++n;
}
public static PathQueryStatus FindStraightPath(NavMeshQuery query, Vector3 startPos, Vector3 endPos
, NativeSlice<PolygonId> path, int pathSize
, ref NativeArray<NavMeshLocation> straightPath
, ref NativeArray<StraightPathFlags> straightPathFlags
, ref NativeArray<float> vertexSide
, ref int straightPathCount
, int maxStraightPath)
{
#if DEBUG_CROWDSYSTEM_ASSERTS
Assert.IsTrue(pathSize > 0, "FindStraightPath: The path cannot be empty");
Assert.IsTrue(path.Length >= pathSize, "FindStraightPath: The array of path polygons must fit at least the size specified");
Assert.IsTrue(maxStraightPath > 1, "FindStraightPath: At least two corners need to be returned, the start and end");
Assert.IsTrue(straightPath.Length >= maxStraightPath, "FindStraightPath: The array of returned corners cannot be smaller than the desired maximum corner count");
Assert.IsTrue(straightPathFlags.Length >= straightPath.Length, "FindStraightPath: The array of returned flags must not be smaller than the array of returned corners");
#endif
if (!query.IsValid(path[0]))
{
straightPath[0] = new NavMeshLocation(); // empty terminator
return PathQueryStatus.Failure; // | PathQueryStatus.InvalidParam;
}
straightPath[0] = query.CreateLocation(startPos, path[0]);
straightPathFlags[0] = StraightPathFlags.Start;
var apexIndex = 0;
var n = 1;
if (pathSize > 1)
{
var startPolyWorldToLocal = query.PolygonWorldToLocalMatrix(path[0]);
var apex = startPolyWorldToLocal.MultiplyPoint(startPos);
var left = new Vector3(0, 0, 0); // Vector3.zero accesses a static readonly which does not work in burst yet
var right = new Vector3(0, 0, 0);
var leftIndex = -1;
var rightIndex = -1;
for (var i = 1; i <= pathSize; ++i)
{
var polyWorldToLocal = query.PolygonWorldToLocalMatrix(path[apexIndex]);
Vector3 vl, vr;
if (i == pathSize)
{
vl = vr = polyWorldToLocal.MultiplyPoint(endPos);
}
else
{
var success = query.GetPortalPoints(path[i - 1], path[i], out vl, out vr);
if (!success)
{
return PathQueryStatus.Failure; // | PathQueryStatus.InvalidParam;
}
#if DEBUG_CROWDSYSTEM_ASSERTS
Assert.IsTrue(query.IsValid(path[i - 1]));
Assert.IsTrue(query.IsValid(path[i]));
#endif
vl = polyWorldToLocal.MultiplyPoint(vl);
vr = polyWorldToLocal.MultiplyPoint(vr);
}
vl = vl - apex;
vr = vr - apex;
// Ensure left/right ordering
if (Perp2D(vl, vr) < 0)
Swap(ref vl, ref vr);
// Terminate funnel by turning
if (Perp2D(left, vr) < 0)
{
var polyLocalToWorld = query.PolygonLocalToWorldMatrix(path[apexIndex]);
var termPos = polyLocalToWorld.MultiplyPoint(apex + left);
n = RetracePortals(query, apexIndex, leftIndex, path, n, termPos, ref straightPath, ref straightPathFlags, maxStraightPath);
if (vertexSide.Length > 0)
{
vertexSide[n - 1] = -1;
}
//Debug.Log("LEFT");
if (n == maxStraightPath)
{
straightPathCount = n;
return PathQueryStatus.Success; // | PathQueryStatus.BufferTooSmall;
}
apex = polyWorldToLocal.MultiplyPoint(termPos);
left.Set(0, 0, 0);
right.Set(0, 0, 0);
i = apexIndex = leftIndex;
continue;
}
if (Perp2D(right, vl) > 0)
{
var polyLocalToWorld = query.PolygonLocalToWorldMatrix(path[apexIndex]);
var termPos = polyLocalToWorld.MultiplyPoint(apex + right);
n = RetracePortals(query, apexIndex, rightIndex, path, n, termPos, ref straightPath, ref straightPathFlags, maxStraightPath);
if (vertexSide.Length > 0)
{
vertexSide[n - 1] = 1;
}
//Debug.Log("RIGHT");
if (n == maxStraightPath)
{
straightPathCount = n;
return PathQueryStatus.Success; // | PathQueryStatus.BufferTooSmall;
}
apex = polyWorldToLocal.MultiplyPoint(termPos);
left.Set(0, 0, 0);
right.Set(0, 0, 0);
i = apexIndex = rightIndex;
continue;
}
// Narrow funnel
if (Perp2D(left, vl) >= 0)
{
left = vl;
leftIndex = i;
}
if (Perp2D(right, vr) <= 0)
{
right = vr;
rightIndex = i;
}
}
}
// Remove the the next to last if duplicate point - e.g. start and end positions are the same
// (in which case we have get a single point)
if (n > 0 && (straightPath[n - 1].position == endPos))
n--;
n = RetracePortals(query, apexIndex, pathSize - 1, path, n, endPos, ref straightPath, ref straightPathFlags, maxStraightPath);
if (vertexSide.Length > 0)
{
vertexSide[n - 1] = 0;
}
if (n == maxStraightPath)
{
straightPathCount = n;
return PathQueryStatus.Success; // | PathQueryStatus.BufferTooSmall;
}
// Fix flag for final path point
straightPathFlags[n - 1] = StraightPathFlags.End;
straightPathCount = n;
return PathQueryStatus.Success;
}
}
public class GeometryUtils
{
// Calculate the closest point of approach for line-segment vs line-segment.
public static bool SegmentSegmentCPA(out float3 c0, out float3 c1, float3 p0, float3 p1, float3 q0, float3 q1)
{
var u = p1 - p0;
var v = q1 - q0;
var w0 = p0 - q0;
float a = math.dot(u, u);
float b = math.dot(u, v);
float c = math.dot(v, v);
float d = math.dot(u, w0);
float e = math.dot(v, w0);
float den = (a * c - b * b);
float sc, tc;
if (den == 0)
{
sc = 0;
tc = d / b;
// todo: handle b = 0 (=> a and/or c is 0)
}
else
{
sc = (b * e - c * d) / (a * c - b * b);
tc = (a * e - b * d) / (a * c - b * b);
}
c0 = math.lerp(p0, p1, sc);
c1 = math.lerp(q0, q1, tc);
return den != 0;
}
}
用意するコンポーネント
用意するコンポーネントは以下のようになります。
これはAIにつけるデータです。
public struct AIController : IComponentData
{
// 目標物
public Entity TargetEntity;
// 見つける範囲とコライダーレイヤー
public float DetectionDistance;
public CustomPhysicsBodyTags DetectionFilter;
//Navgation
// 目指すポイント
public float3 CorrectedWayPoint;
//NavMeshSetting
// NavMesh用のセッティング
public float3 Extents;
public int MaxIterations;
public int MaxPathSize;
}
// AIコントローラーかどうかのタグ
public struct AIControllerTag : IComponentData
{}
Authoring
続いてAuthoringです。
NavMeshさせたい対象にこれをアタッチしてください
public class AIControllerAuthoring : MonoBehaviour
{
// 検索範囲と検索対象のコライダータグ
[Header("Agent")]
public float DetectionDistance;
public CustomPhysicsBodyTags DetectionFilter;
// NavMeshの設定
[Header("NavigationSetting")]
public float3 Extents;
public int MaxIterations;
public int MaxPathSize;
class Baker : Baker<AIControllerAuthoring>
{
public override void Bake(AIControllerAuthoring authoring)
{
Entity entity = GetEntity(TransformUsageFlags.None);
AddComponent(entity, new AIController
{
DetectionDistance = authoring.DetectionDistance,
DetectionFilter = authoring.DetectionFilter,
Extents = authoring.Extents,
MaxIterations = authoring.MaxIterations,
MaxPathSize = authoring.MaxPathSize,
});
AddComponent(entity, new AIControllerTag());
}
}
}
これをアタッチして以下のように設定しておきます。
Detection DistanceとDetectionFilterについては任意の数値を入力してください。
その他については写真の数値を記入しておくのが無難です。
System
今回Systemはターゲットを見つけるシステム,パスを探すシステム,AIを移動させるシステムの3つを作成します。
まずはターゲットを見つけるシステムから
[UpdateInGroup(typeof(SimulationSystemGroup))]
[UpdateAfter(typeof(FixedStepSimulationSystemGroup))]
partial struct AITargetDitectiveSystem : ISystem
{
private PhysicsWorld physicsWorld;
[BurstCompile]
public void OnCreate(ref SystemState state)
{
physicsWorld = SystemAPI.GetSingleton<PhysicsWorldSingleton>().PhysicsWorld;
}
[BurstCompile]
partial struct TargetDitectiveJob : IJobEntity
{
[ReadOnly] public PhysicsWorld PhysWorld;
[ReadOnly] public ComponentLookup<CharacterControl> TargetCC;
[ReadOnly] public ComponentLookup<AIControllerTag> TargetAI;
void Execute(ref AIController aiController, in LocalTransform localTransform)
{
// 指定した距離範囲にあるEntityを取得するための箱を用意
NativeList<DistanceHit> distanceHits = new NativeList<DistanceHit>(Allocator.TempJob);
AllHitsCollector<DistanceHit> hitCollector = new AllHitsCollector<DistanceHit>(aiController.DetectionDistance, ref distanceHits);
// このPointDistanceInputに現在の位置、検索半径とコライダーフィルターを入力してCalculateDistanceを使用すると検索条件内のエンティティをhitCollectorに入れてくれる
PointDistanceInput distInput = new PointDistanceInput
{
Position = localTransform.Position,
MaxDistance = aiController.DetectionDistance,
Filter = new CollisionFilter { BelongsTo = CollisionFilter.Default.BelongsTo, CollidesWith = aiController.DetectionFilter.Value },
};
PhysWorld.CalculateDistance(distInput, ref hitCollector);
// Iterate on all detected hits to try to find a human-controlled character...
aiController.TargetEntity = Entity.Null;
// 検索条件内に入っていたエンティティすべてを見て、
// CharacterControlを持っていてAIControllerTagを持っていないエンティティを見つけ
// そのエンティティをターゲットに格納する
for (int i = 0; i < hitCollector.NumHits; i++)
{
Entity hitEntity = distanceHits[i].Entity;
// If it has a character component but no AIController component, that means it's a human player character
if (TargetCC.HasComponent(hitEntity) && !TargetAI.HasComponent(hitEntity))
{
aiController.TargetEntity = hitEntity;
break; // early out
}
}
distanceHits.Dispose();
}
}
[BurstCompile]
public void OnUpdate(ref SystemState state)
{
physicsWorld = SystemAPI.GetSingleton<PhysicsWorldSingleton>().PhysicsWorld;
// CharacterConponentをTargetCCとして、AIControllerTagをTargetAIとしてJobに送る
var job = new TargetDitectiveJob
{
PhysWorld = physicsWorld,
TargetCC = SystemAPI.GetComponentLookup<CharacterControl>(),
TargetAI = SystemAPI.GetComponentLookup<AIControllerTag>(),
};
job.ScheduleParallel();
}
[BurstCompile]
public void OnDestroy(ref SystemState state)
{
}
}
このシステムではPhysicsWorldの便利関数を使って自身から指定した範囲のエンティティをすべて取得し、
その中でCharacterControlを持っていてAIControllerTagを付けていないエンティティをターゲットとして決めています。(ターゲットはPlayerのエンティティです。)
次に重要なパスファインディングのシステムです。
// NavMeshをポインターとして取得しておくための構造体
unsafe public struct NavMeshQuerePointer
{
[NativeDisableUnsafePtrRestriction]
public void* Value;
}
[UpdateInGroup(typeof(SimulationSystemGroup))]
[UpdateAfter(typeof(FixedStepSimulationSystemGroup))]
unsafe partial struct AIPathFindingSystem : ISystem
{
// NavMeshの情報がはいいているWorld
private NavMeshWorld _navMeshWorld;
// NavMeshQuereが取得できたかどうかのbool値
private bool _navMeshQuereAssign;
// NavMeshをNavMeshPointer[]の形で持つ(配列といってもこの配列の要素はすべて同じ)
private NativeArray<NavMeshQuerePointer> PointerArray;
private NativeList<NavMeshQuery> quereList;
[BurstCompile]
public void OnCreate(ref SystemState state)
{
// NavMeshを取得していないとする
_navMeshQuereAssign = false;
// この関数でNavMeshを取得
_navMeshWorld = NavMeshWorld.GetDefaultWorld();
}
// NavMeshQuereをポインターとして取得するための処理
[BurstCompile]
private void ParallelizePointerQuere()
{
var pointerArray = new NativeArray<NavMeshQuerePointer>(JobsUtility.MaxJobThreadCount, Allocator.Temp);
quereList = new NativeList<NavMeshQuery>(Allocator.Persistent);
for (var i = 0; i < JobsUtility.MaxJobThreadCount; ++i)
{
// ポインターの配列にNavMeshの情報をコピーする
pointerArray[i] = new NavMeshQuerePointer
{
Value = UnsafeUtility.Malloc(
UnsafeUtility.SizeOf<NavMeshQuery>(),
UnsafeUtility.AlignOf<NavMeshQuery>(),
Allocator.Persistent)
};
var quere = new NavMeshQuery(
_navMeshWorld,
Allocator.Persistent,
1024);
//NavmeshQuereをQuereListに追加
quereList.Add(quere);
// 構造体をポインター配列に変換
UnsafeUtility.CopyStructureToPtr(ref quere, pointerArray[i].Value);
}
// PointerArrayにNavMeshQuereの情報をすべて入れておく
PointerArray = new NativeArray<NavMeshQuerePointer>(pointerArray, Allocator.Persistent);
pointerArray.Dispose();
}
[BurstCompile]
public void OnUpdate(ref SystemState state)
{
// NavMeshQueryのポインター配列を作成する処理
if (!_navMeshQuereAssign)
{
ParallelizePointerQuere();
_navMeshQuereAssign = true;
}
// NavMeshQueryを使用してパスを見つける
var job = new PathFindingJob
{
querePointerArray = PointerArray,
targetTransfrom = SystemAPI.GetComponentLookup<LocalTransform>(),
};
job.ScheduleParallel();
}
[BurstCompile]
partial struct PathFindingJob : IJobEntity
{
// 取得したNavMeshの情報
[NativeDisableParallelForRestriction]
public NativeArray<NavMeshQuerePointer> querePointerArray;
[ReadOnly] public ComponentLookup<LocalTransform> targetTransfrom;
[NativeSetThreadIndex]
public int threadIndex;
void Execute(ref AIController aiController, in LocalTransform localTransform, [ChunkIndexInQuery]int sortKey)
{
// ターゲットが存在するとき
if (aiController.TargetEntity != Entity.Null)
{
// 現在の自分の位置と目標の位置を格納
float3 fromPosition = localTransform.Position;
float3 toPosition = targetTransfrom[aiController.TargetEntity].Position;
float3 extents = aiController.Extents;
//NavMeshポインターの配列から現在のスレッドのNavMeshQuereポインターを取得する
var navMeshQuerePointer = querePointerArray[threadIndex];
// ポインターからNavMeshQueryに変換
UnsafeUtility.CopyPtrToStructure(navMeshQuerePointer.Value, out NavMeshQuery navmeshQuery);
// NavMeshQueryを使用して現在の位置と目標の位置をNavMesh上のどこの位置かに変換する
NavMeshLocation fromLocation = navmeshQuery.MapLocation(fromPosition, extents, 0);
NavMeshLocation toLocation = navmeshQuery.MapLocation(toPosition, extents, 0);
// PathFindができたかどうかのステータス
PathQueryStatus status;
PathQueryStatus returningStatus;
// 現在の位置と目標位置がNavMesh上にあったとき
if (navmeshQuery.IsValid(fromLocation) && navmeshQuery.IsValid(toLocation))
{
// NavMeshQueryAPIのパス検索を呼び出す
status = navmeshQuery.BeginFindPath(fromLocation, toLocation);
if (status == PathQueryStatus.InProgress)
{
// 検索したパスをアップデートする
status = navmeshQuery.UpdateFindPath(100, out int iterationsPerformed);
// パスが見つかったら
if (status == PathQueryStatus.Success)
{
// NavMeshQueryのEndFindPathを呼び出し終了する
status = navmeshQuery.EndFindPath(out int pathSize);
// 結果を入れる箱
NativeArray<NavMeshLocation> result = new NativeArray<NavMeshLocation>(pathSize + 1, Allocator.Temp);
// 直線のパスを入れる箱
NativeArray<StraightPathFlags> straightPathFlags = new NativeArray<StraightPathFlags>(aiController.MaxPathSize, Allocator.Temp);
// 頂点の端を入れる箱
NativeArray<float> vertexSide = new NativeArray<float>(aiController.MaxPathSize, Allocator.Temp);
// NavMeshのポリゴンIDを入れる箱
NativeArray<PolygonId> polygonIds = new NativeArray<PolygonId>(pathSize + 1, Allocator.Temp);
int straightPathCount = 0;
// NavMeshQueryAPIを使用して検索したパスのポリゴンＩＤを取得
navmeshQuery.GetPathResult(polygonIds);
// NavMeshQueryAPIの結果のポリゴンＩＤから直線の情報を取得
returningStatus = PathUtils.FindStraightPath
(navmeshQuery, fromPosition, toPosition, polygonIds, pathSize, ref result,
ref straightPathFlags, ref vertexSide, ref straightPathCount, aiController.MaxPathSize);
// 直線のパスを取得出来たらその１つ目の目標点をAIの目標点にする
if (returningStatus == PathQueryStatus.Success)
{
aiController.CorrectedWayPoint = result[1].position;
}
straightPathFlags.Dispose();
polygonIds.Dispose();
vertexSide.Dispose();
}
}
}
}
}
}
[BurstCompile]
public void OnDestroy(ref SystemState state)
{
foreach (var queue in quereList) queue.Dispose();
foreach (var pointer in PointerArray) UnsafeUtility.Free(pointer.Value, Allocator.Persistent);
quereList.Dispose();
PointerArray.Dispose();
}
}
少しややこしくて理解しづらいですが、要はNavMeshQueryを使うと簡単にNavMeshでパス(NavMeshの頂点)が取得できるので、そのパスを見つけた後、直線に変換して、最初の目標点をAIの目標点にしているということです。
ParallelizePointerQuereは並列化を行うために、NavMeshQueryをポインター配列にして、ジョブ内でNavMeshQueryに変換後、NavMeshQueryのAPIを使うようにしています。
最後に移動処理です。
partial struct AIControllerSystem : ISystem
{
[BurstCompile]
public void OnCreate(ref SystemState state)
{
}
[BurstCompile]
public void OnUpdate(ref SystemState state)
{
foreach (var (characterControl, aiController, localTransform, entity) in SystemAPI.Query<RefRW<CharacterControl>, AIController, LocalTransform>().WithEntityAccess())
{
// ターゲットがあるとき、CharacterControl(自身の)のMoveVectorを目標のポジションと自身のポジションから設定している
if (aiController.TargetEntity != Entity.Null)
{
characterControl.ValueRW.MoveVector = math.normalizesafe(aiController.CorrectedWayPoint - localTransform.Position);
}
else
{
characterControl.ValueRW.MoveVector = float3.zero;
}
}
}
[BurstCompile]
public void OnDestroy(ref SystemState state)
{
}
}
この移動処理は簡単ですね、目標の点と自身の点から方向を決めてやり、あとはCharacterControlSystemで移動させてやっているだけです。