新增SensorToolkit

Assets/SensorToolkit/Extras/Micosmo.SensorToolkit.asmref

@@ -0,0 +1,3 @@
+{
+    "reference": "GUID:b9d61b92870877a459c95c25c7d15074"
+}

Assets/SensorToolkit/Extras/Micosmo.SensorToolkit.asmref.meta

@@ -0,0 +1,7 @@
+fileFormatVersion: 2
+guid: 9393cfcd8e280044a8a5af77300e572c
+AssemblyDefinitionReferenceImporter:
+  externalObjects: {}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Assets/SensorToolkit/Extras/NavMeshPathfinder.cs

@@ -0,0 +1,222 @@
+using System.Collections;
+using System.Collections.Generic;
+using UnityEngine;
+using UnityEngine.AI;
+
+namespace Micosmo.SensorToolkit.Extras {
+
+    public class NavMeshPathfinder : MonoBehaviour {
+
+        enum PathfinderMode { None, SeekTransform, SeekPosition };
+
+        [Header("Configuration")]
+        public float AgentRadius = .5f;
+        public float AgentHeight = 2f;
+        public float RecalculateTime = 5f;
+
+        [Header("Destination")]
+        [SerializeField]
+        ObservableTransform target = new ObservableTransform();
+        public ObservableTransform Target => target;
+
+        [Header("Runtime State (Dont touch)")]
+        public bool IsPathReady;
+        public float RemainingDistance;
+        public bool IsDestinationReached;
+
+        public Vector3 TargetPosition {
+            get {
+                if (mode == PathfinderMode.SeekTransform) {
+                    return Target.Value != null ? Target.Value.position : transform.position;
+                } else if (mode == PathfinderMode.SeekPosition) {
+                    return targetPosition;
+                }
+                return transform.position;
+            }
+        }
+
+        Vector3 targetPosition;
+        PathfinderMode mode = PathfinderMode.None;
+
+        public Vector3 NextCorner {
+            get {
+                if (!IsPathReady) {
+                    return Vector3.zero;
+                }
+                storeCorners();
+                var nextCorner = corners[1];
+                if (pathLength > 2 && (nextCorner-transform.position).magnitude < AgentRadius) {
+                    nextCorner = corners[2];
+                }
+                return nextCorner;
+            }
+        }
+
+        NavMeshAgent agent;
+        Vector3[] corners = new Vector3[100];
+        Coroutine calculatePathRoutineInstance;
+        int pathLength = 0;
+
+        public void SetTargetTransform(Transform target) {
+            Target.Value = target;
+        }
+
+        public void SetTargetPosition(Vector3 p) {
+            Target.Value = null;
+            if (mode != PathfinderMode.SeekPosition && targetPosition != p) {
+                targetPosition = p;
+                mode = PathfinderMode.SeekPosition;
+                RestartPathfinderRoutine();
+            }
+        }
+
+        public void StopAndClear() {
+            Target.Value = null;
+            mode = PathfinderMode.None;
+            StopPathfinderRoutine();
+        }
+
+        void Awake() {
+            GameObject agentGO = new GameObject("AINavigation Agent");
+            agentGO.transform.SetParent(transform, false);
+            agent = agentGO.AddComponent<NavMeshAgent>();
+            agent.agentTypeID = 0;
+            agent.baseOffset = 0;
+            agent.speed = 0;
+            agent.angularSpeed = 0;
+            agent.acceleration = 0;
+            agent.stoppingDistance = 0;
+            agent.autoBraking = false;
+            agent.radius = AgentRadius;
+            agent.height = AgentHeight;
+            agent.obstacleAvoidanceType = ObstacleAvoidanceType.HighQualityObstacleAvoidance;
+            agent.avoidancePriority = 50;
+
+            agent.updatePosition = false;
+            agent.updateRotation = false;
+            agent.updateUpAxis = false;
+        }
+
+        void OnDestroy() {
+            Destroy(agent.gameObject);
+        }
+
+        void OnEnable() {
+            RestartPathfinderRoutine();
+            Target.OnChanged += TargetChangeHandler;
+            TargetChangeHandler();
+        }
+
+        void OnDisable() {
+            Target.OnChanged -= TargetChangeHandler;
+        }
+
+        void Update() {
+            agent.nextPosition = transform.position;
+
+            WarpOnStuck();
+
+            RemainingDistance = IsPathReady ? agent.remainingDistance : 0f;
+            IsDestinationReached = IsPathReady && (TargetPosition - transform.position).magnitude < AgentRadius;
+        }
+
+        void StopPathfinderRoutine() {
+            IsPathReady = false;
+            RemainingDistance = 0f;
+            IsDestinationReached = false;
+            if (calculatePathRoutineInstance != null) {
+                StopCoroutine(calculatePathRoutineInstance);
+            }
+        }
+
+        void RestartPathfinderRoutine() {
+            StopPathfinderRoutine();
+            calculatePathRoutineInstance = StartCoroutine(calculatePathRoutine());
+        }
+
+        float stuckTimer = 0f;
+        void WarpOnStuck() {
+            var delta = agent.nextPosition - transform.position;
+            var xzDist = new Vector2(delta.x, delta.z).magnitude;
+            var yDist = Mathf.Abs(delta.y);
+            if (xzDist > AgentRadius || yDist > AgentHeight) {
+                stuckTimer += Time.deltaTime;
+            } else {
+                stuckTimer = 0f;
+            }
+
+            if (stuckTimer >= 1f) {
+                var isSuccess = agent.Warp(transform.position);
+                Debug.LogWarning($"The NavMeshAgent's position is out-of-sync, attempted to warp to gameobject. Success: {isSuccess}", gameObject);
+                stuckTimer = 0f;
+                RestartPathfinderRoutine();
+            }
+        }
+
+        IEnumerator calculatePathRoutine() {
+            IsPathReady = false;
+            agent.Warp(transform.position);
+
+            while (true) {
+                if (IsDestinationReached) {
+                    yield return null;
+                    continue;
+                }
+
+                agent.SetDestination(TargetPosition);
+
+                while (agent.pathPending) {
+                    IsPathReady = false;
+                    yield return null;
+                }
+
+                storeCorners();
+                IsPathReady = true;
+
+                yield return new WaitForSeconds(RecalculateTime);
+            }
+        }
+
+        void storeCorners() {
+            while (true) {
+                pathLength = agent.path.GetCornersNonAlloc(corners);
+                if (pathLength < corners.Length) {
+                    break;
+                }
+
+                // Gotta try again
+                corners = new Vector3[corners.Length * 2];
+            }
+        }
+
+        void TargetChangeHandler() {
+            if (Target.Value != null) {
+                mode = PathfinderMode.SeekTransform;
+                RestartPathfinderRoutine();
+            }
+        }
+
+        void OnDrawGizmosSelected() {
+            if (!IsPathReady) {
+                return;
+            }
+
+            for (int i = 0; i < pathLength; i++) {
+                if (i == 0) {
+                    continue;
+                }
+
+                var corner = corners[i];
+                var prevCorner = corners[i - 1];
+
+                SensorGizmos.PushColor(i == 1 ? Color.green : STPrefs.defaultCyan);
+
+                Gizmos.DrawLine(prevCorner, corner);
+                Gizmos.DrawCube(corner, Vector3.one * 0.2f);
+
+                SensorGizmos.PopColor();
+            }
+        }
+    }
+
+}

