ParkingRobot/CommonUsage-MultiVehicleSync/commonusage/Geometries/CircularArc.cs

using System;
using System.Collections.Generic;
using System.Drawing;
using System.Numerics;
using System.Reflection;
using System.Security.Cryptography;
using System.Text;
using CommonUsage.Mathematics;

namespace CommonUsage.Geometries
{
	public class CircularArc : AbstractGeometry
	{
		/// <summary>
		/// 以center为圆心、radius为半径，从angleStart逆时针转到angleEnd所构成的圆弧。direction表示圆弧走向。
		/// </summary>
		/// <param name="center"></param>
		/// <param name="radius"></param>
		/// <param name="angleStart"></param>
		/// <param name="angleEnd"></param>
		/// <param name="direction">表示圆弧走向，1为angleStart到angleEnd，-1为angleEnd到angleStart</param>
		public CircularArc(Vector2 center, float radius, float angleStart, float angleEnd, int direction, Padding paddingType)
		{
			_center = center;
			_radius = radius;
			_angleStart = angleStart;
			_angleEnd = angleEnd;
			_direction = direction;
			PaddingType = paddingType;

			ChangeShape();
			CalculateVisPoints();
		}

		public override void Visualize(Action<VisDot> processDot, Action<VisLine> processLine,
			bool visExtendedPart = false)
		{
			lock (_visPoints)
			{
				if (visExtendedPart)
				{

				}

				for (var i = 0; i < _visPoints.Length - 1; ++i)
				{
					if ((i == 0 || i == _visPoints.Length - 2) && !visExtendedPart) continue;
					var color = Color.Red;
					if (i == 0 || i == _visPoints.Length - 2) color = Color.Gray;
					processLine(new VisLine(_visPoints[i], _visPoints[i + 1], 
						false, i == (_visPoints.Length - 1) / 2, color));
				}

				if (visExtendedPart)
				{

				}
			}
		}

		public void SwitchSide()
		{
			(_angleStart, _angleEnd) = (_angleEnd, _angleStart);
			ChangeShape();
			CalculateVisPoints();
		}

		public float VisAngleResolution = 1;

		public Vector2 Center
		{
			get => _center;
			set
			{
				_center = value;
				ChangeShape();
				CalculateVisPoints();
			}
		}

		public float Radius
		{
			get => _radius;
			set
			{
				_radius = value;
				ChangeShape();
				CalculateVisPoints();
			}
		}

		public float AngleStart
		{
			get => _angleStart;
			set
			{
				_angleStart = value;
				ChangeShape();
				CalculateVisPoints();
			}
		}

		public float AngleEnd
		{
			get => _angleEnd;
			set
			{
				_angleEnd = value;
				ChangeShape();
				CalculateVisPoints();
			}
		}

		public int Direction
		{
			get => _direction;
			set
			{
				_direction = value;
				ChangeShape();
				CalculateVisPoints();
			}
		}

        public float AngleRange => _totalTh;

		public Vector2 PointStart => _center + new Vector2(_radius * (float)Math.Cos(_angleStart / 180 * Math.PI),
			_radius * (float)Math.Sin(_angleStart / 180 * Math.PI));

		public Vector2 PointEnd => _center + new Vector2(_radius * (float)Math.Cos(_angleEnd / 180 * Math.PI),
			_radius * (float)Math.Sin(_angleEnd / 180 * Math.PI));

		public Vector2 Src => _src;

		public Vector2 Dst => _dst;

		public float TangentSrc => _tangentSrc;

		public float TangentDst => _tangentDst;

