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