Initial commit from MyParking project
This commit is contained in:
@@ -0,0 +1,93 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Drawing;
|
||||
using System.Numerics;
|
||||
using System.Text;
|
||||
using static CommonUsage.Geometries.CircularArc;
|
||||
|
||||
namespace CommonUsage.Geometries
|
||||
{
|
||||
/// <summary>
|
||||
/// 便于直接创建几何形状并求几何形状的切点、切线等。
|
||||
/// </summary>
|
||||
public abstract class AbstractGeometry
|
||||
{
|
||||
protected AbstractGeometry()
|
||||
{
|
||||
PaddingType = Padding.StartExtendEndExtend;
|
||||
VisualizeOption = new VisualizeOption(Color.Red, Color.Gray);
|
||||
}
|
||||
|
||||
public Padding PaddingType;
|
||||
|
||||
public abstract (Vector2 Pt, float Angle, float Bias, float Position) QueryTangentPoint(Vector2 point);
|
||||
|
||||
public abstract void Visualize(Action<VisDot> processDot, Action<VisLine> processLine,
|
||||
bool visExtendedPart = false);
|
||||
|
||||
public VisualizeOption VisualizeOption;
|
||||
|
||||
/// <summary>
|
||||
/// 查询指定位置的曲率。
|
||||
/// </summary>
|
||||
/// <param name="position">从起点到查询位置的距离。</param>
|
||||
/// <returns></returns>
|
||||
public abstract float QueryCurvature(float position);
|
||||
|
||||
public abstract float Length();
|
||||
}
|
||||
|
||||
public class VisualizeOption
|
||||
{
|
||||
public VisualizeOption(Color mainColor, Color auxiliaryColor)
|
||||
{
|
||||
MainColor = mainColor;
|
||||
AuxiliaryColor = auxiliaryColor;
|
||||
}
|
||||
|
||||
public Color MainColor;
|
||||
public Color AuxiliaryColor;
|
||||
public bool DrawAuxiliary = true;
|
||||
public bool VisualizeDirection = true;
|
||||
}
|
||||
|
||||
public enum Padding
|
||||
{
|
||||
StartLineEndLine = 0b_0001_0001,
|
||||
StartLineEndExtend = 0b_0001_0010,
|
||||
StartExtendEndLine = 0b_0010_0001,
|
||||
StartExtendEndExtend = 0b_0010_0010,
|
||||
}
|
||||
|
||||
public class VisDot
|
||||
{
|
||||
public VisDot(Vector2 point, Color color)
|
||||
{
|
||||
Point = point;
|
||||
Color = color;
|
||||
}
|
||||
|
||||
public Vector2 Point;
|
||||
public Color Color;
|
||||
}
|
||||
|
||||
public class VisLine
|
||||
{
|
||||
public VisLine(Vector2 start, Vector2 end, bool startArrow, bool endArrow, Color color, float width = 1)
|
||||
{
|
||||
Start = start;
|
||||
End = end;
|
||||
StartArrow = startArrow;
|
||||
EndArrow = endArrow;
|
||||
Color = color;
|
||||
Width = width;
|
||||
}
|
||||
|
||||
public Vector2 Start;
|
||||
public Vector2 End;
|
||||
public bool StartArrow = false;
|
||||
public bool EndArrow = false;
|
||||
public Color Color;
|
||||
public float Width;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,334 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Diagnostics;
|
||||
using System.Linq;
|
||||
using System.Numerics;
|
||||
using System.Reflection;
|
||||
using CommonUsage.Mathematics;
|
||||
|
||||
namespace CommonUsage.Geometries
|
||||
{
|
||||
public class BezierCurve : AbstractGeometry
|
||||
{
|
||||
|
||||
public BezierCurve(List<Vector2> controlPoints, int resolution = 100)
|
||||
{
|
||||
// Console.WriteLine($"BezierCurve1");
|
||||
// Console.WriteLine(string.Join(" ",controlPoints.Select(p=>$"{p.X:f2},{p.Y:f2}")));
|
||||
_controlPoints = controlPoints;
|
||||
_resolution = resolution;
|
||||
InitializeBezier();
|
||||
}
|
||||
|
||||
public override void Visualize(Action<VisDot> processDot, Action<VisLine> processLine, bool visExtendedPart = false)
|
||||
{
|
||||
if (VisualizeOption.DrawAuxiliary)
|
||||
for (var i = 0; i < _controlPoints.Count - 1; ++i)
|
||||
{
|
||||
processLine(new VisLine(_controlPoints[i], _controlPoints[i + 1],
|
||||
false, false, VisualizeOption.AuxiliaryColor));
|
||||
if (i == 0) continue;
|
||||
processDot(new VisDot(_controlPoints[i], VisualizeOption.AuxiliaryColor));
|
||||
}
|
||||
|
||||
for (var i = 0; i < _bezierPoints.Count - 1; ++i)
|
||||
{
|
||||
if (Direction == -1)
|
||||
{
|
||||
processLine(new VisLine(_bezierPoints[i + 1], _bezierPoints[i],
|
||||
false, i == (int)(_bezierPoints.Count / 2), VisualizeOption.MainColor, 2));
|
||||
}
|
||||
else
|
||||
{
|
||||
processLine(new VisLine(_bezierPoints[i], _bezierPoints[i + 1],
|
||||
false, i == (int)(_bezierPoints.Count / 2), VisualizeOption.MainColor, 2));
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
public (Vector2 Point, int Id) QueryPoint(Vector2 point)
|
||||
{
|
||||
var p = new Vector2();
|
||||
var id = -1;
|
||||
var bestDistance = float.MaxValue;
|
||||
|
||||
var hashes = _bias.Select(bb => CalculateHash(point, 100, bb.X, bb.Y)).ToList();
|
||||
|
||||
void TryQuery(Dictionary<uint, List<(Vector2 Point, int Id)>> dict, List<uint> hashList)
|
||||
{
|
||||
foreach (var hash in hashList)
|
||||
{
|
||||
if (!dict.TryGetValue(hash, out var ll)) continue;
|
||||
foreach (var (q, qId) in ll)
|
||||
{
|
||||
var d = Vector2.Distance(q, point);
|
||||
if (d < bestDistance)
|
||||
{
|
||||
p = q;
|
||||
id = qId;
|
||||
bestDistance = d;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
//map目前有bug,取消cpu占用也不严重，必要时候在优化
|
||||
// TryQuery(_pointsMappingSmall, hashes);
|
||||
//
|
||||
// if (id == -1)
|
||||
// {
|
||||
// hashes = _bias.Select(bb => CalculateHash(point, 1000, bb.X, bb.Y)).ToList();
|
||||
// TryQuery(_pointsMappingBig, hashes);
|
||||
// }
|
||||
|
||||
if (id == -1)
|
||||
{
|
||||
// todo: improve the way to find closest point if mappings fail
|
||||
(p, id) = _bezierPoints.Select((p, i) => (p, i))
|
||||
.OrderBy(pair => CommonMath.dist(pair.p.X, pair.p.Y, point.X, point.Y)).First();
|
||||
}
|
||||
|
||||
return (p, id);
|
||||
}
|
||||
|
||||
public override (Vector2 Pt, float Angle, float Bias, float Position) QueryTangentPoint(Vector2 point)
|
||||
{
|
||||
var (p, id) = QueryPoint(point);
|
||||
var tangent = _tangents[id];
|
||||
var (bias, lp, fd) = CommonMath.Project2DLine(point, p, tangent);
|
||||
var next = fd > 0 ? id + 1 : id - 1;
|
||||
if (id == 0) next = 1;
|
||||
// Console.WriteLine($"id:{id} next:{next} tangent:{tangent} _tangents.Count:{_tangents.Count}");
|
||||
if (next > 0 && next < _tangents.Count)//线性插值
|
||||
{
|
||||
var (_, _, t) = CommonMath.Project2DLine(point, _bezierPoints[id], _bezierPoints[next]);
|
||||
var partial = t / Vector2.Distance(_bezierPoints[id], _bezierPoints[next]);
|
||||
if (partial >= 0 && partial <= 1)
|
||||
{
|
||||
tangent = CommonMath.RoundTh(_tangents[id] +
|
||||
partial * CommonMath.RoundTh(_tangents[next] - _tangents[id]));
|
||||
if (CommonMath.RoundTh(_tangents[next] - _tangents[id]) > 5)
|
||||
Console.WriteLine($"bezier tangents bug, tanget: {id}:{_tangents[id]} {next}:{_tangents[next]}");
|
||||
}
|
||||
// else Console.WriteLine("bezier tangents bug");
|
||||
}
|
||||
return (lp, tangent, bias, fd + _sumDistances[id]);
|
||||
}
|
||||
|
||||
public override float Length()
|
||||
{
|
||||
return _length;
|
||||
}
|
||||
|
||||
public override float QueryCurvature(float position)
|
||||
{
|
||||
int id = _sumDistances.Count - 1;
|
||||
if (position <= 0) id = 0;
|
||||
else
|
||||
{
|
||||
for (int i = 1; i < _sumDistances.Count; i++)
|
||||
{
|
||||
if (position > _sumDistances[i - 1] && position <= _sumDistances[i])
|
||||
{
|
||||
id = i;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
var result = _curvatures[id];
|
||||
if (id > 0 && id < _sumDistances.Count - 1)//插值
|
||||
{
|
||||
var partial = (position - _sumDistances[id - 1]) / (_sumDistances[id] - _sumDistances[id - 1]);
|
||||
if (partial >= 0 && partial <= 1) result = (1 - partial) * _curvatures[id - 1] + partial * _curvatures[id];
|
||||
else Console.WriteLine("bezier curvature bug");
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
public Vector3 QueryBezierPointsById(int id)
|
||||
{
|
||||
if (id < 0 || id > Resolution)
|
||||
{
|
||||
Console.WriteLine($"QueryBezierPointsById out of range, Resolution:{Resolution},id:{id}.");
|
||||
return new Vector3(0, 0, 0);
|
||||
}
|
||||
|
||||
return new Vector3(_bezierPoints[id].X, _bezierPoints[id].Y, _tangents[id]);
|
||||
}
|
||||
|
||||
public List<Vector2> ControlPoints => _controlPoints;
|
||||
|
||||
public int Resolution => _resolution;
|
||||
/// <summary>
|
||||
/// 仅用于simple显示路径方向
|
||||
/// </summary>
|
||||
public int Direction = 1;
|
||||
public int Order => _order;
|
||||
|
||||
// public List<float> Tangents => _tangents;
|
||||
|
||||
public void UpdateControlPoint(int id, Vector2 point)
|
||||
{
|
||||
_controlPoints[id] = point;
|
||||
InitializeBezier();
|
||||
}
|
||||
|
||||
public void AddControlPoint(int id, Vector2 point)
|
||||
{
|
||||
_controlPoints.Insert(id, point);
|
||||
InitializeBezier();
|
||||
}
|
||||
|
||||
public void RemoveControlPoint(int id)
|
||||
{
|
||||
_controlPoints.RemoveAt(id);
|
||||
InitializeBezier();
|
||||
}
|
||||
|
||||
public Vector2 GetMidPoint()
|
||||
{
|
||||
return _bezierPoints[(int)Math.Ceiling(_resolution / 2d)];
|
||||
}
|
||||
|
||||
private void InitializeBezier()
|
||||
{
|
||||
_order = _controlPoints.Count - 1;
|
||||
// _bezierPoints = new List<Vector2>();
|
||||
var delta = 1.0f / _resolution;
|
||||
// for (int t = 0; t <= _resolution; t += 1)//下面循环算了，没必要先递归算一遍
|
||||
// _bezierPoints.Add(new Vector2(DeCasteljauX(_order, 0, t*delta), DeCasteljauY(_order, 0, t*delta)));
|
||||
var allPoints = new List<List<List<Vector2>>>();
|
||||
for (var i = 0; i < _order; i++)
|
||||
{
|
||||
var size = allPoints.Count;
|
||||
var morePoints = new List<List<Vector2>>();
|
||||
for (var j = 0; j < _order - i; j++)
|
||||
{
|
||||
var points = new List<Vector2>();
|
||||
for (int t = 0; t <= _resolution; t += 1)
|
||||
{
|
||||
float p0x;
|
||||
float p1x;
|
||||
float p0y;
|
||||
float p1y;
|
||||
var z = t;
|
||||
if (size > 0)
|
||||
{
|
||||
p0x = allPoints[i - 1][j][z].X;
|
||||
p1x = allPoints[i - 1][j + 1][z].X;
|
||||
p0y = allPoints[i - 1][j][z].Y;
|
||||
p1y = allPoints[i - 1][j + 1][z].Y;
|
||||
}
|
||||
else
|
||||
{
|
||||
p0x = _controlPoints[j].X;
|
||||
p1x = _controlPoints[j + 1].X;
|
||||
p0y = _controlPoints[j].Y;
|
||||
p1y = _controlPoints[j + 1].Y;
|
||||
}
|
||||
|
||||
var part = t * delta;
|
||||
points.Add(new Vector2((1 - part) * p0x + part * p1x, (1 - part) * p0y + part * p1y));
|
||||
}
|
||||
morePoints.Add(points);
|
||||
}
|
||||
allPoints.Add(morePoints);
|
||||
}
|
||||
|
||||
_bezierPoints = allPoints.Last().Last();
|
||||
_tangentInfo = allPoints;
|
||||
_tangents = Enumerable.Repeat(0f, _bezierPoints.Count).ToList();
|
||||
_curvatures = Enumerable.Repeat(0f, _bezierPoints.Count).ToList();
|
||||
var p2 = allPoints[Order - 2];
|
||||
for (var id = 0; id < _bezierPoints.Count; ++id)
|
||||
{
|
||||
_tangents[id] =
|
||||
(float)(Math.Atan2(p2[1][id].Y - p2[0][id].Y, p2[1][id].X - p2[0][id].X) / Math.PI * 180);
|
||||
if (id != 0) _curvatures[id] = (float)((CommonMath.ThDiff(_tangents[id], _tangents[id - 1]) / 180 * Math.PI)
|
||||
/ (Vector2.Distance(_bezierPoints[id], _bezierPoints[id - 1]) / 1000));
|
||||
}
|
||||
// Console.WriteLine($"{string.Join("\n", _tangents.Select((val, i) => $"{i}: {val}"))}");
|
||||
_tangents[0] = _tangents[1]; // todo: here is temporary fix
|
||||
_curvatures[0] = _curvatures[1];
|
||||
for (var id = 1; id < _bezierPoints.Count - 1; ++id)//前移0.5
|
||||
_curvatures[id] = (_curvatures[id] + _curvatures[id + 1]) / 2;
|
||||
|
||||
_remainDistances = Enumerable.Repeat(0f, _bezierPoints.Count).ToList();
|
||||
_sumDistances = Enumerable.Repeat(0f, _bezierPoints.Count).ToList();
|
||||
for (var i = _bezierPoints.Count - 2; i >= 0; --i)
|
||||
{
|
||||
_remainDistances[i] =
|
||||
_remainDistances[i + 1] + Vector2.Distance(_bezierPoints[i], _bezierPoints[i + 1]);
|
||||
}
|
||||
for (var i = 1; i < _bezierPoints.Count; ++i)
|
||||
{
|
||||
_sumDistances[i] =
|
||||
_sumDistances[i - 1] + Vector2.Distance(_bezierPoints[i], _bezierPoints[i - 1]);
|
||||
}
|
||||
_length = _sumDistances.Last();
|
||||
|
||||
_minX = _bezierPoints.Min(pp => pp.X);
|
||||
_minY = _bezierPoints.Min(pp => pp.Y);
|
||||
|
||||
var tmpList = _bezierPoints.Select((point, index) => (point, index)).ToList();
|
||||
return;
|
||||
void GenerateGridMapping(ref Dictionary<uint, List<(Vector2 Point, int Id)>> dict, float gSize)
|
||||
{
|
||||
dict = new Dictionary<uint, List<(Vector2 Point, int Id)>>();
|
||||
foreach (var (point, index) in tmpList)
|
||||
{
|
||||
var hash = CalculateHash(point, gSize);
|
||||
if (dict.TryGetValue(hash, out var ll))
|
||||
ll.Add((point, index));
|
||||
else dict[hash] = new List<(Vector2 Point, int Id)>() { (point, index) };
|
||||
}
|
||||
}
|
||||
|
||||
GenerateGridMapping(ref _pointsMappingSmall, 100);
|
||||
GenerateGridMapping(ref _pointsMappingBig, 1000);
|
||||
}
|
||||
|
||||
private uint CalculateHash(Vector2 point, float gridSize, int xBias = 0, int yBias = 0)
|
||||
{
|
||||
return (uint)(((int)((point.X - _minX) / gridSize) + xBias) << 16 + (((int)((point.Y - _minY) / gridSize) + yBias) & 0xffff));
|
||||
}
|
||||
|
||||
private readonly List<(int X, int Y)> _bias = new()
|
||||
{
|
||||
new(-1, -1), new(-1, 0), new(-1, 1),
|
||||
new(0, -1), new(0, 0), new(0, 1),
|
||||
new(1, -1), new(1, 0), new(1, 1),
|
||||
};
|
||||
|
||||
private float DeCasteljauX(int i, int j, float t)
|
||||
{
|
||||
if (i == 1)
|
||||
return (1 - t) * _controlPoints[j].X + t * _controlPoints[j + 1].X;
|
||||
return (1 - t) * DeCasteljauX(i - 1, j, t) + t * DeCasteljauX(i - 1, j + 1, t);
|
||||
}
|
||||
|
||||
private float DeCasteljauY(int i, int j, float t)
|
||||
{
|
||||
if (i == 1)
|
||||
return (1 - t) * _controlPoints[j].Y + t * _controlPoints[j + 1].Y;
|
||||
return (1 - t) * DeCasteljauY(i - 1, j, t) + t * DeCasteljauY(i - 1, j + 1, t);
|
||||
}
|
||||
|
||||
private int _order;
|
||||
private int _resolution;
|
||||
private List<Vector2> _controlPoints;
|
||||
private List<Vector2> _bezierPoints;
|
||||
private List<List<List<Vector2>>> _tangentInfo;
|
||||
private List<float> _tangents;
|
||||
private List<float> _remainDistances;
|
||||
private List<float> _sumDistances;
|
||||
private List<float> _curvatures;
|
||||
private Dictionary<uint, List<(Vector2 Point, int Id)>> _pointsMappingSmall;
|
||||
private Dictionary<uint, List<(Vector2 Point, int Id)>> _pointsMappingBig;
|
||||
private float _minX, _minY;
|
||||
private float _length;
|
||||
}
|
||||
}
|
||||
@@ -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>();
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,349 @@
|
||||
using CommonUsage.Mathematics;
|
||||
using System.Collections.Generic;
|
||||
using System.Numerics;
|
||||
using System;
|
||||
using System.Linq;
|
||||
|
||||
namespace CommonUsage.Geometries
|
||||
{
|
||||
public class NurbsCurve : AbstractGeometry
|
||||
{
|
||||
|
||||
public NurbsCurve(List<Vector2> controlPoints, List<float> weights, List<float> knotVector, int frame = 100)
|
||||
{
|
||||
_controlPoints = controlPoints;
|
||||
_weights = weights;
|
||||
_knotVector = knotVector;
|
||||
_frame = frame;
|
||||
InitializeNurbs();
|
||||
}
|
||||
|
||||
public override void Visualize(Action<VisDot> processDot, Action<VisLine> processLine, bool visExtendedPart = false)
|
||||
{
|
||||
if (VisualizeOption.DrawAuxiliary)
|
||||
for (var i = 0; i < _controlPoints.Count - 1; ++i)
|
||||
{
|
||||
processLine(new VisLine(_controlPoints[i], _controlPoints[i + 1],
|
||||
false, false, VisualizeOption.AuxiliaryColor));
|
||||
if (i == 0) continue;
|
||||
processDot(new VisDot(_controlPoints[i], VisualizeOption.AuxiliaryColor));
|
||||
}
|
||||
|
||||
for (var i = 0; i < _nurbsPoints.Count - 1; ++i)
|
||||
{
|
||||
if (Direction == -1)
|
||||
{
|
||||
processLine(new VisLine(_nurbsPoints[i + 1], _nurbsPoints[i],
|
||||
false, i == (int)(_nurbsPoints.Count / 2), VisualizeOption.MainColor, 2));
|
||||
}
|
||||
else
|
||||
{
|
||||
processLine(new VisLine(_nurbsPoints[i], _nurbsPoints[i + 1],
|
||||
false, i == (int)(_nurbsPoints.Count / 2), VisualizeOption.MainColor, 2));
|
||||
}
|
||||
}
|
||||
}
|
||||
public (Vector2 Point, int Id) QueryPoint(Vector2 point)
|
||||
{
|
||||
var p = new Vector2();
|
||||
var id = -1;
|
||||
var bestDistance = float.MaxValue;
|
||||
|
||||
var hashes = _bias.Select(bb => CalculateHash(point, 100, bb.X, bb.Y)).ToList();
|
||||
|
||||
void TryQuery(Dictionary<uint, List<(Vector2 Point, int Id)>> dict, List<uint> hashList)
|
||||
{
|
||||
foreach (var hash in hashList)
|
||||
{
|
||||
if (!dict.TryGetValue(hash, out var ll)) continue;
|
||||
foreach (var (q, qId) in ll)
|
||||
{
|
||||
var d = Vector2.Distance(q, point);
|
||||
if (d < bestDistance)
|
||||
{
|
||||
p = q;
|
||||
id = qId;
|
||||
bestDistance = d;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (id == -1)
|
||||
{
|
||||
// todo: improve the way to find closest point if mappings fail
|
||||
(p, id) = _nurbsPoints.Select((p, i) => (p, i))
|
||||
.OrderBy(pair => CommonMath.dist(pair.p.X, pair.p.Y, point.X, point.Y)).First();
|
||||
}
|
||||
|
||||
return (p, id);
|
||||
}
|
||||
|
||||
private uint CalculateHash(Vector2 point, float gridSize, int xBias = 0, int yBias = 0)
|
||||
{
|
||||
return (uint)(((int)((point.X - _minX) / gridSize) + xBias) << 16 + (((int)((point.Y - _minY) / gridSize) + yBias) & 0xffff));
|
||||
}
|
||||
|
||||
private readonly List<(int X, int Y)> _bias = new()
|
||||
{
|
||||
new(-1, -1), new(-1, 0), new(-1, 1),
|
||||
new(0, -1), new(0, 0), new(0, 1),
|
||||
new(1, -1), new(1, 0), new(1, 1),
|
||||
};
|
||||
|
||||
public override (Vector2 Pt, float Angle, float Bias, float Position) QueryTangentPoint(Vector2 point)
|
||||
{
|
||||
var (p, id) = QueryPoint(point);
|
||||
var tangent = _tangents[id];
|
||||
var (bias, lp, fd) = CommonMath.Project2DLine(point, p, tangent);
|
||||
var next = fd > 0 ? id + 1 : id - 1;
|
||||
if (next > 0 && next < _tangents.Count)//线性插值
|
||||
{
|
||||
var (_, _, t) = CommonMath.Project2DLine(point, _nurbsPoints[id], _nurbsPoints[next]);
|
||||
var partial = t / Vector2.Distance(_nurbsPoints[id], _nurbsPoints[next]);
|
||||
if (partial >= 0 && partial <= 1)
|
||||
{
|
||||
tangent = CommonMath.RoundTh(_tangents[id] +
|
||||
partial * CommonMath.RoundTh(_tangents[next] - _tangents[id]));
|
||||
if (CommonMath.RoundTh(_tangents[next] - _tangents[id]) > 5)
|
||||
Console.WriteLine($"Nurbs tangents bug, tanget: {id}:{_tangents[id]} {next}:{_tangents[next]}");
|
||||
}
|
||||
else Console.WriteLine("Nurbs tangents bug");
|
||||
}
|
||||
return (lp, tangent, bias, fd + _sumDistances[id]);
|
||||
}
|
||||
|
||||
public override float QueryCurvature(float position)
|
||||
{
|
||||
int id = _sumDistances.Count - 1;
|
||||
if (position <= 0) id = 0;
|
||||
else
|
||||
{
|
||||
for (int i = 1; i < _sumDistances.Count; i++)
|
||||
{
|
||||
if (position > _sumDistances[i - 1] && position <= _sumDistances[i])
|
||||
{
|
||||
id = i;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
var result = _curvatures[id];
|
||||
if (id > 0 && id < _sumDistances.Count - 1)//插值
|
||||
{
|
||||
var partial = (position - _sumDistances[id - 1]) / (_sumDistances[id] - _sumDistances[id - 1]);
|
||||
if (partial >= 0 && partial <= 1) result = (1 - partial) * _curvatures[id - 1] + partial * _curvatures[id];
|
||||
else Console.WriteLine("Nurbs curvature bug");
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
public Vector3 QueryNurbsPointsById(int id)
|
||||
{
|
||||
if (id < 0 || id > Frame)
|
||||
{
|
||||
Console.WriteLine($"QueryBezierPointsById out of range, Resolution:{Frame},id:{id}.");
|
||||
return new Vector3(0, 0, 0);
|
||||
}
|
||||
|
||||
return new Vector3(_nurbsPoints[id].X, _nurbsPoints[id].Y, _tangents[id]);
|
||||
}
|
||||
public override float Length()
|
||||
{
|
||||
return _length;
|
||||
}
|
||||
|
||||
public int Order => _order;
|
||||
public List<Vector2> ControlPoints => _controlPoints;
|
||||
public List<float> Weights => _weights;
|
||||
public List<float> KnotVector => _knotVector;
|
||||
public int Frame => _frame;
|
||||
|
||||
public int Direction = 1;
|
||||
|
||||
|
||||
public void UpdateControlPoint(int id, Vector2 point)
|
||||
{
|
||||
_controlPoints[id] = point;
|
||||
InitializeNurbs();
|
||||
}
|
||||
public void UpdateNurbsWeihgts(int id, float weight)
|
||||
{
|
||||
_weights[id] = weight;
|
||||
InitializeNurbs();
|
||||
|
||||
}
|
||||
public void AddControlPoint(int id, Vector2 point)
|
||||
{
|
||||
_controlPoints.Insert(id, point);
|
||||
InitializeNurbs();
|
||||
}
|
||||
|
||||
public void RemoveControlPoint(int id)
|
||||
{
|
||||
_controlPoints.RemoveAt(id);
|
||||
InitializeNurbs();
|
||||
}
|
||||
|
||||
public Vector2 GetMidPoint()
|
||||
{
|
||||
return _nurbsPoints[(int)Math.Ceiling(_frame / 2d)];
|
||||
}
|
||||
|
||||
private void InitializeNurbs()
|
||||
{
|
||||
_order = _controlPoints.Count - 1;
|
||||
List<List<Vector2>> allpoints = new List<List<Vector2>>();
|
||||
List<Vector2> nurbsCurvePoints = new List<Vector2>();
|
||||
float delta = 1.0f / Frame;
|
||||
|
||||
for (float t = 0; t <= 1; t += delta)
|
||||
{
|
||||
var (point, tangent) = DeBoorAlgorithm(t);
|
||||
var points = new List<Vector2>
|
||||
{
|
||||
point,
|
||||
point + tangent // Tangent endpoint
|
||||
};
|
||||
allpoints.Add(points);
|
||||
nurbsCurvePoints.Add(point); // Store the curve point separately
|
||||
}
|
||||
|
||||
_nurbsPoints = nurbsCurvePoints;
|
||||
_tangents = Enumerable.Repeat(0f, _nurbsPoints.Count).ToList();
|
||||
_curvatures = Enumerable.Repeat(0f, _nurbsPoints.Count).ToList();
|
||||
|
||||
for (var id = 0; id < _nurbsPoints.Count - 1; ++id)
|
||||
{
|
||||
Vector2 p1 = _nurbsPoints[id];
|
||||
Vector2 p2 = _nurbsPoints[id + 1];
|
||||
|
||||
float tangentAngle = (float)Math.Atan2(p2.Y - p1.Y, p2.X - p1.X) * 180 / (float)Math.PI;
|
||||
_tangents[id] = tangentAngle;
|
||||
|
||||
// Calculate curvature using finite differences of tangent (second derivative approximation)
|
||||
if (id > 0)
|
||||
{
|
||||
float previousTangent = _tangents[id - 1];
|
||||
float curvature = (float)(CommonMath.ThDiff(tangentAngle, previousTangent) * Math.PI / 180) /
|
||||
(Vector2.Distance(p1, p2) / 1000);
|
||||
_curvatures[id] = curvature;
|
||||
}
|
||||
}
|
||||
|
||||
_curvatures.Insert(0, _curvatures[0]);
|
||||
for (var id = 1; id < _curvatures.Count - 1; ++id)
|
||||
{
|
||||
_curvatures[id] = (_curvatures[id] + _curvatures[id + 1]) / 2;
|
||||
}
|
||||
|
||||
_remainDistances = Enumerable.Repeat(0f, _nurbsPoints.Count).ToList();
|
||||
_sumDistances = Enumerable.Repeat(0f, _nurbsPoints.Count).ToList();
|
||||
_remainDistances[_nurbsPoints.Count - 1] = 0;
|
||||
|
||||
for (var i = _nurbsPoints.Count - 2; i >= 0; --i)
|
||||
{
|
||||
_remainDistances[i] = _remainDistances[i + 1] + Vector2.Distance(_nurbsPoints[i], _nurbsPoints[i + 1]);
|
||||
}
|
||||
|
||||
_sumDistances[0] = 0;
|
||||
for (var i = 1; i < _nurbsPoints.Count; ++i)
|
||||
{
|
||||
_sumDistances[i] = _sumDistances[i - 1] + Vector2.Distance(_nurbsPoints[i], _nurbsPoints[i - 1]);
|
||||
}
|
||||
|
||||
_length = _sumDistances.Last();
|
||||
|
||||
_minX = _nurbsPoints.Min(pp => pp.X);
|
||||
_minY = _nurbsPoints.Min(pp => pp.Y);
|
||||
|
||||
var tmpList = _nurbsPoints.Select((point, index) => (point, index)).ToList();
|
||||
// GenerateGridMapping(ref _pointsMappingSmall, 100, tmpList);
|
||||
// GenerateGridMapping(ref _pointsMappingBig, 1000, tmpList);
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
private float CalculateLength()
|
||||
{
|
||||
return _nurbsPoints.Zip(_nurbsPoints.Skip(1), Vector2.Distance).Sum();
|
||||
}
|
||||
|
||||
private (Vector2, Vector2) DeBoorAlgorithm(float t)
|
||||
{
|
||||
Vector2 numerator = Vector2.Zero;
|
||||
Vector2 tangentNumerator = Vector2.Zero;
|
||||
float denominator = 0f;
|
||||
|
||||
// Calculate the point on the curve
|
||||
for (int i = 0; i < ControlPoints.Count; ++i)
|
||||
{
|
||||
float basis = BasisFunction(i, _order, t) * Weights[i];
|
||||
numerator += basis * ControlPoints[i];
|
||||
denominator += basis;
|
||||
}
|
||||
|
||||
Vector2 point = numerator / denominator;
|
||||
|
||||
// Calculate the tangent vector using the analytical derivative
|
||||
for (int i = 0; i < ControlPoints.Count; ++i)
|
||||
{
|
||||
float basisDerivative = BasisFunctionDerivative(i, _order, t) * Weights[i];
|
||||
tangentNumerator += basisDerivative * ControlPoints[i];
|
||||
}
|
||||
Vector2 tangent = tangentNumerator / denominator;
|
||||
|
||||
return (point, tangent);
|
||||
}
|
||||
|
||||
private float BasisFunction(int i, int p, float t)
|
||||
{
|
||||
if (p == 0)
|
||||
return (KnotVector[i] <= t && t < KnotVector[i + 1]) ? 1.0f : 0.0f;
|
||||
|
||||
float denom1 = KnotVector[i + p] - KnotVector[i];
|
||||
float term1 = denom1 == 0 ? 0 : ((t - KnotVector[i]) / denom1) * BasisFunction(i, p - 1, t);
|
||||
|
||||
float denom2 = KnotVector[i + p + 1] - KnotVector[i + 1];
|
||||
float term2 = denom2 == 0 ? 0 : ((KnotVector[i + p + 1] - t) / denom2) * BasisFunction(i + 1, p - 1, t);
|
||||
|
||||
return term1 + term2;
|
||||
}
|
||||
|
||||
private float BasisFunctionDerivative(int i, int k, float t)
|
||||
{
|
||||
if (k == 0) return 0;
|
||||
|
||||
float denom1 = KnotVector[i + k] - KnotVector[i];
|
||||
float denom2 = KnotVector[i + k + 1] - KnotVector[i + 1];
|
||||
|
||||
float term1 = denom1 != 0 ? BasisFunction(i, k - 1, t) / denom1 : 0;
|
||||
float term2 = denom1 != 0 ? (t - KnotVector[i]) * BasisFunctionDerivative(i, k - 1, t) / denom1 : 0;
|
||||
|
||||
float term3 = denom2 != 0 ? -BasisFunction(i + 1, k - 1, t) / denom2 : 0;
|
||||
float term4 = denom2 != 0 ? (KnotVector[i + k + 1] - t) * BasisFunctionDerivative(i + 1, k - 1, t) / denom2 : 0;
|
||||
|
||||
return term1 + term2 + term3 + term4;
|
||||
}
|
||||
|
||||
private int _order;
|
||||
private List<Vector2> _controlPoints;
|
||||
private List<float> _weights;
|
||||
private List<float> _knotVector;
|
||||
private int _frame;
|
||||
private List<Vector2> _nurbsPoints;
|
||||
private List<List<List<Vector2>>> _tangentPoints;
|
||||
private List<float> _curvatures;
|
||||
private List<float> _sumDistances;
|
||||
private List<float> _remainDistances;
|
||||
private float _minX, _minY;
|
||||
private float _length;
|
||||
private Dictionary<uint, List<(Vector2 Point, int Id)>> _pointsMappingSmall;
|
||||
private Dictionary<uint, List<(Vector2 Point, int Id)>> _pointsMappingBig;
|
||||
private List<float> _tangents;
|
||||
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,71 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Numerics;
|
||||
using System.Text;
|
||||
|
||||
namespace CommonUsage.Geometries
|
||||
{
|
||||
/// <summary>
|
||||
/// MDCS数学类：向量。
|
||||
/// </summary>
|
||||
public class Vectoriel
|
||||
{
|
||||
public Vectoriel()
|
||||
{
|
||||
_vec2 = Vector2.Zero;
|
||||
_dir2 = Vector2.Normalize(_vec2);
|
||||
_len = _vec2.Length();
|
||||
_angle = (float)(Math.Atan2(_vec2.Y, _vec2.X) / Math.PI * 180f);
|
||||
}
|
||||
|
||||
public Vectoriel(Vector2 vec)
|
||||
{
|
||||
_vec2 = vec;
|
||||
_dir2 = Vector2.Normalize(_vec2);
|
||||
_len = _vec2.Length();
|
||||
_angle = (float)(Math.Atan2(_vec2.Y, _vec2.X) / Math.PI * 180f);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 通过笛卡尔坐标系X和Y值构建向量。
|
||||
/// </summary>
|
||||
/// <param name="x"></param>
|
||||
/// <param name="y"></param>
|
||||
/// <returns></returns>
|
||||
public static Vectoriel FromXY(float x, float y)
|
||||
{
|
||||
return new Vectoriel(new Vector2(x, y));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 通过极坐标系的角度和距离值构建向量。
|
||||
/// </summary>
|
||||
/// <param name="angle"></param>
|
||||
/// <param name="len"></param>
|
||||
/// <returns></returns>
|
||||
public static Vectoriel FromAngleLen(float angle, float len)
|
||||
{
|
||||
var rad = angle / 180f * Math.PI;
|
||||
return new Vectoriel(new Vector2((float)Math.Cos(rad), (float)Math.Sin(rad)) * len);
|
||||
}
|
||||
|
||||
public static implicit operator Vector2(Vectoriel vec)
|
||||
{
|
||||
return vec._vec2;
|
||||
}
|
||||
|
||||
public static explicit operator Vectoriel(Vector2 vec)
|
||||
{
|
||||
return FromXY(vec.X, vec.Y);
|
||||
}
|
||||
|
||||
public Vector2 Direction => _dir2;
|
||||
|
||||
public float Length => _len;
|
||||
|
||||
public float Angle => _angle;
|
||||
|
||||
private Vector2 _vec2, _dir2;
|
||||
private float _len, _angle;
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user