		public override (Vector2 Pt, float Angle, float Bias, float Position) QueryTangentPoint(Vector2 point)
		{
			var queryTh = (float)(Math.Atan2(point.Y - _center.Y, point.X - _center.X) / Math.PI * 180);

			var p = new Vector2();
			var tangent = 0f;
			var pd = 0f;
			var bestBias = float.MaxValue;

			if ((((int)PaddingType >> 4) & 0x1) == 1)
			{
				var (bias1, hPnt1, fd1) = CommonMath.Project2DLine(point, _beforeStartSrc, _src);
                if (fd1 <= 1000)
				{
					p = hPnt1;
					tangent = (_direction >= 0 ? _angleStart : _angleEnd) + 90 * _direction;
					pd = fd1;
					bestBias = bias1;
				}
			}

			if (((int)PaddingType & 0x1) == 1)
			{
				var (bias2, hPnt2, fd2) = CommonMath.Project2DLine(point, _dst, _afterEndDst);
				if (fd2 >= 0 && Math.Abs(bias2) < Math.Abs(bestBias))
				{
					p = hPnt2;
					tangent = (_direction >= 0 ? _angleEnd : _angleStart) + 90 * _direction;
					pd = _totalLen + fd2;
					bestBias = bias2;
				}
			}

			var th1 = _direction == 1 ? CommonMath.ThDiff(queryTh, _angleStart) : CommonMath.ThDiff(_angleEnd, queryTh);
			// todo: urgent bug! should use better strategy to prevent sign problem
            if (th1 < -55) th1 += 360;
			var arcBias = (_radius - Vector2.Distance(point, _center)) * _direction;
			if (Math.Abs(arcBias) < Math.Abs(bestBias))
			{
				p = _center + _radius * new Vector2((float)Math.Cos(queryTh / 180 * Math.PI),
					(float)Math.Sin(queryTh / 180 * Math.PI));
				tangent = queryTh + 90 * _direction;
				pd = _radius * th1 / 180 * (float)Math.PI;
				bestBias = arcBias;
			}

			return (p, tangent, bestBias, pd);
		}

		public override float QueryCurvature(float position)
		{
			// var theta = (float)(_angleEnd - position / _radius / Math.PI * 180f + Math.PI);
			// return Vectoriel.FromAngleLen(theta, 1f / _radius);
            return 1000f / _radius * _direction;
        }

		public override float Length()
		{
			return _totalLen;
		}

		private void CalculateVisPoints()
		{
			lock (_visPoints)
			{
				// todo: overlapping start and end is problematic
				var ptCnt = (int)Math.Ceiling((_angleEnd + 360 - _angleStart) % 360 / VisAngleResolution);
				_visPoints = new Vector2[ptCnt + 2];

				var starting = _angleStart;
				if (_direction == -1) starting = _angleEnd;
				_visPoints[0] = _beforeStartSrc;

				for (var j = 0; j < ptCnt; ++j)
				{
					var th = starting + j * VisAngleResolution * _direction;
					var radAngle = (float)(th / 180f * Math.PI);
					_visPoints[j + 1] = Center + new Vector2((float)Math.Cos(radAngle), (float)Math.Sin(radAngle)) * Radius;
				}
				
				_visPoints[ptCnt + 1] = _afterEndDst;
			}
		}

		private void ChangeShape()
		{
			_totalTh = CommonMath.ThDiff(_angleEnd, _angleStart);
			if (_totalTh < 0) _totalTh += 360;
			_totalLen = _radius * _totalTh / 180 * (float)Math.PI;

			var radAngleStart = _angleStart / 180 * Math.PI;
			var radAngleEnd = _angleEnd / 180 * Math.PI;
			double srcAngle = radAngleStart, dstAngle = radAngleEnd;
			if (_direction == -1) (srcAngle, dstAngle) = (dstAngle, srcAngle);
			_src = _center + new Vector2((float)Math.Cos(srcAngle), (float)Math.Sin(srcAngle)) * _radius;
			_dst = _center + new Vector2((float)Math.Cos(dstAngle), (float)Math.Sin(dstAngle)) * _radius;
			_beforeStartSrc = CommonMath.Transform2D(_src,
				(_direction >= 0 ? _angleStart : _angleEnd) + 90 * _direction, new Vector2(-1000, 0));
			_afterEndDst = CommonMath.Transform2D(_dst, (_direction >= 0 ? _angleEnd : _angleStart) + 90 * _direction,
				new Vector2(1000, 0));
			_tangentSrc = QueryTangentPoint(_src).Angle;
			_tangentDst = QueryTangentPoint(_dst).Angle;
		}

		private Vector2 _center;
		private float _radius, _angleStart, _angleEnd;
		private int _direction;

		private float _totalTh, _totalLen;
		private Vector2 _src, _dst;
		private float _tangentSrc, _tangentDst;
		private Vector2 _beforeStartSrc, _afterEndDst;

		private Vector2[] _visPoints = Array.Empty<Vector2>();
	}
}