/// <summary>
/// Adds a data point to the curve builder. This doesn't always result in the generated curve changing immediately.
/// </summary>
/// <param name="p">The data point to add.</param>
/// <returns><see cref="AddPointResult"/> for info about this.</returns>
public AddPointResult AddPoint(VECTOR p)
{
using var _ = AddPointMarker.Auto();
VECTOR prev = _prev;
List <VECTOR> pts = _pts;
int count = pts.Count;
if (count != 0)
{
FLOAT td = VectorHelper.Distance(prev, p);
FLOAT md = _linDist;
if (td > md)
{
int first = int.MaxValue;
bool add = false;
FLOAT rd = td - md;
// OPTIMIZE if we're adding many points at once, we could do them in a batch
VECTOR dir = VectorHelper.Normalize(p - prev);
using var marker = AddPointDoWhileLoopMarker.Auto();
do
{
VECTOR np = prev + dir * md;
AddPointResult res = AddInternal(np);
first = Math.Min(first, res.FirstChangedIndex);
add |= res.WasAdded;
prev = np;
rd -= md;
} while(rd > md);
_prev = prev;
return(new AddPointResult(first, add));
}
return(AddPointResult.NO_CHANGE);
}
else
{
_prev = p;
_pts.Add(p);
_arclen.Add(0);
return(AddPointResult.NO_CHANGE); // no curves were actually added yet
}
}
/// <summary>
/// Gets the tangent for the start of the cure.
/// </summary>
public VECTOR GetLeftTangent(int last)
{
#if OPTIMIZED_GETLEFTTANGENT
using var _ = GetLeftTangentMarker.Auto();
unsafe
{
using (_pts.ViewAsNativeArray(out var pts))
using (_arclen.ViewAsNativeArray(out var arclen))
{
OptimizedHelpers.GetLeftTangent(last, (VECTOR *)pts.GetUnsafePtr(), pts.Length, (FLOAT *)arclen.GetUnsafePtr(), arclen.Length, out var tanL);
return(tanL);
}
}
#else
using var _ = GetLeftTangentMarker.Auto();
List <VECTOR> pts = _pts;
List <FLOAT> arclen = _arclen;
FLOAT totalLen = arclen[arclen.Count - 1];
VECTOR p0 = pts[0];
VECTOR tanL = VectorHelper.Normalize(pts[1] - p0);
VECTOR total = tanL;
FLOAT weightTotal = 1;
last = Math.Min(END_TANGENT_N_PTS, last - 1);
for (int i = 2; i <= last; i++)
{
FLOAT ti = 1 - (arclen[i] / totalLen);
FLOAT weight = ti * ti * ti;
VECTOR v = VectorHelper.Normalize(pts[i] - p0);
total += v * weight;
weightTotal += weight;
}
// if the vectors add up to zero (ie going opposite directions), there's no way to normalize them
if (VectorHelper.Length(total) > EPSILON)
{
tanL = VectorHelper.Normalize(total / weightTotal);
}
return(tanL);
#endif
}
/// <summary>
/// Gets the tangent for the the end of the curve.
/// </summary>
public VECTOR GetRightTangent(int first)
{
#if OPTIMIZED_GETRIGHTTANGENT
using var _ = GetRightTangentMarker.Auto();
unsafe
{
using (_pts.ViewAsNativeArray(out var pts))
using (_arclen.ViewAsNativeArray(out var arclen))
{
OptimizedHelpers.GetRightTangent(first, (VECTOR *)pts.GetUnsafePtr(), pts.Length, (FLOAT *)arclen.GetUnsafePtr(), arclen.Length, out var tanR);
return(tanR);
}
}
#else
using var _ = GetRightTangentMarker.Auto();
List <VECTOR> pts = _pts;
List <FLOAT> arclen = _arclen;
FLOAT totalLen = arclen[arclen.Count - 1];
VECTOR p3 = pts[pts.Count - 1];
VECTOR tanR = VectorHelper.Normalize(pts[pts.Count - 2] - p3);
VECTOR total = tanR;
FLOAT weightTotal = 1;
first = Math.Max(pts.Count - (END_TANGENT_N_PTS + 1), first + 1);
for (int i = pts.Count - 3; i >= first; i--)
{
FLOAT t = arclen[i] / totalLen;
FLOAT weight = t * t * t;
VECTOR v = VectorHelper.Normalize(pts[i] - p3);
total += v * weight;
weightTotal += weight;
}
if (VectorHelper.Length(total) > EPSILON)
{
tanR = VectorHelper.Normalize(total / weightTotal);
}
return(tanR);
#endif
}
public static unsafe void GetRightTangent(int first, Vector2 *pts, int ptsCount, float *arclen, int arclenCount, out Vector2 retval)
{
var totalLen = arclen[arclenCount - 1];
var p3 = pts[ptsCount - 1];
var tanR = VectorHelper.Normalize(pts[ptsCount - 2] - p3);
var total = tanR;
var weightTotal = 1.0f;
first = Math.Max(ptsCount - (CurveFitBase.END_TANGENT_N_PTS + 1), first + 1);
for (int i = ptsCount - 3; i >= first; i--)
{
var t = arclen[i] / totalLen;
var weight = t * t * t;
var v = VectorHelper.Normalize(pts[i] - p3);
total += v * weight;
weightTotal += weight;
}
if (VectorHelper.Length(total) > CurveFitBase.EPSILON)
{
tanR = VectorHelper.Normalize(total / weightTotal);
}
retval = tanR;
}
public static unsafe void GetLeftTangent(int last, Vector2 *pts, int ptsCount, float *arclen, int arclenCount, out Vector2 retval)
{
float totalLen = arclen[arclenCount - 1];
Vector2 p0 = pts[0];
Vector2 tanL = VectorHelper.Normalize(pts[1] - p0);
Vector2 total = tanL;
float weightTotal = 1;
last = Math.Min(CurveFitBase.END_TANGENT_N_PTS, last - 1);
for (int i = 2; i <= last; i++)
{
float ti = 1 - (arclen[i] / totalLen);
float weight = ti * ti * ti;
Vector2 v = VectorHelper.Normalize(pts[i] - p0);
total += v * weight;
weightTotal += weight;
}
// if the vectors add up to zero (ie going opposite directions), there's no way to normalize them
if (VectorHelper.Length(total) > CurveFitBase.EPSILON)
{
tanL = VectorHelper.Normalize(total / weightTotal);
}
retval = tanL;
}
/// <summary>
/// Gets the tangent at a given point in the curve.
/// </summary>
protected VECTOR GetCenterTangent(int first, int last, int split)
{
using var _ = GetCenterTangentMarker.Auto();
List <VECTOR> pts = _pts;
List <FLOAT> arclen = _arclen;
// because we want to maintain C1 continuity on the spline, the tangents on either side must be inverses of one another
Debug.Assert(first < split && split < last);
FLOAT splitLen = arclen[split];
VECTOR pSplit = pts[split];
// left side
FLOAT firstLen = arclen[first];
FLOAT partLen = splitLen - firstLen;
VECTOR total = default(VECTOR);
FLOAT weightTotal = 0;
for (int i = Math.Max(first, split - MID_TANGENT_N_PTS); i < split; i++)
{
FLOAT t = (arclen[i] - firstLen) / partLen;
FLOAT weight = t * t * t;
VECTOR v = VectorHelper.Normalize(pts[i] - pSplit);
total += v * weight;
weightTotal += weight;
}
VECTOR tanL = VectorHelper.Length(total) > EPSILON && weightTotal > EPSILON?
VectorHelper.Normalize(total / weightTotal) :
VectorHelper.Normalize(pts[split - 1] - pSplit);
// right side
partLen = arclen[last] - splitLen;
int rMax = Math.Min(last, split + MID_TANGENT_N_PTS);
total = default(VECTOR);
weightTotal = 0;
for (int i = split + 1; i <= rMax; i++)
{
FLOAT ti = 1 - ((arclen[i] - splitLen) / partLen);
FLOAT weight = ti * ti * ti;
VECTOR v = VectorHelper.Normalize(pSplit - pts[i]);
total += v * weight;
weightTotal += weight;
}
VECTOR tanR = VectorHelper.Length(total) > EPSILON && weightTotal > EPSILON?
VectorHelper.Normalize(total / weightTotal) :
VectorHelper.Normalize(pSplit - pts[split + 1]);
// The reason we separate this into two halves is because we want the right and left tangents to be weighted
// equally no matter the weights of the individual parts of them, so that one of the curves doesn't get screwed
// for the pleasure of the other half
total = tanL + tanR;
// Since the points are never coincident, the vector between any two of them will be normalizable, however this can happen in some really
// odd cases when the points are going directly opposite directions (therefore the tangent is undefined)
if (VectorHelper.LengthSquared(total) < EPSILON)
{
// try one last time using only the three points at the center, otherwise just use one of the sides
tanL = VectorHelper.Normalize(pts[split - 1] - pSplit);
tanR = VectorHelper.Normalize(pSplit - pts[split + 1]);
total = tanL + tanR;
return(VectorHelper.LengthSquared(total) < EPSILON ? tanL : VectorHelper.Normalize(total / 2));
}
else
{
return(VectorHelper.Normalize(total / 2));
}
}
public VECTOR Tangent(FLOAT t)
{
return(VectorHelper.Normalize(Derivative(t)));
}
private AddPointResult AddInternal(VECTOR np)
{
using var _ = AddInternalMarker.Auto();
List <VECTOR> pts = _pts;
int last = pts.Count;
Debug.Assert(last != 0); // should always have one point at least
_pts.Add(np);
_arclen.Add(_totalLength = _totalLength + _linDist);
if (last == 1)
{
// This is the second point
Debug.Assert(_result.Count == 0);
VECTOR p0 = pts[0];
VECTOR tanL = VectorHelper.Normalize(np - p0);
VECTOR tanR = -tanL;
_tanL = tanL;
FLOAT alpha = _linDist / 3;
VECTOR p1 = (tanL * alpha) + p0;
VECTOR p2 = (tanR * alpha) + np;
_result.Add(new CubicBezier(p0, p1, p2, np));
return(new AddPointResult(0, true));
}
else
{
int lastCurve = _result.Count - 1;
int first = _first;
// If we're on the first curve, we're free to improve the left tangent
VECTOR tanL = lastCurve == 0 ? GetLeftTangent(last) : _tanL;
// We can always do the end tangent
VECTOR tanR = GetRightTangent(first);
// Try fitting with the new point
int split;
CubicBezier curve;
if (FitCurve(first, last, tanL, tanR, out curve, out split))
{
_result[lastCurve] = curve;
return(new AddPointResult(lastCurve, false));
}
else
{
// Need to split
// first, get mid tangent
VECTOR tanM1 = GetCenterTangent(first, last, split);
VECTOR tanM2 = -tanM1;
// PERHAPS do a full fitRecursive here since its our last chance?
// our left tangent might be based on points outside the new curve (this is possible for mid tangents too
// but since we need to maintain C1 continuity, it's too late to do anything about it)
if (first == 0 && split < END_TANGENT_N_PTS)
{
tanL = GetLeftTangent(split);
}
// do a final pass on the first half of the curve
int unused;
FitCurve(first, split, tanL, tanM1, out curve, out unused);
_result[lastCurve] = curve;
// perpare to fit the second half
FitCurve(split, last, tanM2, tanR, out curve, out unused);
_result.Add(curve);
_first = split;
_tanL = tanM2;
return(new AddPointResult(lastCurve, true));
}
}
}