Initial commit from MyParking project
This commit is contained in:
@@ -0,0 +1,256 @@
|
||||
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>();
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user