Assets/SensorToolkit/Extras/NavMeshPathfinder.cs.meta

@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: 1af24ac976034792806badec35f0d33f
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Assets/SensorToolkit/Extras/StupidSimpleFSM.cs

@@ -0,0 +1,64 @@
+using System.Text.RegularExpressions;
+using System.Collections;
+using System.Collections.Generic;
+using UnityEngine;
+#if UNITY_EDITOR
+using UnityEditor;
+#endif
+
+namespace Micosmo.SensorToolkit.Extras {
+    
+    public abstract class StupidSimpleFSM : MonoBehaviour {
+        public string CurrentStateName => GetNiceStateName(currentState);
+        public string PreviousStateName => GetNiceStateName(prevState);
+
+        static Regex rxNiceName = new Regex(@"^<(.*)>.*$", RegexOptions.Compiled);
+        string GetNiceStateName(IEnumerator state) {
+            if (state == null) {
+                return "None";
+            }
+            return rxNiceName.Match(state.GetType().Name).Groups[1].Value;
+        }
+
+        IEnumerator currentState;
+        IEnumerator prevState;
+
+        protected virtual void OnDisable() {
+            currentState = null;
+            prevState = null;
+        }
+
+        public Coroutine SetFSMState(IEnumerator nextState) {
+            if (currentState?.GetType() == nextState?.GetType()) {
+                return null;
+            }
+            if (currentState != null) {
+                StopCoroutine(currentState);
+            }
+            prevState = currentState;
+            currentState = nextState;
+            return StartCoroutine(currentState);
+        }
+    }
+}
+
+#if UNITY_EDITOR
+namespace Micosmo.SensorToolkit.Extras.Editors {
+    [CustomEditor(typeof(StupidSimpleFSM), true)]
+    [CanEditMultipleObjects]
+    public class StupidSimpleFSMEditor : Editor {
+        public override void OnInspectorGUI() {
+            DrawDefaultInspector();
+
+            var fsm = target as StupidSimpleFSM;
+
+            if (Application.isPlaying && fsm != null) {
+                EditorGUILayout.Separator();
+                EditorGUILayout.LabelField("FSM Runtime", EditorStyles.boldLabel);
+                EditorGUILayout.TextField("Current State", fsm.CurrentStateName);
+                EditorGUILayout.TextField("Previous State", fsm.PreviousStateName);
+            }
+        }
+    }
+}
+#endif

Assets/SensorToolkit/Extras/StupidSimpleFSM.cs.meta

@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: dff83748b31e45398e1edfc87df93cf2
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: