Initial commit from MyParking project

2026-08-04 10:29:21 +08:00
commit d9432bd529
138 changed files with 16957 additions and 0 deletions
@@ -0,0 +1,100 @@
+using Newtonsoft.Json;
+using System;
+using System.Collections.Generic;
+using System.Numerics;
+using System.Text;
+using CommonUsage.Mathematics;
+
+namespace CommonUsage.Chassis
+{
+	public class SteerWheel : Wheel
+	{
+		public SteerWheel(Vector2 position, float angleLowerLimit, float angleUpperLimit, Action<float> speedWriter,
+			Func<float> speedReader, Action<float> angleWriter, Func<float> angleReader, float angleLimitMarginDeg = 15f) : base(position, speedWriter,
+			speedReader)
+		{
+			_angleLowerLimit = angleLowerLimit;
+			_angleUpperLimit = angleUpperLimit;
+			_angleWriter = angleWriter;
+			_angleReader = angleReader;
+			_centerDistance = position.Length();
+            AngleLimitMarginDeg = angleLimitMarginDeg;
+		}
+
+        public float AngleLimitMarginDeg = 15f;
+
+		public bool TrySetDirection(bool allowReverse, ref float desireDirection, ref int dir)
+		{
+            if (TryNormalizeAngleInLimit(desireDirection, out var normalized))
+			{
+				dir = 1;
+                desireDirection = normalized;
+				return true;
+			}
+
+			if (!allowReverse) return false;
+
+			var oppositeTh = (float)CommonMath.RoundTh(desireDirection + 180);
+			if (TryNormalizeAngleInLimit(oppositeTh, out normalized))
+			{
+				dir = -1;
+				desireDirection = normalized;
+				return true;
+			}
+
+			dir = 0;
+			return false;
+		}
+
+        private bool TryNormalizeAngleInLimit(float angle, out float normalized)
+        {
+            var lower = CommonMath.RoundTh(_angleLowerLimit);
+            var upper = CommonMath.RoundTh(_angleUpperLimit);
+            while (upper < lower) upper += 360;
+
+            normalized = (float)CommonMath.RoundTh(angle);
+            while (normalized < lower) normalized += 360;
+            while (normalized > upper && normalized - 360 >= lower) normalized -= 360;
+
+            var margin = Math.Min(normalized - lower, upper - normalized);
+            return normalized >= lower && normalized <= upper && margin >= Math.Max(0, AngleLimitMarginDeg);
+        }
+
+		public float ReadAngle()
+		{
+			return _angleReader();
+		}
+
+		public void WriteAngle(float angle)
+        {
+            _angleWriter.Invoke(_sendAngle = Math.Max(_angleLowerLimit, Math.Min(angle, _angleUpperLimit)));
+        }
+
+        public float GetSendAngle()
+        {
+            return _sendAngle;
+        }
+
+        public float GetAngleRelativeToChassis()
+        {
+            return ZeroDirection + _sendAngle;
+        }
+
+		public float CenterDistance()
+		{
+			return _centerDistance;
+		}
+
+        public float AngleLowerLimit => _angleLowerLimit;
+
+		public float AngleUpperLimit => _angleUpperLimit;
+
+		[JsonIgnore] private readonly Action<float> _angleWriter;
+		[JsonIgnore] private readonly Func<float> _angleReader;
+
+		private float _angleLowerLimit = -90, _angleUpperLimit = 90;
+		private float _centerDistance;
+
+        public float _sendAngle = 0f;
+    }
+}