├── Simulator.cs ├── README.md ├── Agent.cs.meta ├── KdTree.cs.meta ├── Line.cs.meta ├── BoxObstacle.cs.meta ├── CacheArray.cs.meta ├── Obstacle.cs.meta ├── RVOMath.cs.meta ├── RVONavAgent.cs.meta ├── Simulator.cs.meta ├── CircleObstacle.cs.meta ├── EdgeObstacle.cs.meta ├── ShapeObstacleBase.cs.meta ├── CircleObstacle.cs ├── Line.cs ├── EdgeObstacle.cs ├── Obstacle.cs ├── ShapeObstacleBase.cs ├── BoxObstacle.cs ├── RVONavAgent.cs ├── CacheArray.cs ├── RVOMath.cs ├── KdTree.cs └── Agent.cs /Simulator.cs: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/sunsvip/UnityRVO2/HEAD/Simulator.cs -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # UnityRVO2 2 | RVO for unity 3 | 4 | https://github.com/snape/RVO2-CS 5 | -------------------------------------------------------------------------------- /Agent.cs.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: de329959c4e335e4180a70f4b185dbaf 3 | MonoImporter: 4 | externalObjects: {} 5 | serializedVersion: 2 6 | defaultReferences: [] 7 | executionOrder: 0 8 | icon: {instanceID: 0} 9 | userData: 10 | assetBundleName: 11 | assetBundleVariant: 12 | -------------------------------------------------------------------------------- /KdTree.cs.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: 0ef6d3053a106714c94ae71297720947 3 | MonoImporter: 4 | externalObjects: {} 5 | serializedVersion: 2 6 | defaultReferences: [] 7 | executionOrder: 0 8 | icon: {instanceID: 0} 9 | userData: 10 | assetBundleName: 11 | assetBundleVariant: 12 | -------------------------------------------------------------------------------- /Line.cs.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: fc0051dc74be10a4e97cc8ab68968cd9 3 | MonoImporter: 4 | externalObjects: {} 5 | serializedVersion: 2 6 | defaultReferences: [] 7 | executionOrder: 0 8 | icon: {instanceID: 0} 9 | userData: 10 | assetBundleName: 11 | assetBundleVariant: 12 | -------------------------------------------------------------------------------- /BoxObstacle.cs.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: ba1052c12a0db0847abb4f47df2e51bb 3 | MonoImporter: 4 | externalObjects: {} 5 | serializedVersion: 2 6 | defaultReferences: [] 7 | executionOrder: 0 8 | icon: {instanceID: 0} 9 | userData: 10 | assetBundleName: 11 | assetBundleVariant: 12 | -------------------------------------------------------------------------------- /CacheArray.cs.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: 1f2daddcb97de634cb623c6e83e15608 3 | MonoImporter: 4 | externalObjects: {} 5 | serializedVersion: 2 6 | defaultReferences: [] 7 | executionOrder: 0 8 | icon: {instanceID: 0} 9 | userData: 10 | assetBundleName: 11 | assetBundleVariant: 12 | -------------------------------------------------------------------------------- /Obstacle.cs.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: 26e99e94440f056448bb47437406700d 3 | MonoImporter: 4 | externalObjects: {} 5 | serializedVersion: 2 6 | defaultReferences: [] 7 | executionOrder: 0 8 | icon: {instanceID: 0} 9 | userData: 10 | assetBundleName: 11 | assetBundleVariant: 12 | -------------------------------------------------------------------------------- /RVOMath.cs.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: e5c05312931a4bb4b9a5dfcd982b9991 3 | MonoImporter: 4 | externalObjects: {} 5 | serializedVersion: 2 6 | defaultReferences: [] 7 | executionOrder: 0 8 | icon: {instanceID: 0} 9 | userData: 10 | assetBundleName: 11 | assetBundleVariant: 12 | -------------------------------------------------------------------------------- /RVONavAgent.cs.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: 28b29d50de1accf45b167bb7a84d0ccc 3 | MonoImporter: 4 | externalObjects: {} 5 | serializedVersion: 2 6 | defaultReferences: [] 7 | executionOrder: 0 8 | icon: {instanceID: 0} 9 | userData: 10 | assetBundleName: 11 | assetBundleVariant: 12 | -------------------------------------------------------------------------------- /Simulator.cs.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: de1f414d93df8a24ba0f02da4708f4d5 3 | MonoImporter: 4 | externalObjects: {} 5 | serializedVersion: 2 6 | defaultReferences: [] 7 | executionOrder: 0 8 | icon: {instanceID: 0} 9 | userData: 10 | assetBundleName: 11 | assetBundleVariant: 12 | -------------------------------------------------------------------------------- /CircleObstacle.cs.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: c69f5eb943a5b484a868154aa4ec6386 3 | MonoImporter: 4 | externalObjects: {} 5 | serializedVersion: 2 6 | defaultReferences: [] 7 | executionOrder: 0 8 | icon: {instanceID: 0} 9 | userData: 10 | assetBundleName: 11 | assetBundleVariant: 12 | -------------------------------------------------------------------------------- /EdgeObstacle.cs.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: 662c6be36fc86d54abd837c62d6fea54 3 | MonoImporter: 4 | externalObjects: {} 5 | serializedVersion: 2 6 | defaultReferences: [] 7 | executionOrder: 0 8 | icon: {instanceID: 0} 9 | userData: 10 | assetBundleName: 11 | assetBundleVariant: 12 | -------------------------------------------------------------------------------- /ShapeObstacleBase.cs.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: 08e5858fabbbda44fa6e27c2411af627 3 | MonoImporter: 4 | externalObjects: {} 5 | serializedVersion: 2 6 | defaultReferences: [] 7 | executionOrder: 0 8 | icon: {instanceID: 0} 9 | userData: 10 | assetBundleName: 11 | assetBundleVariant: 12 | -------------------------------------------------------------------------------- /CircleObstacle.cs: -------------------------------------------------------------------------------- 1 | using System.Collections; 2 | using System.Collections.Generic; 3 | using UnityEngine; 4 | using RVO; 5 | #if UNITY_EDITOR 6 | using UnityEditor; 7 | 8 | #endif 9 | public class CircleObstacle : ShapeObstacleBase 10 | { 11 | [SerializeField] float radius = 0.5f; 12 | [SerializeField][Range(3, 100)] int vertexCount = 6; 13 | protected override void CalculatePoints() 14 | { 15 | float anglePadding = 360f / vertexCount; 16 | 17 | var vec = transform.forward * radius; 18 | 19 | var curPos = transform.position; 20 | for (int i = 0; i < vertexCount; i++) 21 | { 22 | mCacheVertexList.Add(new Vector2(vec.x + curPos.x, vec.z + curPos.z)); 23 | #if UNITY_EDITOR 24 | mEditorDebugVertexList.Add(vec + curPos); 25 | #endif 26 | vec = Quaternion.AngleAxis(-anglePadding, transform.up) * vec; 27 | } 28 | } 29 | } 30 | -------------------------------------------------------------------------------- /Line.cs: -------------------------------------------------------------------------------- 1 | /* 2 | * Line.cs 3 | * RVO2 Library C# 4 | * 5 | * SPDX-FileCopyrightText: 2008 University of North Carolina at Chapel Hill 6 | * SPDX-License-Identifier: Apache-2.0 7 | * 8 | * Licensed under the Apache License, Version 2.0 (the "License"); 9 | * you may not use this file except in compliance with the License. 10 | * You may obtain a copy of the License at 11 | * 12 | * http://www.apache.org/licenses/LICENSE-2.0 13 | * 14 | * Unless required by applicable law or agreed to in writing, software 15 | * distributed under the License is distributed on an "AS IS" BASIS, 16 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 17 | * See the License for the specific language governing permissions and 18 | * limitations under the License. 19 | * 20 | * Please send all bug reports to . 21 | * 22 | * The authors may be contacted via: 23 | * 24 | * Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha 25 | * Dept. of Computer Science 26 | * 201 S. Columbia St. 27 | * Frederick P. Brooks, Jr. Computer Science Bldg. 28 | * Chapel Hill, N.C. 27599-3175 29 | * United States of America 30 | * 31 | * 32 | */ 33 | 34 | using UnityEngine; 35 | 36 | namespace RVO 37 | { 38 | /** 39 | * Defines a directed line. 40 | */ 41 | public struct Line 42 | { 43 | public Vector2 direction; 44 | public Vector2 point; 45 | } 46 | } 47 | -------------------------------------------------------------------------------- /EdgeObstacle.cs: -------------------------------------------------------------------------------- 1 | using RVO; 2 | using System.Collections; 3 | using System.Collections.Generic; 4 | using UnityEngine; 5 | 6 | public class EdgeObstacle : ShapeObstacleBase 7 | { 8 | [SerializeField] bool closed = true; 9 | [SerializeField] List points; 10 | 11 | protected override void CalculatePoints() 12 | { 13 | if (points == null) return; 14 | var center = transform.position; 15 | var rotation = Quaternion.Euler(0f, transform.eulerAngles.y, 0f); 16 | var scale = transform.lossyScale; 17 | for (int i = 0; i < points.Count; i++) 18 | { 19 | var point = points[i]; 20 | point.x *= scale.x; 21 | point.z *= scale.z; 22 | point = center + rotation * point; 23 | mCacheVertexList.Add(new Vector2(point.x, point.z)); 24 | #if UNITY_EDITOR 25 | mEditorDebugVertexList.Add(point); 26 | #endif 27 | } 28 | 29 | if (closed && mCacheVertexList.Count > 2) 30 | { 31 | mCacheVertexList.Add(mCacheVertexList[0]); 32 | #if UNITY_EDITOR 33 | mEditorDebugVertexList.Add(mEditorDebugVertexList[0]); 34 | #endif 35 | } 36 | } 37 | 38 | protected override void AddObstacles() 39 | { 40 | for (int i = 0; i < mCacheVertexList.Count - 1; i++) 41 | { 42 | int obsId = Simulator.Instance.addObstacle(new List() { mCacheVertexList[i], mCacheVertexList[i + 1] }); 43 | } 44 | Simulator.Instance.processObstacles(); 45 | } 46 | } 47 | -------------------------------------------------------------------------------- /Obstacle.cs: -------------------------------------------------------------------------------- 1 | /* 2 | * Obstacle.cs 3 | * RVO2 Library C# 4 | * 5 | * SPDX-FileCopyrightText: 2008 University of North Carolina at Chapel Hill 6 | * SPDX-License-Identifier: Apache-2.0 7 | * 8 | * Licensed under the Apache License, Version 2.0 (the "License"); 9 | * you may not use this file except in compliance with the License. 10 | * You may obtain a copy of the License at 11 | * 12 | * http://www.apache.org/licenses/LICENSE-2.0 13 | * 14 | * Unless required by applicable law or agreed to in writing, software 15 | * distributed under the License is distributed on an "AS IS" BASIS, 16 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 17 | * See the License for the specific language governing permissions and 18 | * limitations under the License. 19 | * 20 | * Please send all bug reports to . 21 | * 22 | * The authors may be contacted via: 23 | * 24 | * Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha 25 | * Dept. of Computer Science 26 | * 201 S. Columbia St. 27 | * Frederick P. Brooks, Jr. Computer Science Bldg. 28 | * Chapel Hill, N.C. 27599-3175 29 | * United States of America 30 | * 31 | * 32 | */ 33 | 34 | using UnityEngine; 35 | 36 | namespace RVO 37 | { 38 | /** 39 | * Defines static obstacles in the simulation. 40 | */ 41 | internal class Obstacle 42 | { 43 | 44 | internal Obstacle next_; 45 | internal Obstacle previous_; 46 | internal Vector2 direction_; 47 | internal Vector2 point_; 48 | internal int id_; 49 | internal bool convex_; 50 | } 51 | } 52 | -------------------------------------------------------------------------------- /ShapeObstacleBase.cs: -------------------------------------------------------------------------------- 1 | using RVO; 2 | using System; 3 | using System.Collections; 4 | using System.Collections.Generic; 5 | using UnityEngine; 6 | 7 | public abstract class ShapeObstacleBase : MonoBehaviour 8 | { 9 | protected List mCacheVertexList = new List(); 10 | #if UNITY_EDITOR 11 | protected List mEditorDebugVertexList = new List(); 12 | private void OnDrawGizmos() 13 | { 14 | if (mCacheVertexList != null && mEditorDebugVertexList.Count > 2) 15 | { 16 | Gizmos.color = Color.red; 17 | for (int i = 0; i < mCacheVertexList.Count - 1; ++i) 18 | { 19 | Gizmos.DrawLine(mEditorDebugVertexList[i], mEditorDebugVertexList[i + 1]); 20 | } 21 | int vCount = mEditorDebugVertexList.Count; 22 | Gizmos.DrawLine(mEditorDebugVertexList[vCount - 1], mEditorDebugVertexList[0]); 23 | 24 | } 25 | } 26 | private void OnValidate() 27 | { 28 | GenerateVertex(); 29 | } 30 | #endif 31 | internal void GenerateVertex(bool autoAddRvoSystem = false) 32 | { 33 | mCacheVertexList.Clear(); 34 | #if UNITY_EDITOR 35 | mEditorDebugVertexList.Clear(); 36 | #endif 37 | 38 | CalculatePoints(); 39 | if (autoAddRvoSystem && mCacheVertexList.Count > 1) 40 | { 41 | AddObstacles(); 42 | } 43 | } 44 | 45 | protected virtual void CalculatePoints() 46 | { 47 | 48 | } 49 | 50 | protected virtual void AddObstacles() 51 | { 52 | int obsId = Simulator.Instance.addObstacle(mCacheVertexList); 53 | Simulator.Instance.processObstacles(); 54 | } 55 | } 56 | -------------------------------------------------------------------------------- /BoxObstacle.cs: -------------------------------------------------------------------------------- 1 | using RVO; 2 | using System.Collections; 3 | using System.Collections.Generic; 4 | using UnityEngine; 5 | 6 | /// 7 | /// BoxCollider转化为RVO障碍物 8 | /// 9 | public class BoxObstacle : ShapeObstacleBase 10 | { 11 | [SerializeField] UnityEngine.Vector2 boxSize = UnityEngine.Vector2.one; 12 | [SerializeField] UnityEngine.Vector2 center = UnityEngine.Vector2.one; 13 | 14 | protected override void CalculatePoints() 15 | { 16 | var curPos = transform.position; 17 | var halfSize = new UnityEngine.Vector2(transform.lossyScale.x, transform.lossyScale.z) * boxSize * 0.5f; 18 | var rotation = Quaternion.Euler(0f, transform.eulerAngles.y, 0f); 19 | var pointA = curPos + rotation * (Vector3.right * (halfSize.x + center.x) + Vector3.forward * (halfSize.y + center.y)); 20 | var pointB = curPos + rotation * (Vector3.left * (halfSize.x - center.x) + Vector3.forward * (halfSize.y + center.y)); 21 | var pointC = curPos + rotation * (Vector3.left * (halfSize.x - center.x) + Vector3.back * (halfSize.y - center.y)); 22 | var pointD = curPos + rotation * (Vector3.right * (halfSize.x + center.x) + Vector3.back * (halfSize.y - center.y)); 23 | 24 | mCacheVertexList.Add(new Vector2(pointA.x, pointA.z)); 25 | mCacheVertexList.Add(new Vector2(pointB.x, pointB.z)); 26 | mCacheVertexList.Add(new Vector2(pointC.x, pointC.z)); 27 | mCacheVertexList.Add(new Vector2(pointD.x, pointD.z)); 28 | #if UNITY_EDITOR 29 | mEditorDebugVertexList.Add(pointA); 30 | mEditorDebugVertexList.Add(pointB); 31 | mEditorDebugVertexList.Add(pointC); 32 | mEditorDebugVertexList.Add(pointD); 33 | #endif 34 | } 35 | } 36 | -------------------------------------------------------------------------------- /RVONavAgent.cs: -------------------------------------------------------------------------------- 1 | using GameFramework; 2 | using RVO; 3 | using System; 4 | using UnityEngine; 5 | 6 | /// 7 | /// RVO2寻路对象 8 | /// 9 | public class RVONavAgent : IReference 10 | { 11 | readonly int GROUND_LAYER = LayerMask.GetMask("Ground"); 12 | const float MIN_FLOAT_CHECK = 0.01f; 13 | public int Id { get; private set; } 14 | 15 | private float m_AvoidanceWeight = 0.5f; 16 | /// 17 | /// 移动速度 18 | /// 19 | public Vector2 Velocity 20 | { 21 | get => Simulator.Instance.getAgentVelocity(Id); 22 | } 23 | private Vector2 m_MoveDirection; 24 | /// 25 | /// 移动方向 26 | /// 27 | public Vector2 MoveDirection 28 | { 29 | get => m_MoveDirection; 30 | set 31 | { 32 | m_MoveDirection = value; 33 | Simulator.Instance.setAgentPrefVelocity(Id, m_MoveDirection); 34 | } 35 | } 36 | private float m_Radius; 37 | /// 38 | /// 碰撞体半径 39 | /// 40 | public float Radius 41 | { 42 | get => m_Radius; 43 | set 44 | { 45 | m_Radius = value; 46 | Simulator.Instance.setAgentRadius(Id, m_Radius); 47 | } 48 | } 49 | private float m_MaxSpeed; 50 | /// 51 | /// 最大移动速度 52 | /// 53 | public float MaxSpeed 54 | { 55 | get => m_MaxSpeed; 56 | set 57 | { 58 | m_MaxSpeed = value; 59 | Simulator.Instance.setAgentMaxSpeed(Id, m_MaxSpeed / LevelEntity.m_SyncStepTime); 60 | } 61 | } 62 | public Vector2 Position 63 | { 64 | get => Simulator.Instance.getAgentPosition(Id); 65 | } 66 | public float AvoidanceWeight 67 | { 68 | get => Simulator.Instance.getAgentWeight(Id); 69 | set 70 | { 71 | Simulator.Instance.setAgentWeight(Id, Mathf.Clamp(value, 0.01f, 1f)); 72 | } 73 | } 74 | public bool IsReached { get; private set; } 75 | 76 | private bool m_StopMoving; 77 | public bool StopMoving 78 | { 79 | get => m_StopMoving; 80 | set 81 | { 82 | if (m_StopMoving == value) return; 83 | m_StopMoving = value; 84 | if (m_StopMoving) 85 | { 86 | Simulator.Instance.setAgentWeight(Id, 0.01f); 87 | } 88 | else 89 | { 90 | Simulator.Instance.setAgentWeight(Id, m_AvoidanceWeight); 91 | } 92 | } 93 | } 94 | private Transform m_Transform; 95 | private Transform m_MoveTarget; //移动目标节点 96 | private Vector3 m_MoveTargetPos; //移动目标点 97 | private float m_RotateSmoothSpeed = 10f; 98 | private float m_MoveSmoothSpeed = 20f; 99 | 100 | private Vector3 mCurrentPosition; 101 | private Quaternion mCurrentRotation; 102 | public static RVONavAgent Acquire(Transform transform, float radius, float maxSpeed) 103 | { 104 | var inst = ReferencePool.Acquire(); 105 | inst.m_Transform = transform; 106 | inst.Id = Simulator.Instance.addAgent(new Vector2(transform.position.x, transform.position.z)); 107 | inst.Radius = radius; 108 | inst.MaxSpeed = maxSpeed; 109 | inst.m_StopMoving = false; 110 | inst.mCurrentPosition = transform.position; 111 | inst.mCurrentRotation = transform.rotation; 112 | GF.Event.Fire(null, ReferencePool.Acquire().Fill(inst)); 113 | return inst; 114 | } 115 | /// 116 | /// 设置Agent移动和转向平滑度 117 | /// 118 | /// 119 | /// 120 | public void SetAgentActionSmooth(float moveSmooth, float rotateSmooth) 121 | { 122 | this.m_MoveSmoothSpeed = moveSmooth; 123 | this.m_RotateSmoothSpeed = rotateSmooth; 124 | } 125 | public void LogicUpdate(float elapseSeconds) 126 | { 127 | if (m_MoveTarget == null) return; 128 | 129 | m_MoveTargetPos = m_MoveTarget.position; 130 | m_Transform.position = mCurrentPosition; 131 | m_Transform.rotation = mCurrentRotation; 132 | } 133 | /// 134 | /// 此方法由子线程调用,计算当前位置和方向 135 | /// 136 | /// 137 | public void UpdatePositionAndRotation(float elapseSeconds) 138 | { 139 | var rvoPos = this.Position; 140 | var nextPos = mCurrentPosition; 141 | nextPos.x = rvoPos.x; 142 | nextPos.z = rvoPos.y; 143 | mCurrentPosition = Vector3.Lerp(mCurrentPosition, nextPos, elapseSeconds * m_MoveSmoothSpeed); 144 | if (StopMoving) 145 | { 146 | MoveDirection = Vector2.zero; 147 | return; 148 | } 149 | 150 | var offset = m_MoveTargetPos - mCurrentPosition; 151 | offset.y = 0; 152 | 153 | rvoPos.x = offset.x; 154 | rvoPos.y = offset.z; 155 | this.MoveDirection = RVOMath.normalize(rvoPos); 156 | var rvoForward = this.Velocity; 157 | if (RVOMath.absSq(rvoForward) > MIN_FLOAT_CHECK) 158 | { 159 | rvoForward = RVOMath.normalize(rvoForward); 160 | nextPos.Set(rvoForward.x, 0, rvoForward.y); 161 | } 162 | else 163 | { 164 | offset.y = 0; 165 | nextPos = offset.normalized; 166 | } 167 | mCurrentRotation = Quaternion.Lerp(mCurrentRotation, Quaternion.LookRotation(nextPos), elapseSeconds * m_RotateSmoothSpeed); 168 | } 169 | public void Clear() 170 | { 171 | Simulator.Instance.removeAgent(Id); 172 | Id = -1; 173 | m_Radius = 1; 174 | m_MaxSpeed = 0; 175 | } 176 | 177 | internal void SetMoveTarget(Transform transform) 178 | { 179 | m_MoveTarget = transform; 180 | m_MoveTargetPos = transform.position; 181 | } 182 | internal void SetMoveTarget(Vector3 point) 183 | { 184 | m_MoveTarget = null; 185 | m_MoveTargetPos = point; 186 | } 187 | } 188 | -------------------------------------------------------------------------------- /CacheArray.cs: -------------------------------------------------------------------------------- 1 | using System.Buffers; 2 | using System; 3 | using System.Collections; 4 | using System.Collections.Generic; 5 | using GameFramework; 6 | using System.Collections.Concurrent; 7 | 8 | /// 9 | /// 数组池, 可以避免频繁new数组 10 | /// 11 | /// 12 | public class CacheArray : IList, IReference 13 | { 14 | public T this[int index] 15 | { 16 | get 17 | { 18 | lock (SyncRoot) 19 | { 20 | return _items[index]; 21 | } 22 | } 23 | set 24 | { 25 | lock (SyncRoot) 26 | { 27 | _items[index] = value; 28 | } 29 | } 30 | } 31 | 32 | private T[] _items; 33 | private int _size; 34 | 35 | public bool IsFixedSize => false; 36 | 37 | public bool IsReadOnly => false; 38 | 39 | public int Count => _size; 40 | 41 | public bool IsSynchronized => false; 42 | 43 | public object SyncRoot => _items; 44 | 45 | public CacheArray() 46 | { 47 | 48 | } 49 | public static CacheArray Acquire(int currentSize, int cacheSize) 50 | { 51 | var arr = ReferencePool.Acquire>(); 52 | arr._items = ArrayPool.Shared.Rent(cacheSize); 53 | arr._size = currentSize; 54 | return arr; 55 | } 56 | /// 57 | /// 不用时释放并归还数组到数组池 58 | /// 59 | public void Release() 60 | { 61 | lock (SyncRoot) 62 | { 63 | ReferencePool.Release(this); 64 | } 65 | } 66 | public void Resize(int capacity) 67 | { 68 | lock (SyncRoot) 69 | { 70 | if (_size > capacity) 71 | { 72 | Array.Clear(_items, capacity - 1, _size - capacity); 73 | _size = capacity; 74 | return; 75 | } 76 | 77 | if (capacity > _size) 78 | { 79 | int newArrLength = capacity; 80 | var newArr = ArrayPool.Shared.Rent(newArrLength); 81 | Array.Copy(_items, newArr, _size); 82 | ArrayPool.Shared.Return(_items, true); 83 | _items = newArr; 84 | _size = newArrLength; 85 | } 86 | } 87 | } 88 | public void Add(T item) 89 | { 90 | lock (SyncRoot) 91 | { 92 | if (_items.Length > _size) 93 | { 94 | _items[_size++] = item; 95 | return; 96 | } 97 | 98 | int newArrLength = _size + 1; 99 | var newArr = ArrayPool.Shared.Rent(newArrLength); 100 | Array.Copy(_items, newArr, _size); 101 | ArrayPool.Shared.Return(_items, true); 102 | 103 | _items = newArr; 104 | _size = newArrLength; 105 | } 106 | 107 | } 108 | 109 | public void Clear() 110 | { 111 | lock (SyncRoot) 112 | { 113 | if (_size > 0 && _items != null) 114 | { 115 | ArrayPool.Shared.Return(_items, true); 116 | _size = 0; 117 | } 118 | } 119 | } 120 | 121 | public bool Contains(T item) 122 | { 123 | lock (SyncRoot) 124 | { 125 | if (item == null) 126 | { 127 | for (int i = 0; i < _size; i++) 128 | { 129 | if (_items[i] == null) 130 | { 131 | return true; 132 | } 133 | } 134 | 135 | return false; 136 | } 137 | 138 | EqualityComparer @default = EqualityComparer.Default; 139 | for (int j = 0; j < _size; j++) 140 | { 141 | if (@default.Equals(_items[j], item)) 142 | { 143 | return true; 144 | } 145 | } 146 | 147 | return false; 148 | } 149 | 150 | } 151 | 152 | public void CopyTo(T[] array, int arrayIndex) 153 | { 154 | lock (SyncRoot) 155 | { 156 | Array.Copy(_items, 0, array, arrayIndex, _size); 157 | } 158 | } 159 | 160 | public int IndexOf(T item) 161 | { 162 | lock (SyncRoot) 163 | { 164 | return System.Array.IndexOf(_items, item); 165 | } 166 | } 167 | 168 | public void Insert(int index, T item) 169 | { 170 | lock (SyncRoot) 171 | { 172 | if (_items.Length > _size) 173 | { 174 | for (int i = _size - 1; i >= index; i--) 175 | { 176 | _items[i + 1] = _items[i]; 177 | } 178 | _items[index] = item; 179 | return; 180 | } 181 | 182 | int newArrLength = _size + 1; 183 | var newArr = ArrayPool.Shared.Rent(newArrLength); 184 | for (int i = _size - 1; i >= index; i--) 185 | { 186 | newArr[i + 1] = _items[i]; 187 | } 188 | newArr[index] = item; 189 | ArrayPool.Shared.Return(_items, true); 190 | _items = newArr; 191 | _size = newArrLength; 192 | } 193 | 194 | } 195 | 196 | public bool Remove(T item) 197 | { 198 | lock (SyncRoot) 199 | { 200 | int idx = Array.IndexOf(_items, item); 201 | if (idx >= 0 && idx < _size) 202 | { 203 | RemoveAt(idx); 204 | return true; 205 | } 206 | return false; 207 | } 208 | 209 | } 210 | 211 | public void RemoveAt(int index) 212 | { 213 | lock (SyncRoot) 214 | { 215 | if (index == _size - 1) 216 | { 217 | _items[index] = default; 218 | _size -= 1; 219 | return; 220 | } 221 | for (int i = index + 1; i < _size; i++) 222 | { 223 | _items[i - 1] = _items[i]; 224 | } 225 | _size -= 1; 226 | } 227 | } 228 | 229 | IEnumerator IEnumerable.GetEnumerator() 230 | { 231 | throw new NotSupportedException("Not Supported Enumerator"); 232 | } 233 | 234 | public IEnumerator GetEnumerator() 235 | { 236 | throw new NotSupportedException("Not Supported Enumerator"); 237 | } 238 | } -------------------------------------------------------------------------------- /RVOMath.cs: -------------------------------------------------------------------------------- 1 | /* 2 | * RVOMath.cs 3 | * RVO2 Library C# 4 | * 5 | * SPDX-FileCopyrightText: 2008 University of North Carolina at Chapel Hill 6 | * SPDX-License-Identifier: Apache-2.0 7 | * 8 | * Licensed under the Apache License, Version 2.0 (the "License"); 9 | * you may not use this file except in compliance with the License. 10 | * You may obtain a copy of the License at 11 | * 12 | * http://www.apache.org/licenses/LICENSE-2.0 13 | * 14 | * Unless required by applicable law or agreed to in writing, software 15 | * distributed under the License is distributed on an "AS IS" BASIS, 16 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 17 | * See the License for the specific language governing permissions and 18 | * limitations under the License. 19 | * 20 | * Please send all bug reports to . 21 | * 22 | * The authors may be contacted via: 23 | * 24 | * Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha 25 | * Dept. of Computer Science 26 | * 201 S. Columbia St. 27 | * Frederick P. Brooks, Jr. Computer Science Bldg. 28 | * Chapel Hill, N.C. 27599-3175 29 | * United States of America 30 | * 31 | * 32 | */ 33 | 34 | using System; 35 | using UnityEngine; 36 | using UnityEngine.UIElements; 37 | 38 | namespace RVO 39 | { 40 | /** 41 | * Contains functions and constants used in multiple classes. 42 | * 43 | */ 44 | public struct RVOMath 45 | { 46 | /** 47 | * A sufficiently small positive number. 48 | */ 49 | internal const float RVO_EPSILON = 0.00001f; 50 | 51 | /** 52 | * Computes the length of a specified two-dimensional vector. 53 | * 54 | * 55 | * The two-dimensional vector whose length is to be 56 | * computed. 57 | * The length of the two-dimensional vector. 58 | */ 59 | public static float abs(Vector2 vector) 60 | { 61 | return sqrt(absSq(vector)); 62 | } 63 | 64 | /** 65 | * Computes the squared length of a specified two-dimensional 66 | * vector. 67 | * 68 | * The squared length of the two-dimensional vector. 69 | * 70 | * The two-dimensional vector whose squared length 71 | * is to be computed. 72 | */ 73 | public static float absSq(Vector2 vector) 74 | { 75 | return vector.sqrMagnitude; 76 | } 77 | 78 | /** 79 | * Computes the normalization of the specified two-dimensional 80 | * vector. 81 | * 82 | * The normalization of the two-dimensional vector. 83 | * 84 | * The two-dimensional vector whose normalization 85 | * is to be computed. 86 | */ 87 | public static Vector2 normalize(Vector2 vector) 88 | { 89 | return vector.normalized; 90 | } 91 | 92 | /** 93 | * Computes the determinant of a two-dimensional square matrix 94 | * with rows consisting of the specified two-dimensional vectors. 95 | * 96 | * 97 | * The determinant of the two-dimensional square matrix. 98 | * 99 | * 100 | * The top row of the two-dimensional square 101 | * matrix. 102 | * The bottom row of the two-dimensional square 103 | * matrix. 104 | */ 105 | internal static float det(Vector2 vector1, Vector2 vector2) 106 | { 107 | return vector1.x * vector2.y - vector1.y * vector2.x; 108 | } 109 | 110 | /** 111 | * Computes the squared distance from a line segment with the 112 | * specified endpoints to a specified point. 113 | * 114 | * The squared distance from the line segment to the point. 115 | * 116 | * 117 | * The first endpoint of the line segment. 118 | * The second endpoint of the line segment. 119 | * 120 | * The point to which the squared distance is to 121 | * be calculated. 122 | */ 123 | internal static float distSqPointLineSegment(Vector2 vector1, Vector2 vector2, Vector2 vector3) 124 | { 125 | float value = absSq(vector2 - vector1); 126 | float r = value > 1E-05f ? Vector2.Dot((vector3 - vector1), (vector2 - vector1)) / value : 0f; 127 | 128 | if (r < 0.0f) 129 | { 130 | return absSq(vector3 - vector1); 131 | } 132 | 133 | if (r > 1.0f) 134 | { 135 | return absSq(vector3 - vector2); 136 | } 137 | 138 | return absSq(vector3 - (vector1 + r * (vector2 - vector1))); 139 | } 140 | 141 | /** 142 | * Computes the absolute value of a float. 143 | * 144 | * The absolute value of the float. 145 | * 146 | * The float of which to compute the absolute 147 | * value. 148 | */ 149 | internal static float fabs(float scalar) 150 | { 151 | return Math.Abs(scalar); 152 | } 153 | 154 | /** 155 | * Computes the signed distance from a line connecting the 156 | * specified points to a specified point. 157 | * 158 | * Positive when the point c lies to the left of the line ab. 159 | * 160 | * 161 | * The first point on the line. 162 | * The second point on the line. 163 | * The point to which the signed distance is to be 164 | * calculated. 165 | */ 166 | internal static float leftOf(Vector2 a, Vector2 b, Vector2 c) 167 | { 168 | return det(a - c, b - a); 169 | } 170 | 171 | /** 172 | * Computes the square of a float. 173 | * 174 | * The square of the float. 175 | * 176 | * The float to be squared. 177 | */ 178 | internal static float sqr(float scalar) 179 | { 180 | return scalar * scalar; 181 | } 182 | 183 | /** 184 | * Computes the square root of a float. 185 | * 186 | * The square root of the float. 187 | * 188 | * The float of which to compute the square root. 189 | * 190 | */ 191 | internal static float sqrt(float scalar) 192 | { 193 | return (float)Math.Sqrt(scalar); 194 | } 195 | } 196 | } 197 | -------------------------------------------------------------------------------- /KdTree.cs: -------------------------------------------------------------------------------- 1 | /* 2 | * KdTree.cs 3 | * RVO2 Library C# 4 | * 5 | * SPDX-FileCopyrightText: 2008 University of North Carolina at Chapel Hill 6 | * SPDX-License-Identifier: Apache-2.0 7 | * 8 | * Licensed under the Apache License, Version 2.0 (the "License"); 9 | * you may not use this file except in compliance with the License. 10 | * You may obtain a copy of the License at 11 | * 12 | * http://www.apache.org/licenses/LICENSE-2.0 13 | * 14 | * Unless required by applicable law or agreed to in writing, software 15 | * distributed under the License is distributed on an "AS IS" BASIS, 16 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 17 | * See the License for the specific language governing permissions and 18 | * limitations under the License. 19 | * 20 | * Please send all bug reports to . 21 | * 22 | * The authors may be contacted via: 23 | * 24 | * Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha 25 | * Dept. of Computer Science 26 | * 201 S. Columbia St. 27 | * Frederick P. Brooks, Jr. Computer Science Bldg. 28 | * Chapel Hill, N.C. 27599-3175 29 | * United States of America 30 | * 31 | * 32 | */ 33 | 34 | using System; 35 | using System.Buffers; 36 | using System.Collections.Concurrent; 37 | using System.Collections.Generic; 38 | using System.Threading; 39 | using UnityEngine; 40 | 41 | namespace RVO 42 | { 43 | /** 44 | * Defines k-D trees for agents and static obstacles in the 45 | * simulation. 46 | */ 47 | internal class KdTree 48 | { 49 | /** 50 | * Defines a node of an agent k-D tree. 51 | */ 52 | private struct AgentTreeNode 53 | { 54 | internal int begin_; 55 | internal int end_; 56 | internal int left_; 57 | internal int right_; 58 | internal float maxX_; 59 | internal float maxY_; 60 | internal float minX_; 61 | internal float minY_; 62 | } 63 | 64 | /** 65 | * Defines a pair of scalar values. 66 | */ 67 | private struct FloatPair 68 | { 69 | private readonly float a_; 70 | private readonly float b_; 71 | 72 | /** 73 | * Constructs and initializes a pair of scalar 74 | * values. 75 | * 76 | * The first scalar value. 77 | * The second scalar value. 78 | */ 79 | internal FloatPair(float a, float b) 80 | { 81 | a_ = a; 82 | b_ = b; 83 | } 84 | 85 | /** 86 | * Returns true if the first pair of scalar values is less 87 | * than the second pair of scalar values. 88 | * 89 | * True if the first pair of scalar values is less than the 90 | * second pair of scalar values. 91 | * 92 | * The first pair of scalar values. 93 | * The second pair of scalar values. 94 | */ 95 | public static bool operator <(FloatPair pair1, FloatPair pair2) 96 | { 97 | return pair1.a_ < pair2.a_ || !(pair2.a_ < pair1.a_) && pair1.b_ < pair2.b_; 98 | } 99 | 100 | /** 101 | * Returns true if the first pair of scalar values is less 102 | * than or equal to the second pair of scalar values. 103 | * 104 | * True if the first pair of scalar values is less than or 105 | * equal to the second pair of scalar values. 106 | * 107 | * The first pair of scalar values. 108 | * The second pair of scalar values. 109 | */ 110 | public static bool operator <=(FloatPair pair1, FloatPair pair2) 111 | { 112 | return (pair1.a_ == pair2.a_ && pair1.b_ == pair2.b_) || pair1 < pair2; 113 | } 114 | 115 | /** 116 | * Returns true if the first pair of scalar values is 117 | * greater than the second pair of scalar values. 118 | * 119 | * True if the first pair of scalar values is greater than 120 | * the second pair of scalar values. 121 | * 122 | * The first pair of scalar values. 123 | * The second pair of scalar values. 124 | */ 125 | public static bool operator >(FloatPair pair1, FloatPair pair2) 126 | { 127 | return !(pair1 <= pair2); 128 | } 129 | 130 | /** 131 | * Returns true if the first pair of scalar values is 132 | * greater than or equal to the second pair of scalar values. 133 | * 134 | * 135 | * True if the first pair of scalar values is greater than 136 | * or equal to the second pair of scalar values. 137 | * 138 | * The first pair of scalar values. 139 | * The second pair of scalar values. 140 | */ 141 | public static bool operator >=(FloatPair pair1, FloatPair pair2) 142 | { 143 | return !(pair1 < pair2); 144 | } 145 | } 146 | 147 | /** 148 | * Defines a node of an obstacle k-D tree. 149 | */ 150 | private class ObstacleTreeNode 151 | { 152 | internal Obstacle obstacle_; 153 | internal ObstacleTreeNode left_; 154 | internal ObstacleTreeNode right_; 155 | }; 156 | 157 | /** 158 | * The maximum size of an agent k-D tree leaf. 159 | */ 160 | private const int MAX_LEAF_SIZE = 10; 161 | 162 | private CacheArray agents_; 163 | private AgentTreeNode[] agentTree_; 164 | private ObstacleTreeNode obstacleTree_; 165 | private List obstacles_; 166 | 167 | /** 168 | * Builds an agent k-D tree. 169 | */ 170 | internal void buildAgentTree(CacheArray agentsCached) 171 | { 172 | agents_ = agentsCached; 173 | int treeLength = 2 * agents_.Count; 174 | if (agentTree_ == null || treeLength > agentTree_.Length) 175 | { 176 | if(agentTree_ != null) ArrayPool.Shared.Return(agentTree_, true); 177 | agentTree_ = ArrayPool.Shared.Rent(treeLength); 178 | } 179 | 180 | if (agents_.Count != 0) 181 | { 182 | buildAgentTreeRecursive(0, agents_.Count, 0); 183 | } 184 | } 185 | 186 | /** 187 | * Builds an obstacle k-D tree. 188 | */ 189 | internal void buildObstacleTree() 190 | { 191 | obstacleTree_ = new ObstacleTreeNode(); 192 | 193 | if (obstacles_ == null) 194 | { 195 | obstacles_ = new List(Simulator.Instance.obstacles_); 196 | } 197 | else 198 | { 199 | obstacles_.Clear(); 200 | obstacles_.AddRange(Simulator.Instance.obstacles_); 201 | } 202 | obstacleTree_ = buildObstacleTreeRecursive(obstacles_); 203 | } 204 | 205 | /** 206 | * Computes the agent neighbors of the specified agent. 207 | * 208 | * 209 | * The agent for which agent neighbors are to be 210 | * computed. 211 | * The squared range around the agent. 212 | */ 213 | internal void computeAgentNeighbors(Agent agent, ref float rangeSq) 214 | { 215 | queryAgentTreeRecursive(agent, ref rangeSq, 0); 216 | } 217 | 218 | /** 219 | * Computes the obstacle neighbors of the specified agent. 220 | * 221 | * 222 | * The agent for which obstacle neighbors are to be 223 | * computed. 224 | * The squared range around the agent. 225 | */ 226 | internal void computeObstacleNeighbors(Agent agent, float rangeSq) 227 | { 228 | queryObstacleTreeRecursive(agent, rangeSq, obstacleTree_); 229 | } 230 | 231 | /** 232 | * Queries the visibility between two points within a specified 233 | * radius. 234 | * 235 | * True if q1 and q2 are mutually visible within the radius; 236 | * false otherwise. 237 | * 238 | * The first point between which visibility is to be 239 | * tested. 240 | * The second point between which visibility is to be 241 | * tested. 242 | * The radius within which visibility is to be 243 | * tested. 244 | */ 245 | internal bool queryVisibility(Vector2 q1, Vector2 q2, float radius) 246 | { 247 | return queryVisibilityRecursive(q1, q2, radius, obstacleTree_); 248 | } 249 | 250 | /** 251 | * Recursive method for building an agent k-D tree. 252 | * 253 | * The beginning agent k-D tree node node index. 254 | * 255 | * The ending agent k-D tree node index. 256 | * The current agent k-D tree node index. 257 | */ 258 | private void buildAgentTreeRecursive(int begin, int end, int node) 259 | { 260 | agentTree_[node].begin_ = begin; 261 | agentTree_[node].end_ = end; 262 | agentTree_[node].minX_ = agentTree_[node].maxX_ = agents_[begin].position_.x; 263 | agentTree_[node].minY_ = agentTree_[node].maxY_ = agents_[begin].position_.y; 264 | 265 | for (int i = begin + 1; i < end; ++i) 266 | { 267 | agentTree_[node].maxX_ = Math.Max(agentTree_[node].maxX_, agents_[i].position_.x); 268 | agentTree_[node].minX_ = Math.Min(agentTree_[node].minX_, agents_[i].position_.x); 269 | agentTree_[node].maxY_ = Math.Max(agentTree_[node].maxY_, agents_[i].position_.y); 270 | agentTree_[node].minY_ = Math.Min(agentTree_[node].minY_, agents_[i].position_.y); 271 | } 272 | 273 | if (end - begin > MAX_LEAF_SIZE) 274 | { 275 | /* No leaf node. */ 276 | bool isVertical = agentTree_[node].maxX_ - agentTree_[node].minX_ > agentTree_[node].maxY_ - agentTree_[node].minY_; 277 | float splitValue = 0.5f * (isVertical ? agentTree_[node].maxX_ + agentTree_[node].minX_ : agentTree_[node].maxY_ + agentTree_[node].minY_); 278 | 279 | int left = begin; 280 | int right = end; 281 | 282 | while (left < right) 283 | { 284 | while (left < right && (isVertical ? agents_[left].position_.x : agents_[left].position_.y) < splitValue) 285 | { 286 | ++left; 287 | } 288 | 289 | while (right > left && (isVertical ? agents_[right - 1].position_.x : agents_[right - 1].position_.y) >= splitValue) 290 | { 291 | --right; 292 | } 293 | 294 | if (left < right) 295 | { 296 | Agent tempAgent = agents_[left]; 297 | agents_[left] = agents_[right - 1]; 298 | agents_[right - 1] = tempAgent; 299 | ++left; 300 | --right; 301 | } 302 | } 303 | 304 | int leftSize = left - begin; 305 | 306 | if (leftSize == 0) 307 | { 308 | ++leftSize; 309 | ++left; 310 | } 311 | 312 | agentTree_[node].left_ = node + 1; 313 | agentTree_[node].right_ = node + 2 * leftSize; 314 | 315 | buildAgentTreeRecursive(begin, left, agentTree_[node].left_); 316 | buildAgentTreeRecursive(left, end, agentTree_[node].right_); 317 | } 318 | } 319 | 320 | /** 321 | * Recursive method for building an obstacle k-D tree. 322 | * 323 | * 324 | * An obstacle k-D tree node. 325 | * 326 | * A list of obstacles. 327 | */ 328 | private ObstacleTreeNode buildObstacleTreeRecursive(IList obstacles) 329 | { 330 | if (obstacles.Count == 0) 331 | { 332 | return null; 333 | } 334 | 335 | ObstacleTreeNode node = new(); 336 | 337 | int optimalSplit = 0; 338 | int minLeft = obstacles.Count; 339 | int minRight = obstacles.Count; 340 | 341 | for (int i = 0; i < obstacles.Count; ++i) 342 | { 343 | int leftSize = 0; 344 | int rightSize = 0; 345 | 346 | Obstacle obstacleI1 = obstacles[i]; 347 | Obstacle obstacleI2 = obstacleI1.next_; 348 | 349 | /* Compute optimal split node. */ 350 | for (int j = 0; j < obstacles.Count; ++j) 351 | { 352 | if (i == j) 353 | { 354 | continue; 355 | } 356 | 357 | Obstacle obstacleJ1 = obstacles[j]; 358 | Obstacle obstacleJ2 = obstacleJ1.next_; 359 | 360 | float j1LeftOfI = RVOMath.leftOf(obstacleI1.point_, obstacleI2.point_, obstacleJ1.point_); 361 | float j2LeftOfI = RVOMath.leftOf(obstacleI1.point_, obstacleI2.point_, obstacleJ2.point_); 362 | 363 | if (j1LeftOfI >= -RVOMath.RVO_EPSILON && j2LeftOfI >= -RVOMath.RVO_EPSILON) 364 | { 365 | ++leftSize; 366 | } 367 | else if (j1LeftOfI <= RVOMath.RVO_EPSILON && j2LeftOfI <= RVOMath.RVO_EPSILON) 368 | { 369 | ++rightSize; 370 | } 371 | else 372 | { 373 | ++leftSize; 374 | ++rightSize; 375 | } 376 | 377 | if (new FloatPair(Math.Max(leftSize, rightSize), Math.Min(leftSize, rightSize)) >= new FloatPair(Math.Max(minLeft, minRight), Math.Min(minLeft, minRight))) 378 | { 379 | break; 380 | } 381 | } 382 | 383 | if (new FloatPair(Math.Max(leftSize, rightSize), Math.Min(leftSize, rightSize)) < new FloatPair(Math.Max(minLeft, minRight), Math.Min(minLeft, minRight))) 384 | { 385 | minLeft = leftSize; 386 | minRight = rightSize; 387 | optimalSplit = i; 388 | } 389 | } 390 | 391 | { 392 | /* Build split node. */ 393 | IList leftObstacles = new List(minLeft); 394 | 395 | for (int n = 0; n < minLeft; ++n) 396 | { 397 | leftObstacles.Add(null); 398 | } 399 | 400 | IList rightObstacles = new List(minRight); 401 | 402 | for (int n = 0; n < minRight; ++n) 403 | { 404 | rightObstacles.Add(null); 405 | } 406 | 407 | int leftCounter = 0; 408 | int rightCounter = 0; 409 | int i = optimalSplit; 410 | 411 | Obstacle obstacleI1 = obstacles[i]; 412 | Obstacle obstacleI2 = obstacleI1.next_; 413 | 414 | for (int j = 0; j < obstacles.Count; ++j) 415 | { 416 | if (i == j) 417 | { 418 | continue; 419 | } 420 | 421 | Obstacle obstacleJ1 = obstacles[j]; 422 | Obstacle obstacleJ2 = obstacleJ1.next_; 423 | 424 | float j1LeftOfI = RVOMath.leftOf(obstacleI1.point_, obstacleI2.point_, obstacleJ1.point_); 425 | float j2LeftOfI = RVOMath.leftOf(obstacleI1.point_, obstacleI2.point_, obstacleJ2.point_); 426 | 427 | if (j1LeftOfI >= -RVOMath.RVO_EPSILON && j2LeftOfI >= -RVOMath.RVO_EPSILON) 428 | { 429 | leftObstacles[leftCounter++] = obstacles[j]; 430 | } 431 | else if (j1LeftOfI <= RVOMath.RVO_EPSILON && j2LeftOfI <= RVOMath.RVO_EPSILON) 432 | { 433 | rightObstacles[rightCounter++] = obstacles[j]; 434 | } 435 | else 436 | { 437 | /* Split obstacle j. */ 438 | float t = RVOMath.det(obstacleI2.point_ - obstacleI1.point_, obstacleJ1.point_ - obstacleI1.point_) / RVOMath.det(obstacleI2.point_ - obstacleI1.point_, obstacleJ1.point_ - obstacleJ2.point_); 439 | 440 | Vector2 splitPoint = obstacleJ1.point_ + t * (obstacleJ2.point_ - obstacleJ1.point_); 441 | 442 | Obstacle newObstacle = new(); 443 | newObstacle.point_ = splitPoint; 444 | newObstacle.previous_ = obstacleJ1; 445 | newObstacle.next_ = obstacleJ2; 446 | newObstacle.convex_ = true; 447 | newObstacle.direction_ = obstacleJ1.direction_; 448 | 449 | newObstacle.id_ = Simulator.Instance.obstacles_.Count; 450 | 451 | Simulator.Instance.obstacles_.Add(newObstacle); 452 | 453 | obstacleJ1.next_ = newObstacle; 454 | obstacleJ2.previous_ = newObstacle; 455 | 456 | if (j1LeftOfI > 0.0f) 457 | { 458 | leftObstacles[leftCounter++] = obstacleJ1; 459 | rightObstacles[rightCounter++] = newObstacle; 460 | } 461 | else 462 | { 463 | rightObstacles[rightCounter++] = obstacleJ1; 464 | leftObstacles[leftCounter++] = newObstacle; 465 | } 466 | } 467 | } 468 | 469 | node.obstacle_ = obstacleI1; 470 | node.left_ = buildObstacleTreeRecursive(leftObstacles); 471 | node.right_ = buildObstacleTreeRecursive(rightObstacles); 472 | 473 | return node; 474 | } 475 | } 476 | 477 | /** 478 | * Recursive method for computing the agent neighbors of the 479 | * specified agent. 480 | * 481 | * The agent for which agent neighbors are to be 482 | * computed. 483 | * The squared range around the agent. 484 | * The current agent k-D tree node index. 485 | */ 486 | private void queryAgentTreeRecursive(Agent agent, ref float rangeSq, int node) 487 | { 488 | if (agentTree_[node].end_ - agentTree_[node].begin_ <= MAX_LEAF_SIZE) 489 | { 490 | for (int i = agentTree_[node].begin_; i < agentTree_[node].end_; ++i) 491 | { 492 | agent.insertAgentNeighbor(agents_[i], ref rangeSq); 493 | } 494 | } 495 | else 496 | { 497 | float distSqLeft = RVOMath.sqr(Math.Max(0.0f, agentTree_[agentTree_[node].left_].minX_ - agent.position_.x)) + RVOMath.sqr(Math.Max(0.0f, agent.position_.x - agentTree_[agentTree_[node].left_].maxX_)) + RVOMath.sqr(Math.Max(0.0f, agentTree_[agentTree_[node].left_].minY_ - agent.position_.y)) + RVOMath.sqr(Math.Max(0.0f, agent.position_.y - agentTree_[agentTree_[node].left_].maxY_)); 498 | float distSqRight = RVOMath.sqr(Math.Max(0.0f, agentTree_[agentTree_[node].right_].minX_ - agent.position_.x)) + RVOMath.sqr(Math.Max(0.0f, agent.position_.x - agentTree_[agentTree_[node].right_].maxX_)) + RVOMath.sqr(Math.Max(0.0f, agentTree_[agentTree_[node].right_].minY_ - agent.position_.y)) + RVOMath.sqr(Math.Max(0.0f, agent.position_.y - agentTree_[agentTree_[node].right_].maxY_)); 499 | 500 | if (distSqLeft < distSqRight) 501 | { 502 | if (distSqLeft < rangeSq) 503 | { 504 | queryAgentTreeRecursive(agent, ref rangeSq, agentTree_[node].left_); 505 | 506 | if (distSqRight < rangeSq) 507 | { 508 | queryAgentTreeRecursive(agent, ref rangeSq, agentTree_[node].right_); 509 | } 510 | } 511 | } 512 | else 513 | { 514 | if (distSqRight < rangeSq) 515 | { 516 | queryAgentTreeRecursive(agent, ref rangeSq, agentTree_[node].right_); 517 | 518 | if (distSqLeft < rangeSq) 519 | { 520 | queryAgentTreeRecursive(agent, ref rangeSq, agentTree_[node].left_); 521 | } 522 | } 523 | } 524 | 525 | } 526 | } 527 | 528 | /** 529 | * Recursive method for computing the obstacle neighbors of the 530 | * specified agent. 531 | * 532 | * The agent for which obstacle neighbors are to be 533 | * computed. 534 | * The squared range around the agent. 535 | * The current obstacle k-D node. 536 | */ 537 | private void queryObstacleTreeRecursive(Agent agent, float rangeSq, ObstacleTreeNode node) 538 | { 539 | if (node != null) 540 | { 541 | Obstacle obstacle1 = node.obstacle_; 542 | Obstacle obstacle2 = obstacle1.next_; 543 | 544 | float agentLeftOfLine = RVOMath.leftOf(obstacle1.point_, obstacle2.point_, agent.position_); 545 | 546 | queryObstacleTreeRecursive(agent, rangeSq, agentLeftOfLine >= 0.0f ? node.left_ : node.right_); 547 | 548 | float distSqLine = RVOMath.sqr(agentLeftOfLine) / RVOMath.absSq(obstacle2.point_ - obstacle1.point_); 549 | 550 | if (distSqLine < rangeSq) 551 | { 552 | if (agentLeftOfLine < 0.0f) 553 | { 554 | /* 555 | * Try obstacle at this node only if agent is on right side of 556 | * obstacle (and can see obstacle). 557 | */ 558 | agent.insertObstacleNeighbor(node.obstacle_, rangeSq); 559 | } 560 | 561 | /* Try other side of line. */ 562 | queryObstacleTreeRecursive(agent, rangeSq, agentLeftOfLine >= 0.0f ? node.right_ : node.left_); 563 | } 564 | } 565 | } 566 | 567 | /** 568 | * Recursive method for querying the visibility between two 569 | * points within a specified radius. 570 | * 571 | * True if q1 and q2 are mutually visible within the radius; 572 | * false otherwise. 573 | * 574 | * The first point between which visibility is to be 575 | * tested. 576 | * The second point between which visibility is to be 577 | * tested. 578 | * The radius within which visibility is to be 579 | * tested. 580 | * The current obstacle k-D node. 581 | */ 582 | private bool queryVisibilityRecursive(Vector2 q1, Vector2 q2, float radius, ObstacleTreeNode node) 583 | { 584 | if (node == null) 585 | { 586 | return true; 587 | } 588 | 589 | Obstacle obstacle1 = node.obstacle_; 590 | Obstacle obstacle2 = obstacle1.next_; 591 | 592 | float q1LeftOfI = RVOMath.leftOf(obstacle1.point_, obstacle2.point_, q1); 593 | float q2LeftOfI = RVOMath.leftOf(obstacle1.point_, obstacle2.point_, q2); 594 | float invLengthI = 1.0f / RVOMath.absSq(obstacle2.point_ - obstacle1.point_); 595 | 596 | if (q1LeftOfI >= 0.0f && q2LeftOfI >= 0.0f) 597 | { 598 | return queryVisibilityRecursive(q1, q2, radius, node.left_) && ((RVOMath.sqr(q1LeftOfI) * invLengthI >= RVOMath.sqr(radius) && RVOMath.sqr(q2LeftOfI) * invLengthI >= RVOMath.sqr(radius)) || queryVisibilityRecursive(q1, q2, radius, node.right_)); 599 | } 600 | 601 | if (q1LeftOfI <= 0.0f && q2LeftOfI <= 0.0f) 602 | { 603 | return queryVisibilityRecursive(q1, q2, radius, node.right_) && ((RVOMath.sqr(q1LeftOfI) * invLengthI >= RVOMath.sqr(radius) && RVOMath.sqr(q2LeftOfI) * invLengthI >= RVOMath.sqr(radius)) || queryVisibilityRecursive(q1, q2, radius, node.left_)); 604 | } 605 | 606 | if (q1LeftOfI >= 0.0f && q2LeftOfI <= 0.0f) 607 | { 608 | /* One can see through obstacle from left to right. */ 609 | return queryVisibilityRecursive(q1, q2, radius, node.left_) && queryVisibilityRecursive(q1, q2, radius, node.right_); 610 | } 611 | 612 | float point1LeftOfQ = RVOMath.leftOf(q1, q2, obstacle1.point_); 613 | float point2LeftOfQ = RVOMath.leftOf(q1, q2, obstacle2.point_); 614 | float invLengthQ = 1.0f / RVOMath.absSq(q2 - q1); 615 | 616 | return point1LeftOfQ * point2LeftOfQ >= 0.0f && RVOMath.sqr(point1LeftOfQ) * invLengthQ > RVOMath.sqr(radius) && RVOMath.sqr(point2LeftOfQ) * invLengthQ > RVOMath.sqr(radius) && queryVisibilityRecursive(q1, q2, radius, node.left_) && queryVisibilityRecursive(q1, q2, radius, node.right_); 617 | } 618 | } 619 | } 620 | -------------------------------------------------------------------------------- /Agent.cs: -------------------------------------------------------------------------------- 1 | /* 2 | * Agent.cs 3 | * RVO2 Library C# 4 | * 5 | * SPDX-FileCopyrightText: 2008 University of North Carolina at Chapel Hill 6 | * SPDX-License-Identifier: Apache-2.0 7 | * 8 | * Licensed under the Apache License, Version 2.0 (the "License"); 9 | * you may not use this file except in compliance with the License. 10 | * You may obtain a copy of the License at 11 | * 12 | * http://www.apache.org/licenses/LICENSE-2.0 13 | * 14 | * Unless required by applicable law or agreed to in writing, software 15 | * distributed under the License is distributed on an "AS IS" BASIS, 16 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 17 | * See the License for the specific language governing permissions and 18 | * limitations under the License. 19 | * 20 | * Please send all bug reports to . 21 | * 22 | * The authors may be contacted via: 23 | * 24 | * Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha 25 | * Dept. of Computer Science 26 | * 201 S. Columbia St. 27 | * Frederick P. Brooks, Jr. Computer Science Bldg. 28 | * Chapel Hill, N.C. 27599-3175 29 | * United States of America 30 | * 31 | * 32 | */ 33 | 34 | using System; 35 | using System.Collections.Generic; 36 | using UnityEngine; 37 | 38 | namespace RVO 39 | { 40 | /** 41 | * Defines an agent in the simulation. 42 | */ 43 | internal class Agent 44 | { 45 | internal IList> agentNeighbors_ = new List>(); 46 | internal IList> obstacleNeighbors_ = new List>(); 47 | internal IList orcaLines_ = new List(); 48 | internal Vector2 position_; 49 | internal Vector2 prefVelocity_; 50 | internal Vector2 velocity_; 51 | internal int id_ = 0; 52 | internal bool removed_ = false; 53 | internal int maxNeighbors_ = 0; 54 | internal float maxSpeed_ = 0.0f; 55 | internal float neighborDist_ = 0.0f; 56 | internal float radius_ = 0.0f; 57 | internal float timeHorizon_ = 0.0f; 58 | internal float timeHorizonObst_ = 0.0f; 59 | internal float weight_ = 0.5f; 60 | private Vector2 newVelocity_; 61 | 62 | /** 63 | * Computes the neighbors of this agent. 64 | */ 65 | internal void computeNeighbors() 66 | { 67 | obstacleNeighbors_.Clear(); 68 | float rangeSq = RVOMath.sqr(timeHorizonObst_ * maxSpeed_ + radius_); 69 | Simulator.Instance.kdTree_.computeObstacleNeighbors(this, rangeSq); 70 | 71 | agentNeighbors_.Clear(); 72 | 73 | if (maxNeighbors_ > 0) 74 | { 75 | rangeSq = RVOMath.sqr(neighborDist_); 76 | Simulator.Instance.kdTree_.computeAgentNeighbors(this, ref rangeSq); 77 | } 78 | } 79 | 80 | /** 81 | * Computes the new velocity of this agent. 82 | */ 83 | internal void computeNewVelocity() 84 | { 85 | orcaLines_.Clear(); 86 | 87 | float invTimeHorizonObst = 1.0f / timeHorizonObst_; 88 | 89 | /* Create obstacle ORCA lines. */ 90 | for (int i = 0; i < obstacleNeighbors_.Count; ++i) 91 | { 92 | 93 | Obstacle obstacle1 = obstacleNeighbors_[i].Value; 94 | Obstacle obstacle2 = obstacle1.next_; 95 | 96 | Vector2 relativePosition1 = obstacle1.point_ - position_; 97 | Vector2 relativePosition2 = obstacle2.point_ - position_; 98 | 99 | /* 100 | * Check if velocity obstacle of obstacle is already taken care 101 | * of by previously constructed obstacle ORCA lines. 102 | */ 103 | bool alreadyCovered = false; 104 | 105 | for (int j = 0; j < orcaLines_.Count; ++j) 106 | { 107 | if (RVOMath.det(invTimeHorizonObst * relativePosition1 - orcaLines_[j].point, orcaLines_[j].direction) - invTimeHorizonObst * radius_ >= -RVOMath.RVO_EPSILON && RVOMath.det(invTimeHorizonObst * relativePosition2 - orcaLines_[j].point, orcaLines_[j].direction) - invTimeHorizonObst * radius_ >= -RVOMath.RVO_EPSILON) 108 | { 109 | alreadyCovered = true; 110 | 111 | break; 112 | } 113 | } 114 | 115 | if (alreadyCovered) 116 | { 117 | continue; 118 | } 119 | 120 | /* Not yet covered. Check for collisions. */ 121 | float distSq1 = RVOMath.absSq(relativePosition1); 122 | float distSq2 = RVOMath.absSq(relativePosition2); 123 | 124 | float radiusSq = RVOMath.sqr(radius_); 125 | 126 | Vector2 obstacleVector = obstacle2.point_ - obstacle1.point_; 127 | float s = Vector2.Dot(-relativePosition1, obstacleVector) / RVOMath.absSq(obstacleVector); 128 | float distSqLine = RVOMath.absSq(-relativePosition1 - s * obstacleVector); 129 | 130 | Line line; 131 | 132 | if (s < 0.0f && distSq1 <= radiusSq) 133 | { 134 | /* Collision with left vertex. Ignore if non-convex. */ 135 | if (obstacle1.convex_) 136 | { 137 | line.point = new Vector2(0.0f, 0.0f); 138 | line.direction = RVOMath.normalize(new Vector2(-relativePosition1.y, relativePosition1.x)); 139 | orcaLines_.Add(line); 140 | } 141 | 142 | continue; 143 | } 144 | else if (s > 1.0f && distSq2 <= radiusSq) 145 | { 146 | /* 147 | * Collision with right vertex. Ignore if non-convex or if 148 | * it will be taken care of by neighboring obstacle. 149 | */ 150 | if (obstacle2.convex_ && RVOMath.det(relativePosition2, obstacle2.direction_) >= 0.0f) 151 | { 152 | line.point = new Vector2(0.0f, 0.0f); 153 | line.direction = RVOMath.normalize(new Vector2(-relativePosition2.y, relativePosition2.x)); 154 | orcaLines_.Add(line); 155 | } 156 | 157 | continue; 158 | } 159 | else if (s >= 0.0f && s <= 1.0f && distSqLine <= radiusSq) 160 | { 161 | /* Collision with obstacle segment. */ 162 | line.point = new Vector2(0.0f, 0.0f); 163 | line.direction = -obstacle1.direction_; 164 | orcaLines_.Add(line); 165 | 166 | continue; 167 | } 168 | 169 | /* 170 | * No collision. Compute legs. When obliquely viewed, both legs 171 | * can come from a single vertex. Legs extend cut-off line when 172 | * non-convex vertex. 173 | */ 174 | 175 | Vector2 leftLegDirection, rightLegDirection; 176 | 177 | if (s < 0.0f && distSqLine <= radiusSq) 178 | { 179 | /* 180 | * Obstacle viewed obliquely so that left vertex 181 | * defines velocity obstacle. 182 | */ 183 | if (!obstacle1.convex_) 184 | { 185 | /* Ignore obstacle. */ 186 | continue; 187 | } 188 | 189 | obstacle2 = obstacle1; 190 | 191 | float leg1 = RVOMath.sqrt(distSq1 - radiusSq); 192 | leftLegDirection = new Vector2(relativePosition1.x * leg1 - relativePosition1.y * radius_, relativePosition1.x * radius_ + relativePosition1.y * leg1) / distSq1; 193 | rightLegDirection = new Vector2(relativePosition1.x * leg1 + relativePosition1.y * radius_, -relativePosition1.x * radius_ + relativePosition1.y * leg1) / distSq1; 194 | } 195 | else if (s > 1.0f && distSqLine <= radiusSq) 196 | { 197 | /* 198 | * Obstacle viewed obliquely so that 199 | * right vertex defines velocity obstacle. 200 | */ 201 | if (!obstacle2.convex_) 202 | { 203 | /* Ignore obstacle. */ 204 | continue; 205 | } 206 | 207 | obstacle1 = obstacle2; 208 | 209 | float leg2 = RVOMath.sqrt(distSq2 - radiusSq); 210 | leftLegDirection = new Vector2(relativePosition2.x * leg2 - relativePosition2.y * radius_, relativePosition2.x * radius_ + relativePosition2.y * leg2) / distSq2; 211 | rightLegDirection = new Vector2(relativePosition2.x * leg2 + relativePosition2.y * radius_, -relativePosition2.x * radius_ + relativePosition2.y * leg2) / distSq2; 212 | } 213 | else 214 | { 215 | /* Usual situation. */ 216 | if (obstacle1.convex_) 217 | { 218 | float leg1 = RVOMath.sqrt(distSq1 - radiusSq); 219 | leftLegDirection = new Vector2(relativePosition1.x * leg1 - relativePosition1.y * radius_, relativePosition1.x * radius_ + relativePosition1.y * leg1) / distSq1; 220 | } 221 | else 222 | { 223 | /* Left vertex non-convex; left leg extends cut-off line. */ 224 | leftLegDirection = -obstacle1.direction_; 225 | } 226 | 227 | if (obstacle2.convex_) 228 | { 229 | float leg2 = RVOMath.sqrt(distSq2 - radiusSq); 230 | rightLegDirection = new Vector2(relativePosition2.x * leg2 + relativePosition2.y * radius_, -relativePosition2.x * radius_ + relativePosition2.y * leg2) / distSq2; 231 | } 232 | else 233 | { 234 | /* Right vertex non-convex; right leg extends cut-off line. */ 235 | rightLegDirection = obstacle1.direction_; 236 | } 237 | } 238 | 239 | /* 240 | * Legs can never point into neighboring edge when convex 241 | * vertex, take cutoff-line of neighboring edge instead. If 242 | * velocity projected on "foreign" leg, no constraint is added. 243 | */ 244 | 245 | Obstacle leftNeighbor = obstacle1.previous_; 246 | 247 | bool isLeftLegForeign = false; 248 | bool isRightLegForeign = false; 249 | 250 | if (obstacle1.convex_ && RVOMath.det(leftLegDirection, -leftNeighbor.direction_) >= 0.0f) 251 | { 252 | /* Left leg points into obstacle. */ 253 | leftLegDirection = -leftNeighbor.direction_; 254 | isLeftLegForeign = true; 255 | } 256 | 257 | if (obstacle2.convex_ && RVOMath.det(rightLegDirection, obstacle2.direction_) <= 0.0f) 258 | { 259 | /* Right leg points into obstacle. */ 260 | rightLegDirection = obstacle2.direction_; 261 | isRightLegForeign = true; 262 | } 263 | 264 | /* Compute cut-off centers. */ 265 | Vector2 leftCutOff = invTimeHorizonObst * (obstacle1.point_ - position_); 266 | Vector2 rightCutOff = invTimeHorizonObst * (obstacle2.point_ - position_); 267 | Vector2 cutOffVector = rightCutOff - leftCutOff; 268 | 269 | /* Project current velocity on velocity obstacle. */ 270 | 271 | /* Check if current velocity is projected on cutoff circles. */ 272 | float t = obstacle1 == obstacle2 ? 0.5f : Vector2.Dot((velocity_ - leftCutOff), cutOffVector) / RVOMath.absSq(cutOffVector); 273 | float tLeft = Vector2.Dot((velocity_ - leftCutOff), leftLegDirection); 274 | float tRight = Vector2.Dot((velocity_ - rightCutOff), rightLegDirection); 275 | 276 | if ((t < 0.0f && tLeft < 0.0f) || (obstacle1 == obstacle2 && tLeft < 0.0f && tRight < 0.0f)) 277 | { 278 | /* Project on left cut-off circle. */ 279 | Vector2 unitW = RVOMath.normalize(velocity_ - leftCutOff); 280 | 281 | line.direction = new Vector2(unitW.y, -unitW.x); 282 | line.point = leftCutOff + radius_ * invTimeHorizonObst * unitW; 283 | orcaLines_.Add(line); 284 | 285 | continue; 286 | } 287 | else if (t > 1.0f && tRight < 0.0f) 288 | { 289 | /* Project on right cut-off circle. */ 290 | Vector2 unitW = RVOMath.normalize(velocity_ - rightCutOff); 291 | 292 | line.direction = new Vector2(unitW.y, -unitW.x); 293 | line.point = rightCutOff + radius_ * invTimeHorizonObst * unitW; 294 | orcaLines_.Add(line); 295 | 296 | continue; 297 | } 298 | 299 | /* 300 | * Project on left leg, right leg, or cut-off line, whichever is 301 | * closest to velocity. 302 | */ 303 | float distSqCutoff = (t < 0.0f || t > 1.0f || obstacle1 == obstacle2) ? float.PositiveInfinity : RVOMath.absSq(velocity_ - (leftCutOff + t * cutOffVector)); 304 | float distSqLeft = tLeft < 0.0f ? float.PositiveInfinity : RVOMath.absSq(velocity_ - (leftCutOff + tLeft * leftLegDirection)); 305 | float distSqRight = tRight < 0.0f ? float.PositiveInfinity : RVOMath.absSq(velocity_ - (rightCutOff + tRight * rightLegDirection)); 306 | 307 | if (distSqCutoff <= distSqLeft && distSqCutoff <= distSqRight) 308 | { 309 | /* Project on cut-off line. */ 310 | line.direction = -obstacle1.direction_; 311 | line.point = leftCutOff + radius_ * invTimeHorizonObst * new Vector2(-line.direction.y, line.direction.x); 312 | orcaLines_.Add(line); 313 | 314 | continue; 315 | } 316 | 317 | if (distSqLeft <= distSqRight) 318 | { 319 | /* Project on left leg. */ 320 | if (isLeftLegForeign) 321 | { 322 | continue; 323 | } 324 | 325 | line.direction = leftLegDirection; 326 | line.point = leftCutOff + radius_ * invTimeHorizonObst * new Vector2(-line.direction.y, line.direction.x); 327 | orcaLines_.Add(line); 328 | 329 | continue; 330 | } 331 | 332 | /* Project on right leg. */ 333 | if (isRightLegForeign) 334 | { 335 | continue; 336 | } 337 | 338 | line.direction = -rightLegDirection; 339 | line.point = rightCutOff + radius_ * invTimeHorizonObst * new Vector2(-line.direction.y, line.direction.x); 340 | orcaLines_.Add(line); 341 | } 342 | 343 | int numObstLines = orcaLines_.Count; 344 | 345 | float invTimeHorizon = 1.0f / timeHorizon_; 346 | 347 | /* Create agent ORCA lines. */ 348 | for (int i = 0; i < agentNeighbors_.Count; ++i) 349 | { 350 | Agent other = agentNeighbors_[i].Value; 351 | 352 | Vector2 relativePosition = other.position_ - position_; 353 | Vector2 relativeVelocity = velocity_ - other.velocity_; 354 | float distSq = RVOMath.absSq(relativePosition); 355 | float combinedRadius = radius_ + other.radius_; 356 | float combinedRadiusSq = RVOMath.sqr(combinedRadius); 357 | 358 | Line line; 359 | Vector2 u; 360 | 361 | if (distSq > combinedRadiusSq) 362 | { 363 | /* No collision. */ 364 | Vector2 w = relativeVelocity - invTimeHorizon * relativePosition; 365 | 366 | /* Vector from cutoff center to relative velocity. */ 367 | float wLengthSq = RVOMath.absSq(w); 368 | float dotProduct1 = Vector2.Dot(w, relativePosition); 369 | 370 | if (dotProduct1 < 0.0f && RVOMath.sqr(dotProduct1) > combinedRadiusSq * wLengthSq) 371 | { 372 | /* Project on cut-off circle. */ 373 | float wLength = RVOMath.sqrt(wLengthSq); 374 | Vector2 unitW = w / wLength; 375 | 376 | line.direction = new Vector2(unitW.y, -unitW.x); 377 | u = (combinedRadius * invTimeHorizon - wLength) * unitW; 378 | } 379 | else 380 | { 381 | /* Project on legs. */ 382 | float leg = RVOMath.sqrt(distSq - combinedRadiusSq); 383 | 384 | if (RVOMath.det(relativePosition, w) > 0.0f) 385 | { 386 | /* Project on left leg. */ 387 | line.direction = new Vector2(relativePosition.x * leg - relativePosition.y * combinedRadius, relativePosition.x * combinedRadius + relativePosition.y * leg) / distSq; 388 | } 389 | else 390 | { 391 | /* Project on right leg. */ 392 | line.direction = -new Vector2(relativePosition.x * leg + relativePosition.y * combinedRadius, -relativePosition.x * combinedRadius + relativePosition.y * leg) / distSq; 393 | } 394 | 395 | float dotProduct2 = Vector2.Dot(relativeVelocity, line.direction); 396 | u = dotProduct2 * line.direction - relativeVelocity; 397 | } 398 | } 399 | else 400 | { 401 | /* Collision. Project on cut-off circle of time timeStep. */ 402 | float invTimeStep = 1.0f / Simulator.Instance.timeStep_; 403 | 404 | /* Vector from cutoff center to relative velocity. */ 405 | Vector2 w = relativeVelocity - invTimeStep * relativePosition; 406 | 407 | float wLength = RVOMath.abs(w); 408 | Vector2 unitW = w / wLength; 409 | 410 | line.direction = new Vector2(unitW.y, -unitW.x); 411 | u = (combinedRadius * invTimeStep - wLength) * unitW; 412 | } 413 | 414 | line.point = velocity_ + getAgentWeight(other.weight_) * u; 415 | orcaLines_.Add(line); 416 | } 417 | 418 | int lineFail = linearProgram2(orcaLines_, maxSpeed_, prefVelocity_, false, ref newVelocity_); 419 | 420 | if (lineFail < orcaLines_.Count) 421 | { 422 | linearProgram3(orcaLines_, numObstLines, lineFail, maxSpeed_, ref newVelocity_); 423 | } 424 | } 425 | internal float getAgentWeight(float otherWeight) 426 | { 427 | return weight_ / (weight_ + otherWeight); 428 | } 429 | /** 430 | * Inserts an agent neighbor into the set of neighbors of this 431 | * agent. 432 | * 433 | * A pointer to the agent to be inserted. 434 | * The squared range around this agent. 435 | */ 436 | internal void insertAgentNeighbor(Agent agent, ref float rangeSq) 437 | { 438 | if (this != agent) 439 | { 440 | float distSq = RVOMath.absSq(position_ - agent.position_); 441 | 442 | if (distSq < rangeSq) 443 | { 444 | if (agentNeighbors_.Count < maxNeighbors_) 445 | { 446 | agentNeighbors_.Add(KeyValuePair.Create(distSq, agent)); 447 | } 448 | 449 | int i = agentNeighbors_.Count - 1; 450 | 451 | while (i != 0 && distSq < agentNeighbors_[i - 1].Key) 452 | { 453 | agentNeighbors_[i] = agentNeighbors_[i - 1]; 454 | --i; 455 | } 456 | 457 | agentNeighbors_[i] = KeyValuePair.Create(distSq, agent); 458 | 459 | if (agentNeighbors_.Count == maxNeighbors_) 460 | { 461 | rangeSq = agentNeighbors_[agentNeighbors_.Count - 1].Key; 462 | } 463 | } 464 | } 465 | } 466 | 467 | /** 468 | * Inserts a static obstacle neighbor into the set of neighbors 469 | * of this agent. 470 | * 471 | * The number of the static obstacle to be 472 | * inserted. 473 | * The squared range around this agent. 474 | */ 475 | internal void insertObstacleNeighbor(Obstacle obstacle, float rangeSq) 476 | { 477 | Obstacle nextObstacle = obstacle.next_; 478 | 479 | float distSq = RVOMath.distSqPointLineSegment(obstacle.point_, nextObstacle.point_, position_); 480 | 481 | if (distSq < rangeSq) 482 | { 483 | obstacleNeighbors_.Add(KeyValuePair.Create(distSq, obstacle)); 484 | 485 | int i = obstacleNeighbors_.Count - 1; 486 | 487 | while (i != 0 && distSq < obstacleNeighbors_[i - 1].Key) 488 | { 489 | obstacleNeighbors_[i] = obstacleNeighbors_[i - 1]; 490 | --i; 491 | } 492 | obstacleNeighbors_[i] = KeyValuePair.Create(distSq, obstacle); 493 | } 494 | } 495 | 496 | /** 497 | * Updates the two-dimensional position and two-dimensional 498 | * velocity of this agent. 499 | */ 500 | internal void update() 501 | { 502 | velocity_ = newVelocity_; 503 | position_ += velocity_ * Simulator.Instance.timeStep_; 504 | } 505 | 506 | /** 507 | * Solves a one-dimensional linear program on a specified line 508 | * subject to linear constraints defined by lines and a circular 509 | * constraint. 510 | * 511 | * True if successful. 512 | * 513 | * Lines defining the linear constraints. 514 | * The specified line constraint. 515 | * The radius of the circular constraint. 516 | * The optimization velocity. 517 | * True if the direction should be optimized. 518 | * 519 | * A reference to the result of the linear program. 520 | * 521 | */ 522 | private bool linearProgram1(IList lines, int lineNo, float radius, Vector2 optVelocity, bool directionOpt, ref Vector2 result) 523 | { 524 | float dotProduct = Vector2.Dot(lines[lineNo].point, lines[lineNo].direction); 525 | float discriminant = RVOMath.sqr(dotProduct) + RVOMath.sqr(radius) - RVOMath.absSq(lines[lineNo].point); 526 | 527 | if (discriminant < 0.0f) 528 | { 529 | /* Max speed circle fully invalidates line lineNo. */ 530 | return false; 531 | } 532 | 533 | float sqrtDiscriminant = RVOMath.sqrt(discriminant); 534 | float tLeft = -dotProduct - sqrtDiscriminant; 535 | float tRight = -dotProduct + sqrtDiscriminant; 536 | 537 | for (int i = 0; i < lineNo; ++i) 538 | { 539 | float denominator = RVOMath.det(lines[lineNo].direction, lines[i].direction); 540 | float numerator = RVOMath.det(lines[i].direction, lines[lineNo].point - lines[i].point); 541 | 542 | if (RVOMath.fabs(denominator) <= RVOMath.RVO_EPSILON) 543 | { 544 | /* Lines lineNo and i are (almost) parallel. */ 545 | if (numerator < 0.0f) 546 | { 547 | return false; 548 | } 549 | 550 | continue; 551 | } 552 | 553 | float t = numerator / denominator; 554 | 555 | if (denominator >= 0.0f) 556 | { 557 | /* Line i bounds line lineNo on the right. */ 558 | tRight = Math.Min(tRight, t); 559 | } 560 | else 561 | { 562 | /* Line i bounds line lineNo on the left. */ 563 | tLeft = Math.Max(tLeft, t); 564 | } 565 | 566 | if (tLeft > tRight) 567 | { 568 | return false; 569 | } 570 | } 571 | 572 | if (directionOpt) 573 | { 574 | /* Optimize direction. */ 575 | if (Vector2.Dot(optVelocity, lines[lineNo].direction) > 0.0f) 576 | { 577 | /* Take right extreme. */ 578 | result = lines[lineNo].point + tRight * lines[lineNo].direction; 579 | } 580 | else 581 | { 582 | /* Take left extreme. */ 583 | result = lines[lineNo].point + tLeft * lines[lineNo].direction; 584 | } 585 | } 586 | else 587 | { 588 | /* Optimize closest point. */ 589 | float t = Vector2.Dot(lines[lineNo].direction, (optVelocity - lines[lineNo].point)); 590 | 591 | if (t < tLeft) 592 | { 593 | result = lines[lineNo].point + tLeft * lines[lineNo].direction; 594 | } 595 | else if (t > tRight) 596 | { 597 | result = lines[lineNo].point + tRight * lines[lineNo].direction; 598 | } 599 | else 600 | { 601 | result = lines[lineNo].point + t * lines[lineNo].direction; 602 | } 603 | } 604 | 605 | return true; 606 | } 607 | 608 | /** 609 | * Solves a two-dimensional linear program subject to linear 610 | * constraints defined by lines and a circular constraint. 611 | * 612 | * The number of the line it fails on, and the number of lines 613 | * if successful. 614 | * 615 | * Lines defining the linear constraints. 616 | * The radius of the circular constraint. 617 | * The optimization velocity. 618 | * True if the direction should be optimized. 619 | * 620 | * A reference to the result of the linear program. 621 | * 622 | */ 623 | private int linearProgram2(IList lines, float radius, Vector2 optVelocity, bool directionOpt, ref Vector2 result) 624 | { 625 | if (directionOpt) 626 | { 627 | /* 628 | * Optimize direction. Note that the optimization velocity is of 629 | * unit length in this case. 630 | */ 631 | result = optVelocity * radius; 632 | } 633 | else if (RVOMath.absSq(optVelocity) > RVOMath.sqr(radius)) 634 | { 635 | /* Optimize closest point and outside circle. */ 636 | result = RVOMath.normalize(optVelocity) * radius; 637 | } 638 | else 639 | { 640 | /* Optimize closest point and inside circle. */ 641 | result = optVelocity; 642 | } 643 | 644 | for (int i = 0; i < lines.Count; ++i) 645 | { 646 | if (RVOMath.det(lines[i].direction, lines[i].point - result) > 0.0f) 647 | { 648 | /* Result does not satisfy constraint i. Compute new optimal result. */ 649 | Vector2 tempResult = result; 650 | if (!linearProgram1(lines, i, radius, optVelocity, directionOpt, ref result)) 651 | { 652 | result = tempResult; 653 | 654 | return i; 655 | } 656 | } 657 | } 658 | 659 | return lines.Count; 660 | } 661 | 662 | /** 663 | * Solves a two-dimensional linear program subject to linear 664 | * constraints defined by lines and a circular constraint. 665 | * 666 | * Lines defining the linear constraints. 667 | * Count of obstacle lines. 668 | * The line on which the 2-d linear program 669 | * failed. 670 | * The radius of the circular constraint. 671 | * A reference to the result of the linear program. 672 | * 673 | */ 674 | private void linearProgram3(IList lines, int numObstLines, int beginLine, float radius, ref Vector2 result) 675 | { 676 | float distance = 0.0f; 677 | 678 | for (int i = beginLine; i < lines.Count; ++i) 679 | { 680 | if (RVOMath.det(lines[i].direction, lines[i].point - result) > distance) 681 | { 682 | /* Result does not satisfy constraint of line i. */ 683 | IList projLines = new List(); 684 | for (int ii = 0; ii < numObstLines; ++ii) 685 | { 686 | projLines.Add(lines[ii]); 687 | } 688 | 689 | for (int j = numObstLines; j < i; ++j) 690 | { 691 | Line line; 692 | 693 | float determinant = RVOMath.det(lines[i].direction, lines[j].direction); 694 | 695 | if (RVOMath.fabs(determinant) <= RVOMath.RVO_EPSILON) 696 | { 697 | /* Line i and line j are parallel. */ 698 | if (Vector2.Dot(lines[i].direction, lines[j].direction) > 0.0f) 699 | { 700 | /* Line i and line j point in the same direction. */ 701 | continue; 702 | } 703 | else 704 | { 705 | /* Line i and line j point in opposite direction. */ 706 | line.point = weight_ * (lines[i].point + lines[j].point); 707 | } 708 | } 709 | else 710 | { 711 | line.point = lines[i].point + (RVOMath.det(lines[j].direction, lines[i].point - lines[j].point) / determinant) * lines[i].direction; 712 | } 713 | 714 | line.direction = RVOMath.normalize(lines[j].direction - lines[i].direction); 715 | projLines.Add(line); 716 | } 717 | 718 | Vector2 tempResult = result; 719 | if (linearProgram2(projLines, radius, new Vector2(-lines[i].direction.y, lines[i].direction.x), true, ref result) < projLines.Count) 720 | { 721 | /* 722 | * This should in principle not happen. The result is by 723 | * definition already in the feasible region of this 724 | * linear program. If it fails, it is due to small 725 | * floating point error, and the current result is kept. 726 | */ 727 | result = tempResult; 728 | } 729 | 730 | distance = RVOMath.det(lines[i].direction, lines[i].point - result); 731 | } 732 | } 733 | } 734 | } 735 | } 736 | --------------------------------------------------------------------------------