diff --git a/src/DotRecast.Core/ArrayUtils.cs b/src/DotRecast.Core/ArrayUtils.cs index f2f6c43..55f9c3a 100644 --- a/src/DotRecast.Core/ArrayUtils.cs +++ b/src/DotRecast.Core/ArrayUtils.cs @@ -2,43 +2,41 @@ namespace DotRecast.Core { - - -public static class ArrayUtils -{ - public static T[] CopyOf(T[] source, int startIdx, int length) + public static class ArrayUtils { - var deatArr = new T[length]; - for (int i = 0; i < length; ++i) + public static T[] CopyOf(T[] source, int startIdx, int length) { - deatArr[i] = source[startIdx + i]; + var deatArr = new T[length]; + for (int i = 0; i < length; ++i) + { + deatArr[i] = source[startIdx + i]; + } + + return deatArr; } - return deatArr; + public static T[] CopyOf(T[] source, int length) + { + var deatArr = new T[length]; + var count = Math.Max(0, Math.Min(source.Length, length)); + for (int i = 0; i < count; ++i) + { + deatArr[i] = source[i]; + } + + return deatArr; + } + + public static T[][] Of(int len1, int len2) + { + var temp = new T[len1][]; + + for (int i = 0; i < len1; ++i) + { + temp[i] = new T[len2]; + } + + return temp; + } } - - public static T[] CopyOf(T[] source, int length) - { - var deatArr = new T[length]; - var count = Math.Max(0, Math.Min(source.Length, length)); - for (int i = 0; i < count; ++i) - { - deatArr[i] = source[i]; - } - - return deatArr; - } - - public static T[][] Of(int len1, int len2) - { - var temp = new T[len1][]; - - for (int i = 0; i < len1; ++i) - { - temp[i] = new T[len2]; - } - - return temp; - } -} } \ No newline at end of file diff --git a/src/DotRecast.Core/AtomicBoolean.cs b/src/DotRecast.Core/AtomicBoolean.cs index 2d7848c..dd8d615 100644 --- a/src/DotRecast.Core/AtomicBoolean.cs +++ b/src/DotRecast.Core/AtomicBoolean.cs @@ -2,20 +2,18 @@ namespace DotRecast.Core { - - -public class AtomicBoolean -{ - private volatile int _location; - - public bool set(bool v) + public class AtomicBoolean { - return 0 != Interlocked.Exchange(ref _location, v ? 1 : 0); - } + private volatile int _location; - public bool get() - { - return 0 != _location; + public bool set(bool v) + { + return 0 != Interlocked.Exchange(ref _location, v ? 1 : 0); + } + + public bool get() + { + return 0 != _location; + } } -} } \ No newline at end of file diff --git a/src/DotRecast.Core/AtomicFloat.cs b/src/DotRecast.Core/AtomicFloat.cs index a9a1dd5..7508a27 100644 --- a/src/DotRecast.Core/AtomicFloat.cs +++ b/src/DotRecast.Core/AtomicFloat.cs @@ -2,30 +2,28 @@ namespace DotRecast.Core { - - -public class AtomicFloat -{ - private volatile float _location; - - public AtomicFloat(float location) + public class AtomicFloat { - _location = location; - } + private volatile float _location; - public float Get() - { - return _location; - } + public AtomicFloat(float location) + { + _location = location; + } - public float Exchange(float exchange) - { - return Interlocked.Exchange(ref _location, exchange); - } + public float Get() + { + return _location; + } - public float CompareExchange(float value, float comparand) - { - return Interlocked.CompareExchange(ref _location, value, comparand); + public float Exchange(float exchange) + { + return Interlocked.Exchange(ref _location, exchange); + } + + public float CompareExchange(float value, float comparand) + { + return Interlocked.CompareExchange(ref _location, value, comparand); + } } -} } \ No newline at end of file diff --git a/src/DotRecast.Core/AtomicInteger.cs b/src/DotRecast.Core/AtomicInteger.cs index 97f02b2..4c80650 100644 --- a/src/DotRecast.Core/AtomicInteger.cs +++ b/src/DotRecast.Core/AtomicInteger.cs @@ -2,68 +2,64 @@ namespace DotRecast.Core { - - -public class AtomicInteger -{ - private volatile int _location; - - public AtomicInteger() : this(0) + public class AtomicInteger { - - } + private volatile int _location; - public AtomicInteger(int location) - { - _location = location; - } + public AtomicInteger() : this(0) + { + } - public int IncrementAndGet() - { - return Interlocked.Increment(ref _location); - } - - public int GetAndIncrement() - { - var next = Interlocked.Increment(ref _location); - return next - 1; - } + public AtomicInteger(int location) + { + _location = location; + } + + public int IncrementAndGet() + { + return Interlocked.Increment(ref _location); + } + + public int GetAndIncrement() + { + var next = Interlocked.Increment(ref _location); + return next - 1; + } - public int DecrementAndGet() - { - return Interlocked.Decrement(ref _location); - } + public int DecrementAndGet() + { + return Interlocked.Decrement(ref _location); + } - public int Read() - { - return _location; - } + public int Read() + { + return _location; + } - public int GetSoft() - { - return _location; - } + public int GetSoft() + { + return _location; + } - public int Exchange(int exchange) - { - return Interlocked.Exchange(ref _location, exchange); - } + public int Exchange(int exchange) + { + return Interlocked.Exchange(ref _location, exchange); + } - public int Decrease(int value) - { - return Interlocked.Add(ref _location, -value); - } + public int Decrease(int value) + { + return Interlocked.Add(ref _location, -value); + } - public int CompareExchange(int value, int comparand) - { - return Interlocked.CompareExchange(ref _location, value, comparand); - } + public int CompareExchange(int value, int comparand) + { + return Interlocked.CompareExchange(ref _location, value, comparand); + } - public int Add(int value) - { - return Interlocked.Add(ref _location, value); + public int Add(int value) + { + return Interlocked.Add(ref _location, value); + } } - -} } \ No newline at end of file diff --git a/src/DotRecast.Core/AtomicLong.cs b/src/DotRecast.Core/AtomicLong.cs index 5046bda..b02ed82 100644 --- a/src/DotRecast.Core/AtomicLong.cs +++ b/src/DotRecast.Core/AtomicLong.cs @@ -2,54 +2,52 @@ namespace DotRecast.Core { - - -public class AtomicLong -{ - private long _location; - - public AtomicLong() : this(0) + public class AtomicLong { - } - - public AtomicLong(long location) - { - _location = location; - } + private long _location; - public long IncrementAndGet() - { - return Interlocked.Increment(ref _location); - } + public AtomicLong() : this(0) + { + } - public long DecrementAndGet() - { - return Interlocked.Decrement(ref _location); - } + public AtomicLong(long location) + { + _location = location; + } - public long Read() - { - return Interlocked.Read(ref _location); - } + public long IncrementAndGet() + { + return Interlocked.Increment(ref _location); + } - public long Exchange(long exchange) - { - return Interlocked.Exchange(ref _location, exchange); - } + public long DecrementAndGet() + { + return Interlocked.Decrement(ref _location); + } - public long Decrease(long value) - { - return Interlocked.Add(ref _location, -value); - } + public long Read() + { + return Interlocked.Read(ref _location); + } - public long CompareExchange(long value, long comparand) - { - return Interlocked.CompareExchange(ref _location, value, comparand); - } + public long Exchange(long exchange) + { + return Interlocked.Exchange(ref _location, exchange); + } - public long AddAndGet(long value) - { - return Interlocked.Add(ref _location, value); + public long Decrease(long value) + { + return Interlocked.Add(ref _location, -value); + } + + public long CompareExchange(long value, long comparand) + { + return Interlocked.CompareExchange(ref _location, value, comparand); + } + + public long AddAndGet(long value) + { + return Interlocked.Add(ref _location, value); + } } -} } \ No newline at end of file diff --git a/src/DotRecast.Core/ByteBuffer.cs b/src/DotRecast.Core/ByteBuffer.cs index 90c50af..b487b98 100644 --- a/src/DotRecast.Core/ByteBuffer.cs +++ b/src/DotRecast.Core/ByteBuffer.cs @@ -3,131 +3,131 @@ using System.Buffers.Binary; namespace DotRecast.Core { - - -public class ByteBuffer -{ - private ByteOrder _order; - private byte[] _bytes; - private int _position; - - public ByteBuffer(byte[] bytes) + public class ByteBuffer { - _order = BitConverter.IsLittleEndian - ? ByteOrder.LITTLE_ENDIAN - : ByteOrder.BIG_ENDIAN; + private ByteOrder _order; + private byte[] _bytes; + private int _position; - _bytes = bytes; - _position = 0; - } - - public ByteOrder order() - { - return _order; - } - - public void order(ByteOrder order) - { - _order = order; - } - - public int limit() { - return _bytes.Length - _position; - } - - public int remaining() { - int rem = limit(); - return rem > 0 ? rem : 0; - } - - - public void position(int pos) - { - _position = pos; - } - - public int position() - { - return _position; - } - - public Span ReadBytes(int length) - { - var nextPos = _position + length; - (nextPos, _position) = (_position, nextPos); - - return _bytes.AsSpan(nextPos, length); - } - - public byte get() - { - var span = ReadBytes(1); - return span[0]; - } - - public short getShort() - { - var span = ReadBytes(2); - if (_order == ByteOrder.BIG_ENDIAN) + public ByteBuffer(byte[] bytes) { - return BinaryPrimitives.ReadInt16BigEndian(span); - } - else - { - return BinaryPrimitives.ReadInt16LittleEndian(span); - } - } + _order = BitConverter.IsLittleEndian + ? ByteOrder.LITTLE_ENDIAN + : ByteOrder.BIG_ENDIAN; - - public int getInt() - { - var span = ReadBytes(4); - if (_order == ByteOrder.BIG_ENDIAN) - { - return BinaryPrimitives.ReadInt32BigEndian(span); - } - else - { - return BinaryPrimitives.ReadInt32LittleEndian(span); - } - } - - public float getFloat() - { - var span = ReadBytes(4); - if (_order == ByteOrder.BIG_ENDIAN && BitConverter.IsLittleEndian) - { - span.Reverse(); - } - else if (_order == ByteOrder.LITTLE_ENDIAN && !BitConverter.IsLittleEndian) - { - span.Reverse(); + _bytes = bytes; + _position = 0; } - return BitConverter.ToSingle(span); - } - - public long getLong() - { - var span = ReadBytes(8); - if (_order == ByteOrder.BIG_ENDIAN) + public ByteOrder order() { - return BinaryPrimitives.ReadInt64BigEndian(span); + return _order; } - else + + public void order(ByteOrder order) { - return BinaryPrimitives.ReadInt64LittleEndian(span); + _order = order; + } + + public int limit() + { + return _bytes.Length - _position; + } + + public int remaining() + { + int rem = limit(); + return rem > 0 ? rem : 0; + } + + + public void position(int pos) + { + _position = pos; + } + + public int position() + { + return _position; + } + + public Span ReadBytes(int length) + { + var nextPos = _position + length; + (nextPos, _position) = (_position, nextPos); + + return _bytes.AsSpan(nextPos, length); + } + + public byte get() + { + var span = ReadBytes(1); + return span[0]; + } + + public short getShort() + { + var span = ReadBytes(2); + if (_order == ByteOrder.BIG_ENDIAN) + { + return BinaryPrimitives.ReadInt16BigEndian(span); + } + else + { + return BinaryPrimitives.ReadInt16LittleEndian(span); + } + } + + + public int getInt() + { + var span = ReadBytes(4); + if (_order == ByteOrder.BIG_ENDIAN) + { + return BinaryPrimitives.ReadInt32BigEndian(span); + } + else + { + return BinaryPrimitives.ReadInt32LittleEndian(span); + } + } + + public float getFloat() + { + var span = ReadBytes(4); + if (_order == ByteOrder.BIG_ENDIAN && BitConverter.IsLittleEndian) + { + span.Reverse(); + } + else if (_order == ByteOrder.LITTLE_ENDIAN && !BitConverter.IsLittleEndian) + { + span.Reverse(); + } + + return BitConverter.ToSingle(span); + } + + public long getLong() + { + var span = ReadBytes(8); + if (_order == ByteOrder.BIG_ENDIAN) + { + return BinaryPrimitives.ReadInt64BigEndian(span); + } + else + { + return BinaryPrimitives.ReadInt64LittleEndian(span); + } + } + + public void putFloat(float v) + { + // ? + } + + public void putInt(int v) + { + // ? } } - - public void putFloat(float v) - { - // ? - } - - public void putInt(int v) - { - // ? - } -} } \ No newline at end of file diff --git a/src/DotRecast.Core/ByteOrder.cs b/src/DotRecast.Core/ByteOrder.cs index 47ccdc9..badbe4a 100644 --- a/src/DotRecast.Core/ByteOrder.cs +++ b/src/DotRecast.Core/ByteOrder.cs @@ -1,12 +1,9 @@ namespace DotRecast.Core { - - -public enum ByteOrder -{ - /// Default on most Windows systems - LITTLE_ENDIAN, - BIG_ENDIAN, -} - + public enum ByteOrder + { + /// Default on most Windows systems + LITTLE_ENDIAN, + BIG_ENDIAN, + } } \ No newline at end of file diff --git a/src/DotRecast.Core/CollectionExtensions.cs b/src/DotRecast.Core/CollectionExtensions.cs index 70052d1..b27e3fe 100644 --- a/src/DotRecast.Core/CollectionExtensions.cs +++ b/src/DotRecast.Core/CollectionExtensions.cs @@ -3,30 +3,28 @@ using System.Collections.Generic; namespace DotRecast.Core { - - -public static class CollectionExtensions -{ - public static void forEach(this IEnumerable collection, Action action) + public static class CollectionExtensions { - foreach (var item in collection) + public static void forEach(this IEnumerable collection, Action action) { - action.Invoke(item); + foreach (var item in collection) + { + action.Invoke(item); + } + } + + public static void Shuffle(this IList list) + { + Random random = new Random(); + int n = list.Count; + while (n > 1) + { + n--; + int k = random.Next(n + 1); + T value = list[k]; + list[k] = list[n]; + list[n] = value; + } } } - - public static void Shuffle(this IList list) - { - Random random = new Random(); - int n = list.Count; - while (n > 1) - { - n--; - int k = random.Next(n + 1); - T value = list[k]; - list[k] = list[n]; - list[n] = value; - } - } -} } \ No newline at end of file diff --git a/src/DotRecast.Core/ConvexUtils.cs b/src/DotRecast.Core/ConvexUtils.cs index 43b94ee..5de0ab1 100644 --- a/src/DotRecast.Core/ConvexUtils.cs +++ b/src/DotRecast.Core/ConvexUtils.cs @@ -20,70 +20,84 @@ using System.Collections.Generic; namespace DotRecast.Core { - - -public static class ConvexUtils { - - // Calculates convex hull on xz-plane of points on 'pts', - // stores the indices of the resulting hull in 'out' and - // returns number of points on hull. - public static List convexhull(List pts) { - int npts = pts.Count / 3; - List @out = new List(); - // Find lower-leftmost point. - int hull = 0; - for (int i = 1; i < npts; ++i) { - float[] a = new float[] { pts[i * 3], pts[i * 3 + 1], pts[i * 3 + 2] }; - float[] b = new float[] { pts[hull * 3], pts[hull * 3 + 1], pts[hull * 3 + 2] }; - if (cmppt(a, b)) { - hull = i; - } - } - // Gift wrap hull. - int endpt = 0; - do { - @out.Add(hull); - endpt = 0; - for (int j = 1; j < npts; ++j) { - float[] a = new float[] { pts[hull * 3], pts[hull * 3 + 1], pts[hull * 3 + 2] }; - float[] b = new float[] { pts[endpt * 3], pts[endpt * 3 + 1], pts[endpt * 3 + 2] }; - float[] c = new float[] { pts[j * 3], pts[j * 3 + 1], pts[j * 3 + 2] }; - if (hull == endpt || left(a, b, c)) { - endpt = j; + public static class ConvexUtils + { + // Calculates convex hull on xz-plane of points on 'pts', + // stores the indices of the resulting hull in 'out' and + // returns number of points on hull. + public static List convexhull(List pts) + { + int npts = pts.Count / 3; + List @out = new List(); + // Find lower-leftmost point. + int hull = 0; + for (int i = 1; i < npts; ++i) + { + float[] a = new float[] { pts[i * 3], pts[i * 3 + 1], pts[i * 3 + 2] }; + float[] b = new float[] { pts[hull * 3], pts[hull * 3 + 1], pts[hull * 3 + 2] }; + if (cmppt(a, b)) + { + hull = i; } } - hull = endpt; - } while (endpt != @out[0]); - return @out; - } + // Gift wrap hull. + int endpt = 0; + do + { + @out.Add(hull); + endpt = 0; + for (int j = 1; j < npts; ++j) + { + float[] a = new float[] { pts[hull * 3], pts[hull * 3 + 1], pts[hull * 3 + 2] }; + float[] b = new float[] { pts[endpt * 3], pts[endpt * 3 + 1], pts[endpt * 3 + 2] }; + float[] c = new float[] { pts[j * 3], pts[j * 3 + 1], pts[j * 3 + 2] }; + if (hull == endpt || left(a, b, c)) + { + endpt = j; + } + } - // Returns true if 'a' is more lower-left than 'b'. - private static bool cmppt(float[] a, float[] b) { - if (a[0] < b[0]) { - return true; + hull = endpt; + } while (endpt != @out[0]); + + return @out; } - if (a[0] > b[0]) { + + // Returns true if 'a' is more lower-left than 'b'. + private static bool cmppt(float[] a, float[] b) + { + if (a[0] < b[0]) + { + return true; + } + + if (a[0] > b[0]) + { + return false; + } + + if (a[2] < b[2]) + { + return true; + } + + if (a[2] > b[2]) + { + return false; + } + return false; } - if (a[2] < b[2]) { - return true; + + // Returns true if 'c' is left of line 'a'-'b'. + private static bool left(float[] a, float[] b, float[] c) + { + float u1 = b[0] - a[0]; + float v1 = b[2] - a[2]; + float u2 = c[0] - a[0]; + float v2 = c[2] - a[2]; + return u1 * v2 - v1 * u2 < 0; } - if (a[2] > b[2]) { - return false; - } - return false; } - - // Returns true if 'c' is left of line 'a'-'b'. - private static bool left(float[] a, float[] b, float[] c) { - float u1 = b[0] - a[0]; - float v1 = b[2] - a[2]; - float u2 = c[0] - a[0]; - float v2 = c[2] - a[2]; - return u1 * v2 - v1 * u2 < 0; - } - -} - } \ No newline at end of file diff --git a/src/DotRecast.Core/DemoMath.cs b/src/DotRecast.Core/DemoMath.cs index 3479c92..b803863 100644 --- a/src/DotRecast.Core/DemoMath.cs +++ b/src/DotRecast.Core/DemoMath.cs @@ -22,62 +22,72 @@ using System; namespace DotRecast.Core { - - -public class DemoMath { - public static float vDistSqr(float[] v1, float[] v2, int i) { - float dx = v2[i] - v1[0]; - float dy = v2[i + 1] - v1[1]; - float dz = v2[i + 2] - v1[2]; - return dx * dx + dy * dy + dz * dz; - } - - public static float[] vCross(float[] v1, float[] v2) { - float[] dest = new float[3]; - dest[0] = v1[1] * v2[2] - v1[2] * v2[1]; - dest[1] = v1[2] * v2[0] - v1[0] * v2[2]; - dest[2] = v1[0] * v2[1] - v1[1] * v2[0]; - return dest; - } - - public static float vDot(float[] v1, float[] v2) { - return v1[0] * v2[0] + v1[1] * v2[1] + v1[2] * v2[2]; - } - - public static float sqr(float f) { - return f * f; - } - - public static float getPathLen(float[] path, int npath) { - float totd = 0; - for (int i = 0; i < npath - 1; ++i) { - totd += (float)Math.Sqrt(vDistSqr(path, i * 3, (i + 1) * 3)); + public class DemoMath + { + public static float vDistSqr(float[] v1, float[] v2, int i) + { + float dx = v2[i] - v1[0]; + float dy = v2[i + 1] - v1[1]; + float dz = v2[i + 2] - v1[2]; + return dx * dx + dy * dy + dz * dz; } - return totd; - } - public static float vDistSqr(float[] v, int i, int j) { - float dx = v[i] - v[j]; - float dy = v[i + 1] - v[j + 1]; - float dz = v[i + 2] - v[j + 2]; - return dx * dx + dy * dy + dz * dz; - } + public static float[] vCross(float[] v1, float[] v2) + { + float[] dest = new float[3]; + dest[0] = v1[1] * v2[2] - v1[2] * v2[1]; + dest[1] = v1[2] * v2[0] - v1[0] * v2[2]; + dest[2] = v1[0] * v2[1] - v1[1] * v2[0]; + return dest; + } - public static float step(float threshold, float v) { - return v < threshold ? 0.0f : 1.0f; - } + public static float vDot(float[] v1, float[] v2) + { + return v1[0] * v2[0] + v1[1] * v2[1] + v1[2] * v2[2]; + } - public static int clamp(int v, int min, int max) { - return Math.Max(Math.Min(v, max), min); - } + public static float sqr(float f) + { + return f * f; + } - public static float clamp(float v, float min, float max) { - return Math.Max(Math.Min(v, max), min); - } + public static float getPathLen(float[] path, int npath) + { + float totd = 0; + for (int i = 0; i < npath - 1; ++i) + { + totd += (float)Math.Sqrt(vDistSqr(path, i * 3, (i + 1) * 3)); + } - public static float lerp(float f, float g, float u) { - return u * g + (1f - u) * f; - } -} + return totd; + } + public static float vDistSqr(float[] v, int i, int j) + { + float dx = v[i] - v[j]; + float dy = v[i + 1] - v[j + 1]; + float dz = v[i + 2] - v[j + 2]; + return dx * dx + dy * dy + dz * dz; + } + + public static float step(float threshold, float v) + { + return v < threshold ? 0.0f : 1.0f; + } + + public static int clamp(int v, int min, int max) + { + return Math.Max(Math.Min(v, max), min); + } + + public static float clamp(float v, float min, float max) + { + return Math.Max(Math.Min(v, max), min); + } + + public static float lerp(float f, float g, float u) + { + return u * g + (1f - u) * f; + } + } } \ No newline at end of file diff --git a/src/DotRecast.Core/DotRecast.Core.csproj b/src/DotRecast.Core/DotRecast.Core.csproj index c44e729..12c0675 100644 --- a/src/DotRecast.Core/DotRecast.Core.csproj +++ b/src/DotRecast.Core/DotRecast.Core.csproj @@ -3,10 +3,10 @@ netstandard2.1 - + - - + + diff --git a/src/DotRecast.Core/Loader.cs b/src/DotRecast.Core/Loader.cs index c9e8da0..b9589ea 100644 --- a/src/DotRecast.Core/Loader.cs +++ b/src/DotRecast.Core/Loader.cs @@ -2,34 +2,32 @@ namespace DotRecast.Core { - - -public static class Loader -{ - public static byte[] ToBytes(string filename) + public static class Loader { - var filepath = ToRPath(filename); - using var fs = new FileStream(filepath, FileMode.Open); - byte[] buffer = new byte[fs.Length]; - fs.Read(buffer, 0, buffer.Length); - - return buffer; - } - - public static string ToRPath(string filename) - { - string filePath = Path.Combine("resources", filename); - for (int i = 0; i < 10; ++i) + public static byte[] ToBytes(string filename) { - if (File.Exists(filePath)) - { - return Path.GetFullPath(filePath); - } + var filepath = ToRPath(filename); + using var fs = new FileStream(filepath, FileMode.Open); + byte[] buffer = new byte[fs.Length]; + fs.Read(buffer, 0, buffer.Length); - filePath = Path.Combine("..", filePath); + return buffer; } - return filename; + public static string ToRPath(string filename) + { + string filePath = Path.Combine("resources", filename); + for (int i = 0; i < 10; ++i) + { + if (File.Exists(filePath)) + { + return Path.GetFullPath(filePath); + } + + filePath = Path.Combine("..", filePath); + } + + return filename; + } } -} } \ No newline at end of file diff --git a/src/DotRecast.Core/NodeQueue.cs b/src/DotRecast.Core/NodeQueue.cs index 469d461..2874741 100644 --- a/src/DotRecast.Core/NodeQueue.cs +++ b/src/DotRecast.Core/NodeQueue.cs @@ -22,56 +22,55 @@ using System; namespace DotRecast.Core { + using System.Collections.Generic; - -using System.Collections.Generic; - -public class OrderedQueue -{ - - private readonly List _items; - private readonly Comparison _comparison; - - public OrderedQueue(Comparison comparison) + public class OrderedQueue { - _items = new List(); - _comparison = comparison; - } + private readonly List _items; + private readonly Comparison _comparison; - public int count() - { - return _items.Count; - } + public OrderedQueue(Comparison comparison) + { + _items = new List(); + _comparison = comparison; + } - public void clear() { - _items.Clear(); - } + public int count() + { + return _items.Count; + } - public T top() - { - return _items[0]; - } + public void clear() + { + _items.Clear(); + } - public T Dequeue() - { - var node = top(); - _items.Remove(node); - return node; - } + public T top() + { + return _items[0]; + } - public void Enqueue(T item) { - _items.Add(item); - _items.Sort(_comparison); - } + public T Dequeue() + { + var node = top(); + _items.Remove(node); + return node; + } - public void Remove(T item) { - _items.Remove(item); - } + public void Enqueue(T item) + { + _items.Add(item); + _items.Sort(_comparison); + } - public bool isEmpty() - { - return 0 == _items.Count; - } -} + public void Remove(T item) + { + _items.Remove(item); + } + public bool isEmpty() + { + return 0 == _items.Count; + } + } } \ No newline at end of file diff --git a/src/DotRecast.Detour.Crowd/Crowd.cs b/src/DotRecast.Detour.Crowd/Crowd.cs index c8d8de9..739bf1d 100644 --- a/src/DotRecast.Detour.Crowd/Crowd.cs +++ b/src/DotRecast.Detour.Crowd/Crowd.cs @@ -27,11 +27,9 @@ using DotRecast.Detour.Crowd.Tracking; namespace DotRecast.Detour.Crowd { + using static DetourCommon; - -using static DetourCommon; - -/** + /** * Members in this module implement local steering and dynamic avoidance features. * * The crowd is the big beast of the navigation features. It not only handles a lot of the path management for you, but @@ -104,122 +102,132 @@ using static DetourCommon; * steering in tight spaces. * */ -/** - * This is the core class of the refs crowd module. See the refs crowd documentation for a summary of the crowd - * features. A common method for setting up the crowd is as follows: -# Allocate the crowd -# Set the avoidance - * configurations using #setObstacleAvoidanceParams(). -# Add agents using #addAgent() and make an initial movement - * request using #requestMoveTarget(). A common process for managing the crowd is as follows: -# Call #update() to allow - * the crowd to manage its agents. -# Retrieve agent information using #getActiveAgents(). -# Make movement requests - * using #requestMoveTarget() when movement goal changes. -# Repeat every frame. Some agent configuration settings can - * be updated using #updateAgentParameters(). But the crowd owns the agent position. So it is not possible to update an - * active agent's position. If agent position must be fed back into the crowd, the agent must be removed and re-added. - * Notes: - Path related information is available for newly added agents only after an #update() has been performed. - - * Agent objects are kept in a pool and re-used. So it is important when using agent objects to check the value of - * #dtCrowdAgent::active to determine if the agent is actually in use or not. - This class is meant to provide 'local' - * movement. There is a limit of 256 polygons in the path corridor. So it is not meant to provide automatic pathfinding - * services over long distances. - * - * @see dtAllocCrowd(), dtFreeCrowd(), init(), dtCrowdAgent - */ -public class Crowd { - - /// The maximum number of corners a crowd agent will look ahead in the path. - /// This value is used for sizing the crowd agent corner buffers. - /// Due to the behavior of the crowd manager, the actual number of useful - /// corners will be one less than this number. - /// @ingroup crowd - public const int DT_CROWDAGENT_MAX_CORNERS = 4; - - /// The maximum number of crowd avoidance configurations supported by the - /// crowd manager. - /// @ingroup crowd - /// @see dtObstacleAvoidanceParams, dtCrowd::setObstacleAvoidanceParams(), dtCrowd::getObstacleAvoidanceParams(), - /// dtCrowdAgentParams::obstacleAvoidanceType - public const int DT_CROWD_MAX_OBSTAVOIDANCE_PARAMS = 8; - - /// The maximum number of query filter types supported by the crowd manager. - /// @ingroup crowd - /// @see dtQueryFilter, dtCrowd::getFilter() dtCrowd::getEditableFilter(), - /// dtCrowdAgentParams::queryFilterType - public const int DT_CROWD_MAX_QUERY_FILTER_TYPE = 16; - - private readonly AtomicInteger agentId = new AtomicInteger(); - private readonly HashSet m_agents; - private readonly PathQueue m_pathq; - private readonly ObstacleAvoidanceQuery.ObstacleAvoidanceParams[] m_obstacleQueryParams = new ObstacleAvoidanceQuery.ObstacleAvoidanceParams[DT_CROWD_MAX_OBSTAVOIDANCE_PARAMS]; - private readonly ObstacleAvoidanceQuery m_obstacleQuery; - private ProximityGrid m_grid; - private readonly float[] m_ext = new float[3]; - private readonly QueryFilter[] m_filters = new QueryFilter[DT_CROWD_MAX_QUERY_FILTER_TYPE]; - private NavMeshQuery navQuery; - private NavMesh navMesh; - private readonly CrowdConfig _config; - private readonly CrowdTelemetry _telemetry = new CrowdTelemetry(); - int m_velocitySampleCount; - - public Crowd(CrowdConfig config, NavMesh nav) : - this(config, nav, i => new DefaultQueryFilter()) - { - } - - public Crowd(CrowdConfig config, NavMesh nav, Func queryFilterFactory) { - - _config = config; - vSet(m_ext, config.maxAgentRadius * 2.0f, config.maxAgentRadius * 1.5f, config.maxAgentRadius * 2.0f); - - m_obstacleQuery = new ObstacleAvoidanceQuery(config.maxObstacleAvoidanceCircles, config.maxObstacleAvoidanceSegments); - - for (int i = 0; i < DT_CROWD_MAX_QUERY_FILTER_TYPE; i++) { - m_filters[i] = queryFilterFactory.Invoke(i); - } - // Init obstacle query option. - for (int i = 0; i < DT_CROWD_MAX_OBSTAVOIDANCE_PARAMS; ++i) { - m_obstacleQueryParams[i] = new ObstacleAvoidanceQuery.ObstacleAvoidanceParams(); - } - - // Allocate temp buffer for merging paths. - m_pathq = new PathQueue(config); - m_agents = new HashSet(); - - // The navQuery is mostly used for local searches, no need for large node pool. - navMesh = nav; - navQuery = new NavMeshQuery(nav); - } - - public void setNavMesh(NavMesh nav) { - navMesh = nav; - navQuery = new NavMeshQuery(nav); - } - - /// Sets the shared avoidance configuration for the specified index. - /// @param[in] idx The index. [Limits: 0 <= value < - /// #DT_CROWD_MAX_OBSTAVOIDANCE_PARAMS] - /// @param[in] option The new configuration. - public void setObstacleAvoidanceParams(int idx, ObstacleAvoidanceQuery.ObstacleAvoidanceParams option) { - if (idx >= 0 && idx < DT_CROWD_MAX_OBSTAVOIDANCE_PARAMS) { - m_obstacleQueryParams[idx] = new ObstacleAvoidanceQuery.ObstacleAvoidanceParams(option); - } - } - - /// Gets the shared avoidance configuration for the specified index. - /// @param[in] idx The index of the configuration to retreive. - /// [Limits: 0 <= value < #DT_CROWD_MAX_OBSTAVOIDANCE_PARAMS] - /// @return The requested configuration. - public ObstacleAvoidanceQuery.ObstacleAvoidanceParams getObstacleAvoidanceParams(int idx) { - if (idx >= 0 && idx < DT_CROWD_MAX_OBSTAVOIDANCE_PARAMS) { - return m_obstacleQueryParams[idx]; - } - return null; - } - - /// Updates the specified agent's configuration. - /// @param[in] idx The agent index. [Limits: 0 <= value < #getAgentCount()] - /// @param[in] params The new agent configuration. - public void updateAgentParameters(CrowdAgent agent, CrowdAgentParams option) { - agent.option = option; - } - /** + * This is the core class of the refs crowd module. See the refs crowd documentation for a summary of the crowd + * features. A common method for setting up the crowd is as follows: -# Allocate the crowd -# Set the avoidance + * configurations using #setObstacleAvoidanceParams(). -# Add agents using #addAgent() and make an initial movement + * request using #requestMoveTarget(). A common process for managing the crowd is as follows: -# Call #update() to allow + * the crowd to manage its agents. -# Retrieve agent information using #getActiveAgents(). -# Make movement requests + * using #requestMoveTarget() when movement goal changes. -# Repeat every frame. Some agent configuration settings can + * be updated using #updateAgentParameters(). But the crowd owns the agent position. So it is not possible to update an + * active agent's position. If agent position must be fed back into the crowd, the agent must be removed and re-added. + * Notes: - Path related information is available for newly added agents only after an #update() has been performed. - + * Agent objects are kept in a pool and re-used. So it is important when using agent objects to check the value of + * #dtCrowdAgent::active to determine if the agent is actually in use or not. - This class is meant to provide 'local' + * movement. There is a limit of 256 polygons in the path corridor. So it is not meant to provide automatic pathfinding + * services over long distances. + * + * @see dtAllocCrowd(), dtFreeCrowd(), init(), dtCrowdAgent + */ + public class Crowd + { + /// The maximum number of corners a crowd agent will look ahead in the path. + /// This value is used for sizing the crowd agent corner buffers. + /// Due to the behavior of the crowd manager, the actual number of useful + /// corners will be one less than this number. + /// @ingroup crowd + public const int DT_CROWDAGENT_MAX_CORNERS = 4; + + /// The maximum number of crowd avoidance configurations supported by the + /// crowd manager. + /// @ingroup crowd + /// @see dtObstacleAvoidanceParams, dtCrowd::setObstacleAvoidanceParams(), dtCrowd::getObstacleAvoidanceParams(), + /// dtCrowdAgentParams::obstacleAvoidanceType + public const int DT_CROWD_MAX_OBSTAVOIDANCE_PARAMS = 8; + + /// The maximum number of query filter types supported by the crowd manager. + /// @ingroup crowd + /// @see dtQueryFilter, dtCrowd::getFilter() dtCrowd::getEditableFilter(), + /// dtCrowdAgentParams::queryFilterType + public const int DT_CROWD_MAX_QUERY_FILTER_TYPE = 16; + + private readonly AtomicInteger agentId = new AtomicInteger(); + private readonly HashSet m_agents; + private readonly PathQueue m_pathq; + private readonly ObstacleAvoidanceQuery.ObstacleAvoidanceParams[] m_obstacleQueryParams = new ObstacleAvoidanceQuery.ObstacleAvoidanceParams[DT_CROWD_MAX_OBSTAVOIDANCE_PARAMS]; + private readonly ObstacleAvoidanceQuery m_obstacleQuery; + private ProximityGrid m_grid; + private readonly float[] m_ext = new float[3]; + private readonly QueryFilter[] m_filters = new QueryFilter[DT_CROWD_MAX_QUERY_FILTER_TYPE]; + private NavMeshQuery navQuery; + private NavMesh navMesh; + private readonly CrowdConfig _config; + private readonly CrowdTelemetry _telemetry = new CrowdTelemetry(); + int m_velocitySampleCount; + + public Crowd(CrowdConfig config, NavMesh nav) : + this(config, nav, i => new DefaultQueryFilter()) + { + } + + public Crowd(CrowdConfig config, NavMesh nav, Func queryFilterFactory) + { + _config = config; + vSet(m_ext, config.maxAgentRadius * 2.0f, config.maxAgentRadius * 1.5f, config.maxAgentRadius * 2.0f); + + m_obstacleQuery = new ObstacleAvoidanceQuery(config.maxObstacleAvoidanceCircles, config.maxObstacleAvoidanceSegments); + + for (int i = 0; i < DT_CROWD_MAX_QUERY_FILTER_TYPE; i++) + { + m_filters[i] = queryFilterFactory.Invoke(i); + } + + // Init obstacle query option. + for (int i = 0; i < DT_CROWD_MAX_OBSTAVOIDANCE_PARAMS; ++i) + { + m_obstacleQueryParams[i] = new ObstacleAvoidanceQuery.ObstacleAvoidanceParams(); + } + + // Allocate temp buffer for merging paths. + m_pathq = new PathQueue(config); + m_agents = new HashSet(); + + // The navQuery is mostly used for local searches, no need for large node pool. + navMesh = nav; + navQuery = new NavMeshQuery(nav); + } + + public void setNavMesh(NavMesh nav) + { + navMesh = nav; + navQuery = new NavMeshQuery(nav); + } + + /// Sets the shared avoidance configuration for the specified index. + /// @param[in] idx The index. [Limits: 0 <= value < + /// #DT_CROWD_MAX_OBSTAVOIDANCE_PARAMS] + /// @param[in] option The new configuration. + public void setObstacleAvoidanceParams(int idx, ObstacleAvoidanceQuery.ObstacleAvoidanceParams option) + { + if (idx >= 0 && idx < DT_CROWD_MAX_OBSTAVOIDANCE_PARAMS) + { + m_obstacleQueryParams[idx] = new ObstacleAvoidanceQuery.ObstacleAvoidanceParams(option); + } + } + + /// Gets the shared avoidance configuration for the specified index. + /// @param[in] idx The index of the configuration to retreive. + /// [Limits: 0 <= value < #DT_CROWD_MAX_OBSTAVOIDANCE_PARAMS] + /// @return The requested configuration. + public ObstacleAvoidanceQuery.ObstacleAvoidanceParams getObstacleAvoidanceParams(int idx) + { + if (idx >= 0 && idx < DT_CROWD_MAX_OBSTAVOIDANCE_PARAMS) + { + return m_obstacleQueryParams[idx]; + } + + return null; + } + + /// Updates the specified agent's configuration. + /// @param[in] idx The agent index. [Limits: 0 <= value < #getAgentCount()] + /// @param[in] params The new agent configuration. + public void updateAgentParameters(CrowdAgent agent, CrowdAgentParams option) + { + agent.option = option; + } + + /** * Adds a new agent to the crowd. * * @param pos @@ -228,949 +236,1159 @@ public class Crowd { * The configutation of the agent. * @return The newly created agent object */ - public CrowdAgent addAgent(float[] pos, CrowdAgentParams option) { - CrowdAgent ag = new CrowdAgent(agentId.GetAndIncrement()); - m_agents.Add(ag); - updateAgentParameters(ag, option); + public CrowdAgent addAgent(float[] pos, CrowdAgentParams option) + { + CrowdAgent ag = new CrowdAgent(agentId.GetAndIncrement()); + m_agents.Add(ag); + updateAgentParameters(ag, option); - // Find nearest position on navmesh and place the agent there. - Result nearestPoly = navQuery.findNearestPoly(pos, m_ext, m_filters[ag.option.queryFilterType]); + // Find nearest position on navmesh and place the agent there. + Result nearestPoly = navQuery.findNearestPoly(pos, m_ext, m_filters[ag.option.queryFilterType]); - float[] nearest = nearestPoly.succeeded() ? nearestPoly.result.getNearestPos() : pos; - long refs = nearestPoly.succeeded() ? nearestPoly.result.getNearestRef() : 0L; - ag.corridor.reset(refs, nearest); - ag.boundary.reset(); - ag.partial = false; + float[] nearest = nearestPoly.succeeded() ? nearestPoly.result.getNearestPos() : pos; + long refs = nearestPoly.succeeded() ? nearestPoly.result.getNearestRef() : 0L; + ag.corridor.reset(refs, nearest); + ag.boundary.reset(); + ag.partial = false; - ag.topologyOptTime = 0; - ag.targetReplanTime = 0; + ag.topologyOptTime = 0; + ag.targetReplanTime = 0; - vSet(ag.dvel, 0, 0, 0); - vSet(ag.nvel, 0, 0, 0); - vSet(ag.vel, 0, 0, 0); - vCopy(ag.npos, nearest); + vSet(ag.dvel, 0, 0, 0); + vSet(ag.nvel, 0, 0, 0); + vSet(ag.vel, 0, 0, 0); + vCopy(ag.npos, nearest); - ag.desiredSpeed = 0; + ag.desiredSpeed = 0; - if (refs != 0) { - ag.state = CrowdAgent.CrowdAgentState.DT_CROWDAGENT_STATE_WALKING; - } else { - ag.state = CrowdAgent.CrowdAgentState.DT_CROWDAGENT_STATE_INVALID; + if (refs != 0) + { + ag.state = CrowdAgent.CrowdAgentState.DT_CROWDAGENT_STATE_WALKING; + } + else + { + ag.state = CrowdAgent.CrowdAgentState.DT_CROWDAGENT_STATE_INVALID; + } + + ag.targetState = CrowdAgent.MoveRequestState.DT_CROWDAGENT_TARGET_NONE; + + return ag; } - ag.targetState = CrowdAgent.MoveRequestState.DT_CROWDAGENT_TARGET_NONE; - - return ag; - } - - /** + /** * Removes the agent from the crowd. * * @param agent * Agent to be removed */ - public void removeAgent(CrowdAgent agent) { - m_agents.Remove(agent); - } - - private bool requestMoveTargetReplan(CrowdAgent ag, long refs, float[] pos) { - ag.setTarget(refs, pos); - ag.targetReplan = true; - return true; - } - - /// Submits a new move request for the specified agent. - /// @param[in] idx The agent index. [Limits: 0 <= value < #getAgentCount()] - /// @param[in] ref The position's polygon reference. - /// @param[in] pos The position within the polygon. [(x, y, z)] - /// @return True if the request was successfully submitted. - /// - /// This method is used when a new target is set. - /// - /// The position will be constrained to the surface of the navigation mesh. - /// - /// The request will be processed during the next #update(). - public bool requestMoveTarget(CrowdAgent agent, long refs, float[] pos) { - if (refs == 0) { - return false; + public void removeAgent(CrowdAgent agent) + { + m_agents.Remove(agent); } - // Initialize request. - agent.setTarget(refs, pos); - agent.targetReplan = false; - return true; - } + private bool requestMoveTargetReplan(CrowdAgent ag, long refs, float[] pos) + { + ag.setTarget(refs, pos); + ag.targetReplan = true; + return true; + } - /// Submits a new move request for the specified agent. - /// @param[in] idx The agent index. [Limits: 0 <= value < #getAgentCount()] - /// @param[in] vel The movement velocity. [(x, y, z)] - /// @return True if the request was successfully submitted. - public bool requestMoveVelocity(CrowdAgent agent, float[] vel) { - // Initialize request. - agent.targetRef = 0; - vCopy(agent.targetPos, vel); - agent.targetPathQueryResult = null; - agent.targetReplan = false; - agent.targetState = CrowdAgent.MoveRequestState.DT_CROWDAGENT_TARGET_VELOCITY; + /// Submits a new move request for the specified agent. + /// @param[in] idx The agent index. [Limits: 0 <= value < #getAgentCount()] + /// @param[in] ref The position's polygon reference. + /// @param[in] pos The position within the polygon. [(x, y, z)] + /// @return True if the request was successfully submitted. + /// + /// This method is used when a new target is set. + /// + /// The position will be constrained to the surface of the navigation mesh. + /// + /// The request will be processed during the next #update(). + public bool requestMoveTarget(CrowdAgent agent, long refs, float[] pos) + { + if (refs == 0) + { + return false; + } - return true; - } + // Initialize request. + agent.setTarget(refs, pos); + agent.targetReplan = false; + return true; + } - /// Resets any request for the specified agent. - /// @param[in] idx The agent index. [Limits: 0 <= value < #getAgentCount()] - /// @return True if the request was successfully reseted. - public bool resetMoveTarget(CrowdAgent agent) { - // Initialize request. - agent.targetRef = 0; - vSet(agent.targetPos, 0, 0, 0); - vSet(agent.dvel, 0, 0, 0); - agent.targetPathQueryResult = null; - agent.targetReplan = false; - agent.targetState = CrowdAgent.MoveRequestState.DT_CROWDAGENT_TARGET_NONE; - return true; - } + /// Submits a new move request for the specified agent. + /// @param[in] idx The agent index. [Limits: 0 <= value < #getAgentCount()] + /// @param[in] vel The movement velocity. [(x, y, z)] + /// @return True if the request was successfully submitted. + public bool requestMoveVelocity(CrowdAgent agent, float[] vel) + { + // Initialize request. + agent.targetRef = 0; + vCopy(agent.targetPos, vel); + agent.targetPathQueryResult = null; + agent.targetReplan = false; + agent.targetState = CrowdAgent.MoveRequestState.DT_CROWDAGENT_TARGET_VELOCITY; - /** + return true; + } + + /// Resets any request for the specified agent. + /// @param[in] idx The agent index. [Limits: 0 <= value < #getAgentCount()] + /// @return True if the request was successfully reseted. + public bool resetMoveTarget(CrowdAgent agent) + { + // Initialize request. + agent.targetRef = 0; + vSet(agent.targetPos, 0, 0, 0); + vSet(agent.dvel, 0, 0, 0); + agent.targetPathQueryResult = null; + agent.targetReplan = false; + agent.targetState = CrowdAgent.MoveRequestState.DT_CROWDAGENT_TARGET_NONE; + return true; + } + + /** * Gets the active agents int the agent pool. * * @return List of active agents */ - public List getActiveAgents() { - return new List(m_agents); - } + public List getActiveAgents() + { + return new List(m_agents); + } - public float[] getQueryExtents() { - return m_ext; - } + public float[] getQueryExtents() + { + return m_ext; + } - public QueryFilter getFilter(int i) { - return i >= 0 && i < DT_CROWD_MAX_QUERY_FILTER_TYPE ? m_filters[i] : null; - } + public QueryFilter getFilter(int i) + { + return i >= 0 && i < DT_CROWD_MAX_QUERY_FILTER_TYPE ? m_filters[i] : null; + } - public ProximityGrid getGrid() { - return m_grid; - } + public ProximityGrid getGrid() + { + return m_grid; + } - public PathQueue getPathQueue() { - return m_pathq; - } + public PathQueue getPathQueue() + { + return m_pathq; + } - public CrowdTelemetry telemetry() { - return _telemetry; - } + public CrowdTelemetry telemetry() + { + return _telemetry; + } - public CrowdConfig config() { - return _config; - } + public CrowdConfig config() + { + return _config; + } - public CrowdTelemetry update(float dt, CrowdAgentDebugInfo debug) { - m_velocitySampleCount = 0; + public CrowdTelemetry update(float dt, CrowdAgentDebugInfo debug) + { + m_velocitySampleCount = 0; - _telemetry.start(); + _telemetry.start(); - ICollection agents = getActiveAgents(); + ICollection agents = getActiveAgents(); - // Check that all agents still have valid paths. - checkPathValidity(agents, dt); + // Check that all agents still have valid paths. + checkPathValidity(agents, dt); - // Update async move request and path finder. - updateMoveRequest(agents, dt); + // Update async move request and path finder. + updateMoveRequest(agents, dt); - // Optimize path topology. - updateTopologyOptimization(agents, dt); + // Optimize path topology. + updateTopologyOptimization(agents, dt); - // Register agents to proximity grid. - buildProximityGrid(agents); + // Register agents to proximity grid. + buildProximityGrid(agents); - // Get nearby navmesh segments and agents to collide with. - buildNeighbours(agents); + // Get nearby navmesh segments and agents to collide with. + buildNeighbours(agents); - // Find next corner to steer to. - findCorners(agents, debug); + // Find next corner to steer to. + findCorners(agents, debug); - // Trigger off-mesh connections (depends on corners). - triggerOffMeshConnections(agents); + // Trigger off-mesh connections (depends on corners). + triggerOffMeshConnections(agents); - // Calculate steering. - calculateSteering(agents); + // Calculate steering. + calculateSteering(agents); - // Velocity planning. - planVelocity(debug, agents); + // Velocity planning. + planVelocity(debug, agents); - // Integrate. - integrate(dt, agents); + // Integrate. + integrate(dt, agents); - // Handle collisions. - handleCollisions(agents); + // Handle collisions. + handleCollisions(agents); - moveAgents(agents); + moveAgents(agents); - // Update agents using off-mesh connection. - updateOffMeshConnections(agents, dt); - return _telemetry; - } + // Update agents using off-mesh connection. + updateOffMeshConnections(agents, dt); + return _telemetry; + } - private void checkPathValidity(ICollection agents, float dt) { - _telemetry.start("checkPathValidity"); + private void checkPathValidity(ICollection agents, float dt) + { + _telemetry.start("checkPathValidity"); - foreach (CrowdAgent ag in agents) { - - if (ag.state != CrowdAgent.CrowdAgentState.DT_CROWDAGENT_STATE_WALKING) { - continue; - } - - ag.targetReplanTime += dt; - - bool replan = false; - - // First check that the current location is valid. - float[] agentPos = new float[3]; - long agentRef = ag.corridor.getFirstPoly(); - vCopy(agentPos, ag.npos); - if (!navQuery.isValidPolyRef(agentRef, m_filters[ag.option.queryFilterType])) { - // Current location is not valid, try to reposition. - // TODO: this can snap agents, how to handle that? - Result nearestPoly = navQuery.findNearestPoly(ag.npos, m_ext, - m_filters[ag.option.queryFilterType]); - agentRef = nearestPoly.succeeded() ? nearestPoly.result.getNearestRef() : 0L; - if (nearestPoly.succeeded()) { - vCopy(agentPos, nearestPoly.result.getNearestPos()); - } - - if (agentRef == 0) { - // Could not find location in navmesh, set state to invalid. - ag.corridor.reset(0, agentPos); - ag.partial = false; - ag.boundary.reset(); - ag.state = CrowdAgent.CrowdAgentState.DT_CROWDAGENT_STATE_INVALID; + foreach (CrowdAgent ag in agents) + { + if (ag.state != CrowdAgent.CrowdAgentState.DT_CROWDAGENT_STATE_WALKING) + { continue; } - // Make sure the first polygon is valid, but leave other valid - // polygons in the path so that replanner can adjust the path - // better. - ag.corridor.fixPathStart(agentRef, agentPos); - // ag.corridor.trimInvalidPath(agentRef, agentPos, m_navquery, - // &m_filter); - ag.boundary.reset(); - vCopy(ag.npos, agentPos); + ag.targetReplanTime += dt; - replan = true; - } + bool replan = false; - // If the agent does not have move target or is controlled by - // velocity, no need to recover the target nor replan. - if (ag.targetState == CrowdAgent.MoveRequestState.DT_CROWDAGENT_TARGET_NONE - || ag.targetState == CrowdAgent.MoveRequestState.DT_CROWDAGENT_TARGET_VELOCITY) { - continue; - } - - // Try to recover move request position. - if (ag.targetState != CrowdAgent.MoveRequestState.DT_CROWDAGENT_TARGET_NONE - && ag.targetState != CrowdAgent.MoveRequestState.DT_CROWDAGENT_TARGET_FAILED) { - if (!navQuery.isValidPolyRef(ag.targetRef, m_filters[ag.option.queryFilterType])) { - // Current target is not valid, try to reposition. - Result fnp = navQuery.findNearestPoly(ag.targetPos, m_ext, - m_filters[ag.option.queryFilterType]); - ag.targetRef = fnp.succeeded() ? fnp.result.getNearestRef() : 0L; - if (fnp.succeeded()) { - vCopy(ag.targetPos, fnp.result.getNearestPos()); - } - replan = true; - } - if (ag.targetRef == 0) { - // Failed to reposition target, fail moverequest. - ag.corridor.reset(agentRef, agentPos); - ag.partial = false; - ag.targetState = CrowdAgent.MoveRequestState.DT_CROWDAGENT_TARGET_NONE; - } - } - - // If nearby corridor is not valid, replan. - if (!ag.corridor.isValid(_config.checkLookAhead, navQuery, m_filters[ag.option.queryFilterType])) { - // Fix current path. - // ag.corridor.trimInvalidPath(agentRef, agentPos, m_navquery, - // &m_filter); - // ag.boundary.reset(); - replan = true; - } - - // If the end of the path is near and it is not the requested - // location, replan. - if (ag.targetState == CrowdAgent.MoveRequestState.DT_CROWDAGENT_TARGET_VALID) { - if (ag.targetReplanTime > _config.targetReplanDelay && ag.corridor.getPathCount() < _config.checkLookAhead - && ag.corridor.getLastPoly() != ag.targetRef) { - replan = true; - } - } - - // Try to replan path to goal. - if (replan) { - if (ag.targetState != CrowdAgent.MoveRequestState.DT_CROWDAGENT_TARGET_NONE) { - requestMoveTargetReplan(ag, ag.targetRef, ag.targetPos); - } - } - } - _telemetry.stop("checkPathValidity"); - } - - private void updateMoveRequest(ICollection agents, float dt) { - _telemetry.start("updateMoveRequest"); - - OrderedQueue queue = new OrderedQueue((a1, a2) => a2.targetReplanTime.CompareTo(a1.targetReplanTime)); - - // Fire off new requests. - foreach (CrowdAgent ag in agents) { - if (ag.state == CrowdAgent.CrowdAgentState.DT_CROWDAGENT_STATE_INVALID) { - continue; - } - if (ag.targetState == CrowdAgent.MoveRequestState.DT_CROWDAGENT_TARGET_NONE - || ag.targetState == CrowdAgent.MoveRequestState.DT_CROWDAGENT_TARGET_VELOCITY) { - continue; - } - - if (ag.targetState == CrowdAgent.MoveRequestState.DT_CROWDAGENT_TARGET_REQUESTING) { - List path = ag.corridor.getPath(); - if (0 == path.Count) { - throw new ArgumentException("Empty path"); - } - // Quick search towards the goal. - navQuery.initSlicedFindPath(path[0], ag.targetRef, ag.npos, ag.targetPos, - m_filters[ag.option.queryFilterType], 0); - navQuery.updateSlicedFindPath(_config.maxTargetFindPathIterations); - Result> pathFound; - if (ag.targetReplan) // && npath > 10) + // First check that the current location is valid. + float[] agentPos = new float[3]; + long agentRef = ag.corridor.getFirstPoly(); + vCopy(agentPos, ag.npos); + if (!navQuery.isValidPolyRef(agentRef, m_filters[ag.option.queryFilterType])) { - // Try to use existing steady path during replan if - // possible. - pathFound = navQuery.finalizeSlicedFindPathPartial(path); - } else { - // Try to move towards target when goal changes. - pathFound = navQuery.finalizeSlicedFindPath(); + // Current location is not valid, try to reposition. + // TODO: this can snap agents, how to handle that? + Result nearestPoly = navQuery.findNearestPoly(ag.npos, m_ext, + m_filters[ag.option.queryFilterType]); + agentRef = nearestPoly.succeeded() ? nearestPoly.result.getNearestRef() : 0L; + if (nearestPoly.succeeded()) + { + vCopy(agentPos, nearestPoly.result.getNearestPos()); + } + + if (agentRef == 0) + { + // Could not find location in navmesh, set state to invalid. + ag.corridor.reset(0, agentPos); + ag.partial = false; + ag.boundary.reset(); + ag.state = CrowdAgent.CrowdAgentState.DT_CROWDAGENT_STATE_INVALID; + continue; + } + + // Make sure the first polygon is valid, but leave other valid + // polygons in the path so that replanner can adjust the path + // better. + ag.corridor.fixPathStart(agentRef, agentPos); + // ag.corridor.trimInvalidPath(agentRef, agentPos, m_navquery, + // &m_filter); + ag.boundary.reset(); + vCopy(ag.npos, agentPos); + + replan = true; } - List reqPath = pathFound.result; - float[] reqPos = new float[3]; - if (pathFound.succeeded() && reqPath.Count > 0) { - // In progress or succeed. - if (reqPath[reqPath.Count - 1] != ag.targetRef) { - // Partial path, constrain target position inside the - // last polygon. - Result cr = navQuery.closestPointOnPoly(reqPath[reqPath.Count - 1], - ag.targetPos); - if (cr.succeeded()) { - reqPos = cr.result.getClosest(); - } else { - reqPath = new List(); + + // If the agent does not have move target or is controlled by + // velocity, no need to recover the target nor replan. + if (ag.targetState == CrowdAgent.MoveRequestState.DT_CROWDAGENT_TARGET_NONE + || ag.targetState == CrowdAgent.MoveRequestState.DT_CROWDAGENT_TARGET_VELOCITY) + { + continue; + } + + // Try to recover move request position. + if (ag.targetState != CrowdAgent.MoveRequestState.DT_CROWDAGENT_TARGET_NONE + && ag.targetState != CrowdAgent.MoveRequestState.DT_CROWDAGENT_TARGET_FAILED) + { + if (!navQuery.isValidPolyRef(ag.targetRef, m_filters[ag.option.queryFilterType])) + { + // Current target is not valid, try to reposition. + Result fnp = navQuery.findNearestPoly(ag.targetPos, m_ext, + m_filters[ag.option.queryFilterType]); + ag.targetRef = fnp.succeeded() ? fnp.result.getNearestRef() : 0L; + if (fnp.succeeded()) + { + vCopy(ag.targetPos, fnp.result.getNearestPos()); } - } else { - vCopy(reqPos, ag.targetPos); + + replan = true; + } + + if (ag.targetRef == 0) + { + // Failed to reposition target, fail moverequest. + ag.corridor.reset(agentRef, agentPos); + ag.partial = false; + ag.targetState = CrowdAgent.MoveRequestState.DT_CROWDAGENT_TARGET_NONE; } - } else { - // Could not find path, start the request from current - // location. - vCopy(reqPos, ag.npos); - reqPath = new List(); - reqPath.Add(path[0]); } - ag.corridor.setCorridor(reqPos, reqPath); - ag.boundary.reset(); - ag.partial = false; - - if (reqPath[reqPath.Count - 1] == ag.targetRef) { - ag.targetState = CrowdAgent.MoveRequestState.DT_CROWDAGENT_TARGET_VALID; - ag.targetReplanTime = 0; - } else { - // The path is longer or potentially unreachable, full plan. - ag.targetState = CrowdAgent.MoveRequestState.DT_CROWDAGENT_TARGET_WAITING_FOR_QUEUE; + // If nearby corridor is not valid, replan. + if (!ag.corridor.isValid(_config.checkLookAhead, navQuery, m_filters[ag.option.queryFilterType])) + { + // Fix current path. + // ag.corridor.trimInvalidPath(agentRef, agentPos, m_navquery, + // &m_filter); + // ag.boundary.reset(); + replan = true; } - ag.targetReplanWaitTime = 0; - } - if (ag.targetState == CrowdAgent.MoveRequestState.DT_CROWDAGENT_TARGET_WAITING_FOR_QUEUE) { - queue.Enqueue(ag); - } - } - - while (!queue.isEmpty()) { - CrowdAgent ag = queue.Dequeue(); - ag.targetPathQueryResult = m_pathq.request(ag.corridor.getLastPoly(), ag.targetRef, ag.corridor.getTarget(), - ag.targetPos, m_filters[ag.option.queryFilterType]); - if (ag.targetPathQueryResult != null) { - ag.targetState = CrowdAgent.MoveRequestState.DT_CROWDAGENT_TARGET_WAITING_FOR_PATH; - } else { - _telemetry.recordMaxTimeToEnqueueRequest(ag.targetReplanWaitTime); - ag.targetReplanWaitTime += dt; - } - } - - // Update requests. - _telemetry.start("pathQueueUpdate"); - m_pathq.update(navMesh); - _telemetry.stop("pathQueueUpdate"); - - // Process path results. - foreach (CrowdAgent ag in agents) { - if (ag.targetState == CrowdAgent.MoveRequestState.DT_CROWDAGENT_TARGET_NONE - || ag.targetState == CrowdAgent.MoveRequestState.DT_CROWDAGENT_TARGET_VELOCITY) { - continue; - } - - if (ag.targetState == CrowdAgent.MoveRequestState.DT_CROWDAGENT_TARGET_WAITING_FOR_PATH) { - // _telemetry.recordPathWaitTime(ag.targetReplanTime); - // Poll path queue. - Status status = ag.targetPathQueryResult.status; - if (status != null && status.isFailed()) { - // Path find failed, retry if the target location is still - // valid. - ag.targetPathQueryResult = null; - if (ag.targetRef != 0) { - ag.targetState = CrowdAgent.MoveRequestState.DT_CROWDAGENT_TARGET_REQUESTING; - } else { - ag.targetState = CrowdAgent.MoveRequestState.DT_CROWDAGENT_TARGET_FAILED; + // If the end of the path is near and it is not the requested + // location, replan. + if (ag.targetState == CrowdAgent.MoveRequestState.DT_CROWDAGENT_TARGET_VALID) + { + if (ag.targetReplanTime > _config.targetReplanDelay && ag.corridor.getPathCount() < _config.checkLookAhead + && ag.corridor.getLastPoly() != ag.targetRef) + { + replan = true; } - ag.targetReplanTime = 0; - } else if (status != null && status.isSuccess()) { + } + + // Try to replan path to goal. + if (replan) + { + if (ag.targetState != CrowdAgent.MoveRequestState.DT_CROWDAGENT_TARGET_NONE) + { + requestMoveTargetReplan(ag, ag.targetRef, ag.targetPos); + } + } + } + + _telemetry.stop("checkPathValidity"); + } + + private void updateMoveRequest(ICollection agents, float dt) + { + _telemetry.start("updateMoveRequest"); + + OrderedQueue queue = new OrderedQueue((a1, a2) => a2.targetReplanTime.CompareTo(a1.targetReplanTime)); + + // Fire off new requests. + foreach (CrowdAgent ag in agents) + { + if (ag.state == CrowdAgent.CrowdAgentState.DT_CROWDAGENT_STATE_INVALID) + { + continue; + } + + if (ag.targetState == CrowdAgent.MoveRequestState.DT_CROWDAGENT_TARGET_NONE + || ag.targetState == CrowdAgent.MoveRequestState.DT_CROWDAGENT_TARGET_VELOCITY) + { + continue; + } + + if (ag.targetState == CrowdAgent.MoveRequestState.DT_CROWDAGENT_TARGET_REQUESTING) + { List path = ag.corridor.getPath(); - if (0 == path.Count) { + if (0 == path.Count) + { throw new ArgumentException("Empty path"); } - // Apply results. - float[] targetPos = ag.targetPos; - - bool valid = true; - List res = ag.targetPathQueryResult.path; - if (status.isFailed() || 0 == res.Count) { - valid = false; + // Quick search towards the goal. + navQuery.initSlicedFindPath(path[0], ag.targetRef, ag.npos, ag.targetPos, + m_filters[ag.option.queryFilterType], 0); + navQuery.updateSlicedFindPath(_config.maxTargetFindPathIterations); + Result> pathFound; + if (ag.targetReplan) // && npath > 10) + { + // Try to use existing steady path during replan if + // possible. + pathFound = navQuery.finalizeSlicedFindPathPartial(path); + } + else + { + // Try to move towards target when goal changes. + pathFound = navQuery.finalizeSlicedFindPath(); } - if (status.isPartial()) { - ag.partial = true; - } else { - ag.partial = false; + List reqPath = pathFound.result; + float[] reqPos = new float[3]; + if (pathFound.succeeded() && reqPath.Count > 0) + { + // In progress or succeed. + if (reqPath[reqPath.Count - 1] != ag.targetRef) + { + // Partial path, constrain target position inside the + // last polygon. + Result cr = navQuery.closestPointOnPoly(reqPath[reqPath.Count - 1], + ag.targetPos); + if (cr.succeeded()) + { + reqPos = cr.result.getClosest(); + } + else + { + reqPath = new List(); + } + } + else + { + vCopy(reqPos, ag.targetPos); + } + } + else + { + // Could not find path, start the request from current + // location. + vCopy(reqPos, ag.npos); + reqPath = new List(); + reqPath.Add(path[0]); } - // Merge result and existing path. - // The agent might have moved whilst the request is - // being processed, so the path may have changed. - // We assume that the end of the path is at the same - // location - // where the request was issued. + ag.corridor.setCorridor(reqPos, reqPath); + ag.boundary.reset(); + ag.partial = false; - // The last ref in the old path should be the same as - // the location where the request was issued.. - if (valid && path[path.Count - 1] != res[0]) { - valid = false; + if (reqPath[reqPath.Count - 1] == ag.targetRef) + { + ag.targetState = CrowdAgent.MoveRequestState.DT_CROWDAGENT_TARGET_VALID; + ag.targetReplanTime = 0; + } + else + { + // The path is longer or potentially unreachable, full plan. + ag.targetState = CrowdAgent.MoveRequestState.DT_CROWDAGENT_TARGET_WAITING_FOR_QUEUE; } - if (valid) { - // Put the old path infront of the old path. - if (path.Count > 1) { - path.RemoveAt(path.Count - 1); - path.AddRange(res); - res = path; - // Remove trackbacks - for (int j = 1; j < res.Count - 1; ++j) { - if (j - 1 >= 0 && j + 1 < res.Count) { - if (res[j - 1] == res[j + 1]) { - res.RemoveAt(j + 1); - res.RemoveAt(j); - j -= 2; + ag.targetReplanWaitTime = 0; + } + + if (ag.targetState == CrowdAgent.MoveRequestState.DT_CROWDAGENT_TARGET_WAITING_FOR_QUEUE) + { + queue.Enqueue(ag); + } + } + + while (!queue.isEmpty()) + { + CrowdAgent ag = queue.Dequeue(); + ag.targetPathQueryResult = m_pathq.request(ag.corridor.getLastPoly(), ag.targetRef, ag.corridor.getTarget(), + ag.targetPos, m_filters[ag.option.queryFilterType]); + if (ag.targetPathQueryResult != null) + { + ag.targetState = CrowdAgent.MoveRequestState.DT_CROWDAGENT_TARGET_WAITING_FOR_PATH; + } + else + { + _telemetry.recordMaxTimeToEnqueueRequest(ag.targetReplanWaitTime); + ag.targetReplanWaitTime += dt; + } + } + + // Update requests. + _telemetry.start("pathQueueUpdate"); + m_pathq.update(navMesh); + _telemetry.stop("pathQueueUpdate"); + + // Process path results. + foreach (CrowdAgent ag in agents) + { + if (ag.targetState == CrowdAgent.MoveRequestState.DT_CROWDAGENT_TARGET_NONE + || ag.targetState == CrowdAgent.MoveRequestState.DT_CROWDAGENT_TARGET_VELOCITY) + { + continue; + } + + if (ag.targetState == CrowdAgent.MoveRequestState.DT_CROWDAGENT_TARGET_WAITING_FOR_PATH) + { + // _telemetry.recordPathWaitTime(ag.targetReplanTime); + // Poll path queue. + Status status = ag.targetPathQueryResult.status; + if (status != null && status.isFailed()) + { + // Path find failed, retry if the target location is still + // valid. + ag.targetPathQueryResult = null; + if (ag.targetRef != 0) + { + ag.targetState = CrowdAgent.MoveRequestState.DT_CROWDAGENT_TARGET_REQUESTING; + } + else + { + ag.targetState = CrowdAgent.MoveRequestState.DT_CROWDAGENT_TARGET_FAILED; + } + + ag.targetReplanTime = 0; + } + else if (status != null && status.isSuccess()) + { + List path = ag.corridor.getPath(); + if (0 == path.Count) + { + throw new ArgumentException("Empty path"); + } + + // Apply results. + float[] targetPos = ag.targetPos; + + bool valid = true; + List res = ag.targetPathQueryResult.path; + if (status.isFailed() || 0 == res.Count) + { + valid = false; + } + + if (status.isPartial()) + { + ag.partial = true; + } + else + { + ag.partial = false; + } + + // Merge result and existing path. + // The agent might have moved whilst the request is + // being processed, so the path may have changed. + // We assume that the end of the path is at the same + // location + // where the request was issued. + + // The last ref in the old path should be the same as + // the location where the request was issued.. + if (valid && path[path.Count - 1] != res[0]) + { + valid = false; + } + + if (valid) + { + // Put the old path infront of the old path. + if (path.Count > 1) + { + path.RemoveAt(path.Count - 1); + path.AddRange(res); + res = path; + // Remove trackbacks + for (int j = 1; j < res.Count - 1; ++j) + { + if (j - 1 >= 0 && j + 1 < res.Count) + { + if (res[j - 1] == res[j + 1]) + { + res.RemoveAt(j + 1); + res.RemoveAt(j); + j -= 2; + } } } } + + // Check for partial path. + if (res[res.Count - 1] != ag.targetRef) + { + // Partial path, constrain target position inside + // the last polygon. + Result cr = navQuery.closestPointOnPoly(res[res.Count - 1], targetPos); + if (cr.succeeded()) + { + targetPos = cr.result.getClosest(); + } + else + { + valid = false; + } + } } - // Check for partial path. - if (res[res.Count - 1] != ag.targetRef) { - // Partial path, constrain target position inside - // the last polygon. - Result cr = navQuery.closestPointOnPoly(res[res.Count - 1], targetPos); - if (cr.succeeded()) { - targetPos = cr.result.getClosest(); - } else { - valid = false; + if (valid) + { + // Set current corridor. + ag.corridor.setCorridor(targetPos, res); + // Force to update boundary. + ag.boundary.reset(); + ag.targetState = CrowdAgent.MoveRequestState.DT_CROWDAGENT_TARGET_VALID; + } + else + { + // Something went wrong. + ag.targetState = CrowdAgent.MoveRequestState.DT_CROWDAGENT_TARGET_FAILED; + } + + ag.targetReplanTime = 0; + } + + _telemetry.recordMaxTimeToFindPath(ag.targetReplanWaitTime); + ag.targetReplanWaitTime += dt; + } + } + + _telemetry.stop("updateMoveRequest"); + } + + private void updateTopologyOptimization(ICollection agents, float dt) + { + _telemetry.start("updateTopologyOptimization"); + + OrderedQueue queue = new OrderedQueue((a1, a2) => a2.topologyOptTime.CompareTo(a1.topologyOptTime)); + + foreach (CrowdAgent ag in agents) + { + if (ag.state != CrowdAgent.CrowdAgentState.DT_CROWDAGENT_STATE_WALKING) + { + continue; + } + + if (ag.targetState == CrowdAgent.MoveRequestState.DT_CROWDAGENT_TARGET_NONE + || ag.targetState == CrowdAgent.MoveRequestState.DT_CROWDAGENT_TARGET_VELOCITY) + { + continue; + } + + if ((ag.option.updateFlags & CrowdAgentParams.DT_CROWD_OPTIMIZE_TOPO) == 0) + { + continue; + } + + ag.topologyOptTime += dt; + if (ag.topologyOptTime >= _config.topologyOptimizationTimeThreshold) + { + queue.Enqueue(ag); + } + } + + while (!queue.isEmpty()) + { + CrowdAgent ag = queue.Dequeue(); + ag.corridor.optimizePathTopology(navQuery, m_filters[ag.option.queryFilterType], _config.maxTopologyOptimizationIterations); + ag.topologyOptTime = 0; + } + + _telemetry.stop("updateTopologyOptimization"); + } + + private void buildProximityGrid(ICollection agents) + { + _telemetry.start("buildProximityGrid"); + m_grid = new ProximityGrid(_config.maxAgentRadius * 3); + foreach (CrowdAgent ag in agents) + { + float[] p = ag.npos; + float r = ag.option.radius; + m_grid.addItem(ag, p[0] - r, p[2] - r, p[0] + r, p[2] + r); + } + + _telemetry.stop("buildProximityGrid"); + } + + private void buildNeighbours(ICollection agents) + { + _telemetry.start("buildNeighbours"); + foreach (CrowdAgent ag in agents) + { + if (ag.state != CrowdAgent.CrowdAgentState.DT_CROWDAGENT_STATE_WALKING) + { + continue; + } + + // Update the collision boundary after certain distance has been passed or + // if it has become invalid. + float updateThr = ag.option.collisionQueryRange * 0.25f; + if (vDist2DSqr(ag.npos, ag.boundary.getCenter()) > sqr(updateThr) + || !ag.boundary.isValid(navQuery, m_filters[ag.option.queryFilterType])) + { + ag.boundary.update(ag.corridor.getFirstPoly(), ag.npos, ag.option.collisionQueryRange, navQuery, + m_filters[ag.option.queryFilterType]); + } + + // Query neighbour agents + ag.neis = getNeighbours(ag.npos, ag.option.height, ag.option.collisionQueryRange, ag, m_grid); + } + + _telemetry.stop("buildNeighbours"); + } + + private List getNeighbours(float[] pos, float height, float range, CrowdAgent skip, ProximityGrid grid) + { + List result = new List(); + HashSet proxAgents = grid.queryItems(pos[0] - range, pos[2] - range, pos[0] + range, pos[2] + range); + + foreach (CrowdAgent ag in proxAgents) + { + if (ag == skip) + { + continue; + } + + // Check for overlap. + float[] diff = vSub(pos, ag.npos); + if (Math.Abs(diff[1]) >= (height + ag.option.height) / 2.0f) + { + continue; + } + + diff[1] = 0; + float distSqr = vLenSqr(diff); + if (distSqr > sqr(range)) + { + continue; + } + + result.Add(new CrowdNeighbour(ag, distSqr)); + } + + result.Sort((o1, o2) => o1.dist.CompareTo(o2.dist)); + return result; + } + + private void findCorners(ICollection agents, CrowdAgentDebugInfo debug) + { + _telemetry.start("findCorners"); + CrowdAgent debugAgent = debug != null ? debug.agent : null; + foreach (CrowdAgent ag in agents) + { + if (ag.state != CrowdAgent.CrowdAgentState.DT_CROWDAGENT_STATE_WALKING) + { + continue; + } + + if (ag.targetState == CrowdAgent.MoveRequestState.DT_CROWDAGENT_TARGET_NONE + || ag.targetState == CrowdAgent.MoveRequestState.DT_CROWDAGENT_TARGET_VELOCITY) + { + continue; + } + + // Find corners for steering + ag.corners = ag.corridor.findCorners(DT_CROWDAGENT_MAX_CORNERS, navQuery, m_filters[ag.option.queryFilterType]); + + // Check to see if the corner after the next corner is directly visible, + // and short cut to there. + if ((ag.option.updateFlags & CrowdAgentParams.DT_CROWD_OPTIMIZE_VIS) != 0 && ag.corners.Count > 0) + { + float[] target = ag.corners[Math.Min(1, ag.corners.Count - 1)].getPos(); + ag.corridor.optimizePathVisibility(target, ag.option.pathOptimizationRange, navQuery, + m_filters[ag.option.queryFilterType]); + + // Copy data for debug purposes. + if (debugAgent == ag) + { + vCopy(debug.optStart, ag.corridor.getPos()); + vCopy(debug.optEnd, target); + } + } + else + { + // Copy data for debug purposes. + if (debugAgent == ag) + { + vSet(debug.optStart, 0, 0, 0); + vSet(debug.optEnd, 0, 0, 0); + } + } + } + + _telemetry.stop("findCorners"); + } + + private void triggerOffMeshConnections(ICollection agents) + { + _telemetry.start("triggerOffMeshConnections"); + foreach (CrowdAgent ag in agents) + { + if (ag.state != CrowdAgent.CrowdAgentState.DT_CROWDAGENT_STATE_WALKING) + { + continue; + } + + if (ag.targetState == CrowdAgent.MoveRequestState.DT_CROWDAGENT_TARGET_NONE + || ag.targetState == CrowdAgent.MoveRequestState.DT_CROWDAGENT_TARGET_VELOCITY) + { + continue; + } + + // Check + float triggerRadius = ag.option.radius * 2.25f; + if (ag.overOffmeshConnection(triggerRadius)) + { + // Prepare to off-mesh connection. + CrowdAgentAnimation anim = ag.animation; + + // Adjust the path over the off-mesh connection. + long[] refs = new long[2]; + if (ag.corridor.moveOverOffmeshConnection(ag.corners[ag.corners.Count - 1].getRef(), refs, anim.startPos, + anim.endPos, navQuery)) + { + vCopy(anim.initPos, ag.npos); + anim.polyRef = refs[1]; + anim.active = true; + anim.t = 0.0f; + anim.tmax = (vDist2D(anim.startPos, anim.endPos) / ag.option.maxSpeed) * 0.5f; + + ag.state = CrowdAgent.CrowdAgentState.DT_CROWDAGENT_STATE_OFFMESH; + ag.corners.Clear(); + ag.neis.Clear(); + continue; + } + else + { + // Path validity check will ensure that bad/blocked connections will be replanned. + } + } + } + + _telemetry.stop("triggerOffMeshConnections"); + } + + private void calculateSteering(ICollection agents) + { + _telemetry.start("calculateSteering"); + foreach (CrowdAgent ag in agents) + { + if (ag.state != CrowdAgent.CrowdAgentState.DT_CROWDAGENT_STATE_WALKING) + { + continue; + } + + if (ag.targetState == CrowdAgent.MoveRequestState.DT_CROWDAGENT_TARGET_NONE) + { + continue; + } + + float[] dvel = new float[3]; + + if (ag.targetState == CrowdAgent.MoveRequestState.DT_CROWDAGENT_TARGET_VELOCITY) + { + vCopy(dvel, ag.targetPos); + ag.desiredSpeed = vLen(ag.targetPos); + } + else + { + // Calculate steering direction. + if ((ag.option.updateFlags & CrowdAgentParams.DT_CROWD_ANTICIPATE_TURNS) != 0) + { + dvel = ag.calcSmoothSteerDirection(); + } + else + { + dvel = ag.calcStraightSteerDirection(); + } + + // Calculate speed scale, which tells the agent to slowdown at the end of the path. + float slowDownRadius = ag.option.radius * 2; // TODO: make less hacky. + float speedScale = ag.getDistanceToGoal(slowDownRadius) / slowDownRadius; + + ag.desiredSpeed = ag.option.maxSpeed; + dvel = vScale(dvel, ag.desiredSpeed * speedScale); + } + + // Separation + if ((ag.option.updateFlags & CrowdAgentParams.DT_CROWD_SEPARATION) != 0) + { + float separationDist = ag.option.collisionQueryRange; + float invSeparationDist = 1.0f / separationDist; + float separationWeight = ag.option.separationWeight; + + float w = 0; + float[] disp = new float[3]; + + for (int j = 0; j < ag.neis.Count; ++j) + { + CrowdAgent nei = ag.neis[j].agent; + + float[] diff = vSub(ag.npos, nei.npos); + diff[1] = 0; + + float distSqr = vLenSqr(diff); + if (distSqr < 0.00001f) + { + continue; + } + + if (distSqr > sqr(separationDist)) + { + continue; + } + + float dist = (float)Math.Sqrt(distSqr); + float weight = separationWeight * (1.0f - sqr(dist * invSeparationDist)); + + disp = vMad(disp, diff, weight / dist); + w += 1.0f; + } + + if (w > 0.0001f) + { + // Adjust desired velocity. + dvel = vMad(dvel, disp, 1.0f / w); + // Clamp desired velocity to desired speed. + float speedSqr = vLenSqr(dvel); + float desiredSqr = sqr(ag.desiredSpeed); + if (speedSqr > desiredSqr) + { + dvel = vScale(dvel, desiredSqr / speedSqr); + } + } + } + + // Set the desired velocity. + vCopy(ag.dvel, dvel); + } + + _telemetry.stop("calculateSteering"); + } + + private void planVelocity(CrowdAgentDebugInfo debug, ICollection agents) + { + _telemetry.start("planVelocity"); + CrowdAgent debugAgent = debug != null ? debug.agent : null; + foreach (CrowdAgent ag in agents) + { + if (ag.state != CrowdAgent.CrowdAgentState.DT_CROWDAGENT_STATE_WALKING) + { + continue; + } + + if ((ag.option.updateFlags & CrowdAgentParams.DT_CROWD_OBSTACLE_AVOIDANCE) != 0) + { + m_obstacleQuery.reset(); + + // Add neighbours as obstacles. + for (int j = 0; j < ag.neis.Count; ++j) + { + CrowdAgent nei = ag.neis[j].agent; + m_obstacleQuery.addCircle(nei.npos, nei.option.radius, nei.vel, nei.dvel); + } + + // Append neighbour segments as obstacles. + for (int j = 0; j < ag.boundary.getSegmentCount(); ++j) + { + float[] s = ag.boundary.getSegment(j); + float[] s3 = new float[3]; + Array.Copy(s, 3, s3, 0, 3); + if (triArea2D(ag.npos, s, s3) < 0.0f) + { + continue; + } + + m_obstacleQuery.addSegment(s, s3); + } + + ObstacleAvoidanceDebugData vod = null; + if (debugAgent == ag) + { + vod = debug.vod; + } + + // Sample new safe velocity. + bool adaptive = true; + int ns = 0; + + ObstacleAvoidanceQuery.ObstacleAvoidanceParams option = m_obstacleQueryParams[ag.option.obstacleAvoidanceType]; + + if (adaptive) + { + Tuple nsnvel = m_obstacleQuery.sampleVelocityAdaptive(ag.npos, ag.option.radius, + ag.desiredSpeed, ag.vel, ag.dvel, option, vod); + ns = nsnvel.Item1; + ag.nvel = nsnvel.Item2; + } + else + { + Tuple nsnvel = m_obstacleQuery.sampleVelocityGrid(ag.npos, ag.option.radius, + ag.desiredSpeed, ag.vel, ag.dvel, option, vod); + ns = nsnvel.Item1; + ag.nvel = nsnvel.Item2; + } + + m_velocitySampleCount += ns; + } + else + { + // If not using velocity planning, new velocity is directly the desired velocity. + vCopy(ag.nvel, ag.dvel); + } + } + + _telemetry.stop("planVelocity"); + } + + private void integrate(float dt, ICollection agents) + { + _telemetry.start("integrate"); + foreach (CrowdAgent ag in agents) + { + if (ag.state != CrowdAgent.CrowdAgentState.DT_CROWDAGENT_STATE_WALKING) + { + continue; + } + + ag.integrate(dt); + } + + _telemetry.stop("integrate"); + } + + private void handleCollisions(ICollection agents) + { + _telemetry.start("handleCollisions"); + for (int iter = 0; iter < 4; ++iter) + { + foreach (CrowdAgent ag in agents) + { + long idx0 = ag.idx; + if (ag.state != CrowdAgent.CrowdAgentState.DT_CROWDAGENT_STATE_WALKING) + { + continue; + } + + vSet(ag.disp, 0, 0, 0); + + float w = 0; + + for (int j = 0; j < ag.neis.Count; ++j) + { + CrowdAgent nei = ag.neis[j].agent; + long idx1 = nei.idx; + float[] diff = vSub(ag.npos, nei.npos); + diff[1] = 0; + + float dist = vLenSqr(diff); + if (dist > sqr(ag.option.radius + nei.option.radius)) + { + continue; + } + + dist = (float)Math.Sqrt(dist); + float pen = (ag.option.radius + nei.option.radius) - dist; + if (dist < 0.0001f) + { + // Agents on top of each other, try to choose diverging separation directions. + if (idx0 > idx1) + { + vSet(diff, -ag.dvel[2], 0, ag.dvel[0]); } + else + { + vSet(diff, ag.dvel[2], 0, -ag.dvel[0]); + } + + pen = 0.01f; } + else + { + pen = (1.0f / dist) * (pen * 0.5f) * _config.collisionResolveFactor; + } + + ag.disp = vMad(ag.disp, diff, pen); + + w += 1.0f; } - if (valid) { - // Set current corridor. - ag.corridor.setCorridor(targetPos, res); - // Force to update boundary. - ag.boundary.reset(); - ag.targetState = CrowdAgent.MoveRequestState.DT_CROWDAGENT_TARGET_VALID; - } else { - // Something went wrong. - ag.targetState = CrowdAgent.MoveRequestState.DT_CROWDAGENT_TARGET_FAILED; + if (w > 0.0001f) + { + float iw = 1.0f / w; + ag.disp = vScale(ag.disp, iw); + } + } + + foreach (CrowdAgent ag in agents) + { + if (ag.state != CrowdAgent.CrowdAgentState.DT_CROWDAGENT_STATE_WALKING) + { + continue; } - ag.targetReplanTime = 0; - } - _telemetry.recordMaxTimeToFindPath(ag.targetReplanWaitTime); - ag.targetReplanWaitTime += dt; - } - } - _telemetry.stop("updateMoveRequest"); - } - - private void updateTopologyOptimization(ICollection agents, float dt) { - _telemetry.start("updateTopologyOptimization"); - - OrderedQueue queue = new OrderedQueue((a1, a2) => a2.topologyOptTime.CompareTo(a1.topologyOptTime)); - - foreach (CrowdAgent ag in agents) { - if (ag.state != CrowdAgent.CrowdAgentState.DT_CROWDAGENT_STATE_WALKING) { - continue; - } - if (ag.targetState == CrowdAgent.MoveRequestState.DT_CROWDAGENT_TARGET_NONE - || ag.targetState == CrowdAgent.MoveRequestState.DT_CROWDAGENT_TARGET_VELOCITY) { - continue; - } - if ((ag.option.updateFlags & CrowdAgentParams.DT_CROWD_OPTIMIZE_TOPO) == 0) { - continue; - } - ag.topologyOptTime += dt; - if (ag.topologyOptTime >= _config.topologyOptimizationTimeThreshold) { - queue.Enqueue(ag); - } - } - - while (!queue.isEmpty()) { - CrowdAgent ag = queue.Dequeue(); - ag.corridor.optimizePathTopology(navQuery, m_filters[ag.option.queryFilterType], _config.maxTopologyOptimizationIterations); - ag.topologyOptTime = 0; - } - _telemetry.stop("updateTopologyOptimization"); - - } - - private void buildProximityGrid(ICollection agents) { - _telemetry.start("buildProximityGrid"); - m_grid = new ProximityGrid(_config.maxAgentRadius * 3); - foreach (CrowdAgent ag in agents) { - float[] p = ag.npos; - float r = ag.option.radius; - m_grid.addItem(ag, p[0] - r, p[2] - r, p[0] + r, p[2] + r); - } - _telemetry.stop("buildProximityGrid"); - } - - private void buildNeighbours(ICollection agents) { - _telemetry.start("buildNeighbours"); - foreach (CrowdAgent ag in agents) { - if (ag.state != CrowdAgent.CrowdAgentState.DT_CROWDAGENT_STATE_WALKING) { - continue; - } - - // Update the collision boundary after certain distance has been passed or - // if it has become invalid. - float updateThr = ag.option.collisionQueryRange * 0.25f; - if (vDist2DSqr(ag.npos, ag.boundary.getCenter()) > sqr(updateThr) - || !ag.boundary.isValid(navQuery, m_filters[ag.option.queryFilterType])) { - ag.boundary.update(ag.corridor.getFirstPoly(), ag.npos, ag.option.collisionQueryRange, navQuery, - m_filters[ag.option.queryFilterType]); - } - // Query neighbour agents - ag.neis = getNeighbours(ag.npos, ag.option.height, ag.option.collisionQueryRange, ag, m_grid); - } - _telemetry.stop("buildNeighbours"); - } - - private List getNeighbours(float[] pos, float height, float range, CrowdAgent skip, ProximityGrid grid) { - - List result = new List(); - HashSet proxAgents = grid.queryItems(pos[0] - range, pos[2] - range, pos[0] + range, pos[2] + range); - - foreach (CrowdAgent ag in proxAgents) { - - if (ag == skip) { - continue; - } - - // Check for overlap. - float[] diff = vSub(pos, ag.npos); - if (Math.Abs(diff[1]) >= (height + ag.option.height) / 2.0f) { - continue; - } - diff[1] = 0; - float distSqr = vLenSqr(diff); - if (distSqr > sqr(range)) { - continue; - } - - result.Add(new CrowdNeighbour(ag, distSqr)); - } - result.Sort((o1, o2) => o1.dist.CompareTo(o2.dist)); - return result; - - } - - private void findCorners(ICollection agents, CrowdAgentDebugInfo debug) { - _telemetry.start("findCorners"); - CrowdAgent debugAgent = debug != null ? debug.agent : null; - foreach (CrowdAgent ag in agents) { - - if (ag.state != CrowdAgent.CrowdAgentState.DT_CROWDAGENT_STATE_WALKING) { - continue; - } - if (ag.targetState == CrowdAgent.MoveRequestState.DT_CROWDAGENT_TARGET_NONE - || ag.targetState == CrowdAgent.MoveRequestState.DT_CROWDAGENT_TARGET_VELOCITY) { - continue; - } - - // Find corners for steering - ag.corners = ag.corridor.findCorners(DT_CROWDAGENT_MAX_CORNERS, navQuery, m_filters[ag.option.queryFilterType]); - - // Check to see if the corner after the next corner is directly visible, - // and short cut to there. - if ((ag.option.updateFlags & CrowdAgentParams.DT_CROWD_OPTIMIZE_VIS) != 0 && ag.corners.Count > 0) { - float[] target = ag.corners[Math.Min(1, ag.corners.Count - 1)].getPos(); - ag.corridor.optimizePathVisibility(target, ag.option.pathOptimizationRange, navQuery, - m_filters[ag.option.queryFilterType]); - - // Copy data for debug purposes. - if (debugAgent == ag) { - vCopy(debug.optStart, ag.corridor.getPos()); - vCopy(debug.optEnd, target); - } - } else { - // Copy data for debug purposes. - if (debugAgent == ag) { - vSet(debug.optStart, 0, 0, 0); - vSet(debug.optEnd, 0, 0, 0); + ag.npos = vAdd(ag.npos, ag.disp); } } + + _telemetry.stop("handleCollisions"); } - _telemetry.stop("findCorners"); - } - private void triggerOffMeshConnections(ICollection agents) { - _telemetry.start("triggerOffMeshConnections"); - foreach (CrowdAgent ag in agents) { + private void moveAgents(ICollection agents) + { + _telemetry.start("moveAgents"); + foreach (CrowdAgent ag in agents) + { + if (ag.state != CrowdAgent.CrowdAgentState.DT_CROWDAGENT_STATE_WALKING) + { + continue; + } - if (ag.state != CrowdAgent.CrowdAgentState.DT_CROWDAGENT_STATE_WALKING) { - continue; - } - if (ag.targetState == CrowdAgent.MoveRequestState.DT_CROWDAGENT_TARGET_NONE - || ag.targetState == CrowdAgent.MoveRequestState.DT_CROWDAGENT_TARGET_VELOCITY) { - continue; + // Move along navmesh. + ag.corridor.movePosition(ag.npos, navQuery, m_filters[ag.option.queryFilterType]); + // Get valid constrained position back. + vCopy(ag.npos, ag.corridor.getPos()); + + // If not using path, truncate the corridor to just one poly. + if (ag.targetState == CrowdAgent.MoveRequestState.DT_CROWDAGENT_TARGET_NONE + || ag.targetState == CrowdAgent.MoveRequestState.DT_CROWDAGENT_TARGET_VELOCITY) + { + ag.corridor.reset(ag.corridor.getFirstPoly(), ag.npos); + ag.partial = false; + } } - // Check - float triggerRadius = ag.option.radius * 2.25f; - if (ag.overOffmeshConnection(triggerRadius)) { - // Prepare to off-mesh connection. + _telemetry.stop("moveAgents"); + } + + private void updateOffMeshConnections(ICollection agents, float dt) + { + _telemetry.start("updateOffMeshConnections"); + foreach (CrowdAgent ag in agents) + { CrowdAgentAnimation anim = ag.animation; - - // Adjust the path over the off-mesh connection. - long[] refs = new long[2]; - if (ag.corridor.moveOverOffmeshConnection(ag.corners[ag.corners.Count - 1].getRef(), refs, anim.startPos, - anim.endPos, navQuery)) { - vCopy(anim.initPos, ag.npos); - anim.polyRef = refs[1]; - anim.active = true; - anim.t = 0.0f; - anim.tmax = (vDist2D(anim.startPos, anim.endPos) / ag.option.maxSpeed) * 0.5f; - - ag.state = CrowdAgent.CrowdAgentState.DT_CROWDAGENT_STATE_OFFMESH; - ag.corners.Clear(); - ag.neis.Clear(); - continue; - } else { - // Path validity check will ensure that bad/blocked connections will be replanned. - } - } - } - _telemetry.stop("triggerOffMeshConnections"); - } - - private void calculateSteering(ICollection agents) { - _telemetry.start("calculateSteering"); - foreach (CrowdAgent ag in agents) { - - if (ag.state != CrowdAgent.CrowdAgentState.DT_CROWDAGENT_STATE_WALKING) { - continue; - } - if (ag.targetState == CrowdAgent.MoveRequestState.DT_CROWDAGENT_TARGET_NONE) { - continue; - } - - float[] dvel = new float[3]; - - if (ag.targetState == CrowdAgent.MoveRequestState.DT_CROWDAGENT_TARGET_VELOCITY) { - vCopy(dvel, ag.targetPos); - ag.desiredSpeed = vLen(ag.targetPos); - } else { - // Calculate steering direction. - if ((ag.option.updateFlags & CrowdAgentParams.DT_CROWD_ANTICIPATE_TURNS) != 0) { - dvel = ag.calcSmoothSteerDirection(); - } else { - dvel = ag.calcStraightSteerDirection(); - } - // Calculate speed scale, which tells the agent to slowdown at the end of the path. - float slowDownRadius = ag.option.radius * 2; // TODO: make less hacky. - float speedScale = ag.getDistanceToGoal(slowDownRadius) / slowDownRadius; - - ag.desiredSpeed = ag.option.maxSpeed; - dvel = vScale(dvel, ag.desiredSpeed * speedScale); - } - - // Separation - if ((ag.option.updateFlags & CrowdAgentParams.DT_CROWD_SEPARATION) != 0) { - float separationDist = ag.option.collisionQueryRange; - float invSeparationDist = 1.0f / separationDist; - float separationWeight = ag.option.separationWeight; - - float w = 0; - float[] disp = new float[3]; - - for (int j = 0; j < ag.neis.Count; ++j) { - CrowdAgent nei = ag.neis[j].agent; - - float[] diff = vSub(ag.npos, nei.npos); - diff[1] = 0; - - float distSqr = vLenSqr(diff); - if (distSqr < 0.00001f) { - continue; - } - if (distSqr > sqr(separationDist)) { - continue; - } - float dist = (float) Math.Sqrt(distSqr); - float weight = separationWeight * (1.0f - sqr(dist * invSeparationDist)); - - disp = vMad(disp, diff, weight / dist); - w += 1.0f; - } - - if (w > 0.0001f) { - // Adjust desired velocity. - dvel = vMad(dvel, disp, 1.0f / w); - // Clamp desired velocity to desired speed. - float speedSqr = vLenSqr(dvel); - float desiredSqr = sqr(ag.desiredSpeed); - if (speedSqr > desiredSqr) { - dvel = vScale(dvel, desiredSqr / speedSqr); - } - } - } - - // Set the desired velocity. - vCopy(ag.dvel, dvel); - } - _telemetry.stop("calculateSteering"); - } - - private void planVelocity(CrowdAgentDebugInfo debug, ICollection agents) { - _telemetry.start("planVelocity"); - CrowdAgent debugAgent = debug != null ? debug.agent : null; - foreach (CrowdAgent ag in agents) { - - if (ag.state != CrowdAgent.CrowdAgentState.DT_CROWDAGENT_STATE_WALKING) { - continue; - } - - if ((ag.option.updateFlags & CrowdAgentParams.DT_CROWD_OBSTACLE_AVOIDANCE) != 0) { - m_obstacleQuery.reset(); - - // Add neighbours as obstacles. - for (int j = 0; j < ag.neis.Count; ++j) { - CrowdAgent nei = ag.neis[j].agent; - m_obstacleQuery.addCircle(nei.npos, nei.option.radius, nei.vel, nei.dvel); - } - - // Append neighbour segments as obstacles. - for (int j = 0; j < ag.boundary.getSegmentCount(); ++j) { - float[] s = ag.boundary.getSegment(j); - float[] s3 = new float[3]; - Array.Copy(s, 3, s3, 0, 3); - if (triArea2D(ag.npos, s, s3) < 0.0f) { - continue; - } - m_obstacleQuery.addSegment(s, s3); - } - - ObstacleAvoidanceDebugData vod = null; - if (debugAgent == ag) { - vod = debug.vod; - } - - // Sample new safe velocity. - bool adaptive = true; - int ns = 0; - - ObstacleAvoidanceQuery.ObstacleAvoidanceParams option = m_obstacleQueryParams[ag.option.obstacleAvoidanceType]; - - if (adaptive) { - Tuple nsnvel = m_obstacleQuery.sampleVelocityAdaptive(ag.npos, ag.option.radius, - ag.desiredSpeed, ag.vel, ag.dvel, option, vod); - ns = nsnvel.Item1; - ag.nvel = nsnvel.Item2; - } else { - Tuple nsnvel = m_obstacleQuery.sampleVelocityGrid(ag.npos, ag.option.radius, - ag.desiredSpeed, ag.vel, ag.dvel, option, vod); - ns = nsnvel.Item1; - ag.nvel = nsnvel.Item2; - } - m_velocitySampleCount += ns; - } else { - // If not using velocity planning, new velocity is directly the desired velocity. - vCopy(ag.nvel, ag.dvel); - } - } - _telemetry.stop("planVelocity"); - } - - private void integrate(float dt, ICollection agents) { - _telemetry.start("integrate"); - foreach (CrowdAgent ag in agents) { - if (ag.state != CrowdAgent.CrowdAgentState.DT_CROWDAGENT_STATE_WALKING) { - continue; - } - ag.integrate(dt); - } - _telemetry.stop("integrate"); - } - - private void handleCollisions(ICollection agents) { - _telemetry.start("handleCollisions"); - for (int iter = 0; iter < 4; ++iter) { - foreach (CrowdAgent ag in agents) { - long idx0 = ag.idx; - if (ag.state != CrowdAgent.CrowdAgentState.DT_CROWDAGENT_STATE_WALKING) { + if (!anim.active) + { continue; } - vSet(ag.disp, 0, 0, 0); - - float w = 0; - - for (int j = 0; j < ag.neis.Count; ++j) { - CrowdAgent nei = ag.neis[j].agent; - long idx1 = nei.idx; - float[] diff = vSub(ag.npos, nei.npos); - diff[1] = 0; - - float dist = vLenSqr(diff); - if (dist > sqr(ag.option.radius + nei.option.radius)) { - continue; - } - dist = (float) Math.Sqrt(dist); - float pen = (ag.option.radius + nei.option.radius) - dist; - if (dist < 0.0001f) { - // Agents on top of each other, try to choose diverging separation directions. - if (idx0 > idx1) { - vSet(diff, -ag.dvel[2], 0, ag.dvel[0]); - } else { - vSet(diff, ag.dvel[2], 0, -ag.dvel[0]); - } - pen = 0.01f; - } else { - pen = (1.0f / dist) * (pen * 0.5f) * _config.collisionResolveFactor; - } - - ag.disp = vMad(ag.disp, diff, pen); - - w += 1.0f; - } - - if (w > 0.0001f) { - float iw = 1.0f / w; - ag.disp = vScale(ag.disp, iw); - } - } - - foreach (CrowdAgent ag in agents) { - if (ag.state != CrowdAgent.CrowdAgentState.DT_CROWDAGENT_STATE_WALKING) { + anim.t += dt; + if (anim.t > anim.tmax) + { + // Reset animation + anim.active = false; + // Prepare agent for walking. + ag.state = CrowdAgent.CrowdAgentState.DT_CROWDAGENT_STATE_WALKING; continue; } - ag.npos = vAdd(ag.npos, ag.disp); + // Update position + float ta = anim.tmax * 0.15f; + float tb = anim.tmax; + if (anim.t < ta) + { + float u = tween(anim.t, 0.0f, ta); + ag.npos = vLerp(anim.initPos, anim.startPos, u); + } + else + { + float u = tween(anim.t, ta, tb); + ag.npos = vLerp(anim.startPos, anim.endPos, u); + } + + // Update velocity. + vSet(ag.vel, 0, 0, 0); + vSet(ag.dvel, 0, 0, 0); } + + _telemetry.stop("updateOffMeshConnections"); } - _telemetry.stop("handleCollisions"); - } - - private void moveAgents(ICollection agents) { - _telemetry.start("moveAgents"); - foreach (CrowdAgent ag in agents) { - if (ag.state != CrowdAgent.CrowdAgentState.DT_CROWDAGENT_STATE_WALKING) { - continue; - } - - // Move along navmesh. - ag.corridor.movePosition(ag.npos, navQuery, m_filters[ag.option.queryFilterType]); - // Get valid constrained position back. - vCopy(ag.npos, ag.corridor.getPos()); - - // If not using path, truncate the corridor to just one poly. - if (ag.targetState == CrowdAgent.MoveRequestState.DT_CROWDAGENT_TARGET_NONE - || ag.targetState == CrowdAgent.MoveRequestState.DT_CROWDAGENT_TARGET_VELOCITY) { - ag.corridor.reset(ag.corridor.getFirstPoly(), ag.npos); - ag.partial = false; - } - + private float tween(float t, float t0, float t1) + { + return clamp((t - t0) / (t1 - t0), 0.0f, 1.0f); } - _telemetry.stop("moveAgents"); - } - private void updateOffMeshConnections(ICollection agents, float dt) { - _telemetry.start("updateOffMeshConnections"); - foreach (CrowdAgent ag in agents) { - CrowdAgentAnimation anim = ag.animation; - if (!anim.active) { - continue; + /// Provides neighbor data for agents managed by the crowd. + /// @ingroup crowd + /// @see dtCrowdAgent::neis, dtCrowd + public class CrowdNeighbour + { + public readonly CrowdAgent agent; + + /// < The index of the neighbor in the crowd. + public readonly float dist; + + /// < The distance between the current agent and the neighbor. + public CrowdNeighbour(CrowdAgent agent, float dist) + { + this.agent = agent; + this.dist = dist; } - - anim.t += dt; - if (anim.t > anim.tmax) { - // Reset animation - anim.active = false; - // Prepare agent for walking. - ag.state = CrowdAgent.CrowdAgentState.DT_CROWDAGENT_STATE_WALKING; - continue; - } - - // Update position - float ta = anim.tmax * 0.15f; - float tb = anim.tmax; - if (anim.t < ta) { - float u = tween(anim.t, 0.0f, ta); - ag.npos = vLerp(anim.initPos, anim.startPos, u); - } else { - float u = tween(anim.t, ta, tb); - ag.npos = vLerp(anim.startPos, anim.endPos, u); - } - - // Update velocity. - vSet(ag.vel, 0, 0, 0); - vSet(ag.dvel, 0, 0, 0); - } - _telemetry.stop("updateOffMeshConnections"); + }; } - - private float tween(float t, float t0, float t1) { - return clamp((t - t0) / (t1 - t0), 0.0f, 1.0f); - } - - /// Provides neighbor data for agents managed by the crowd. - /// @ingroup crowd - /// @see dtCrowdAgent::neis, dtCrowd - public class CrowdNeighbour { - public readonly CrowdAgent agent; /// < The index of the neighbor in the crowd. - public readonly float dist; /// < The distance between the current agent and the neighbor. - - public CrowdNeighbour(CrowdAgent agent, float dist) { - this.agent = agent; - this.dist = dist; - } - }; - -} - } \ No newline at end of file diff --git a/src/DotRecast.Detour.Crowd/CrowdAgent.cs b/src/DotRecast.Detour.Crowd/CrowdAgent.cs index 88d45d3..fa8f770 100644 --- a/src/DotRecast.Detour.Crowd/CrowdAgent.cs +++ b/src/DotRecast.Detour.Crowd/CrowdAgent.cs @@ -23,173 +23,221 @@ using System.Collections.Generic; namespace DotRecast.Detour.Crowd { + using static DetourCommon; - -using static DetourCommon; - -/// Represents an agent managed by a #dtCrowd object. -/// @ingroup crowd -public class CrowdAgent { - - /// The type of navigation mesh polygon the agent is currently traversing. + /// Represents an agent managed by a #dtCrowd object. /// @ingroup crowd - public enum CrowdAgentState { - DT_CROWDAGENT_STATE_INVALID, /// < The agent is not in a valid state. - DT_CROWDAGENT_STATE_WALKING, /// < The agent is traversing a normal navigation mesh polygon. - DT_CROWDAGENT_STATE_OFFMESH, /// < The agent is traversing an off-mesh connection. - }; + public class CrowdAgent + { + /// The type of navigation mesh polygon the agent is currently traversing. + /// @ingroup crowd + public enum CrowdAgentState + { + DT_CROWDAGENT_STATE_INVALID, - public enum MoveRequestState { - DT_CROWDAGENT_TARGET_NONE, - DT_CROWDAGENT_TARGET_FAILED, - DT_CROWDAGENT_TARGET_VALID, - DT_CROWDAGENT_TARGET_REQUESTING, - DT_CROWDAGENT_TARGET_WAITING_FOR_QUEUE, - DT_CROWDAGENT_TARGET_WAITING_FOR_PATH, - DT_CROWDAGENT_TARGET_VELOCITY, - }; + /// < The agent is not in a valid state. + DT_CROWDAGENT_STATE_WALKING, - public readonly long idx; - /// The type of mesh polygon the agent is traversing. (See: #CrowdAgentState) - public CrowdAgentState state; - /// True if the agent has valid path (targetState == DT_CROWDAGENT_TARGET_VALID) and the path does not lead to the - /// requested position, else false. - public bool partial; - /// The path corridor the agent is using. - public PathCorridor corridor; - /// The local boundary data for the agent. - public LocalBoundary boundary; - /// Time since the agent's path corridor was optimized. - public float topologyOptTime; - /// The known neighbors of the agent. - public List neis = new List(); - /// The desired speed. - public float desiredSpeed; + /// < The agent is traversing a normal navigation mesh polygon. + DT_CROWDAGENT_STATE_OFFMESH, /// < The agent is traversing an off-mesh connection. + }; - public float[] npos = new float[3]; /// < The current agent position. [(x, y, z)] - public float[] disp = new float[3]; /// < A temporary value used to accumulate agent displacement during iterative - /// collision resolution. [(x, y, z)] - public float[] dvel = new float[3]; /// < The desired velocity of the agent. Based on the current path, calculated - /// from - /// scratch each frame. [(x, y, z)] - public float[] nvel = new float[3]; /// < The desired velocity adjusted by obstacle avoidance, calculated from scratch each - /// frame. [(x, y, z)] - public float[] vel = new float[3]; /// < The actual velocity of the agent. The change from nvel -> vel is - /// constrained by max acceleration. [(x, y, z)] + public enum MoveRequestState + { + DT_CROWDAGENT_TARGET_NONE, + DT_CROWDAGENT_TARGET_FAILED, + DT_CROWDAGENT_TARGET_VALID, + DT_CROWDAGENT_TARGET_REQUESTING, + DT_CROWDAGENT_TARGET_WAITING_FOR_QUEUE, + DT_CROWDAGENT_TARGET_WAITING_FOR_PATH, + DT_CROWDAGENT_TARGET_VELOCITY, + }; - /// The agent's configuration parameters. - public CrowdAgentParams option; - /// The local path corridor corners for the agent. - public List corners = new List(); + public readonly long idx; - public MoveRequestState targetState; /// < State of the movement request. - public long targetRef; /// < Target polyref of the movement request. - public float[] targetPos = new float[3]; /// < Target position of the movement request (or velocity in case of - /// DT_CROWDAGENT_TARGET_VELOCITY). - public PathQueryResult targetPathQueryResult; /// < Path finder query - public bool targetReplan; /// < Flag indicating that the current path is being replanned. - public float targetReplanTime; ///