335 lines
13 KiB
C#
335 lines
13 KiB
C#
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;
|
|
}
|
|
}
|