/// <summary> /// Attempts to find a slightly better parameterization for u on the given curve. /// </summary> protected void Reparameterize(int first, int last, CubicBezier curve) { List <VECTOR> pts = _pts; List <FLOAT> u = _u; int nPts = last - first; for (int i = 1; i < nPts; i++) { VECTOR p = pts[first + i]; FLOAT t = u[i]; FLOAT ti = 1 - t; // Control vertices for Q' VECTOR qp0 = (curve.p1 - curve.p0) * 3; VECTOR qp1 = (curve.p2 - curve.p1) * 3; VECTOR qp2 = (curve.p3 - curve.p2) * 3; // Control vertices for Q'' VECTOR qpp0 = (qp1 - qp0) * 2; VECTOR qpp1 = (qp2 - qp1) * 2; // Evaluate Q(t), Q'(t), and Q''(t) VECTOR p0 = curve.Sample(t); VECTOR p1 = ((ti * ti) * qp0) + ((2 * ti * t) * qp1) + ((t * t) * qp2); VECTOR p2 = (ti * qpp0) + (t * qpp1); // these are the actual fitting calculations using http://en.wikipedia.org/wiki/Newton%27s_method // We can't just use .X and .Y because Unity uses lower-case "x" and "y". FLOAT num = ((VectorHelper.GetX(p0) - VectorHelper.GetX(p)) * VectorHelper.GetX(p1)) + ((VectorHelper.GetY(p0) - VectorHelper.GetY(p)) * VectorHelper.GetY(p1)); FLOAT den = (VectorHelper.GetX(p1) * VectorHelper.GetX(p1)) + (VectorHelper.GetY(p1) * VectorHelper.GetY(p1)) + ((VectorHelper.GetX(p0) - VectorHelper.GetX(p)) * VectorHelper.GetX(p2)) + ((VectorHelper.GetY(p0) - VectorHelper.GetY(p)) * VectorHelper.GetY(p2)); FLOAT newU = t - num / den; if (Math.Abs(den) > EPSILON && newU >= 0 && newU <= 1) { u[i] = newU; } } }
/// <summary> /// Modifies a curve in the spline. It must connect with the previous and next curves (if applicable). This requires that the /// arc length table be recalculated for that curve and all curves after it -- for example, if you update the first curve in the /// spline, each curve after that would need to be recalculated (could avoid this by caching the lengths on a per-curve basis if you're /// doing this often, but since the typical case is only updating the last curve, and the entire array needs to be visited anyway, it /// wouldn't save much). /// </summary> /// <param name="index">Index of the curve to update in <see cref="Curves"/>.</param> /// <param name="curve">The new curve with which to replace it.</param> public void Update(int index, CubicBezier curve) { if (index < 0) { throw new IndexOutOfRangeException("Negative index"); } if (index >= _curves.Count) { throw new IndexOutOfRangeException("Curve index " + index + " is out of range (there are " + _curves.Count + " curves in the spline)"); } if (index > 0 && !VectorHelper.EqualsOrClose(_curves[index - 1].p3, curve.p0)) { throw new InvalidOperationException("The updated curve at index " + index + " does not connect with the previous curve at index " + (index - 1)); } if (index < _curves.Count - 1 && !VectorHelper.EqualsOrClose(_curves[index + 1].p0, curve.p3)) { throw new InvalidOperationException("The updated curve at index " + index + " does not connect with the next curve at index " + (index + 1)); } _curves[index] = curve; for (int i = index; i < _curves.Count; i++) { UpdateArcLengths(i); } }
/// <summary> /// Gets the tangent at a given point in the curve. /// </summary> protected VECTOR GetCenterTangent(int first, int last, int split) { 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) { 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)); } } }
[MethodImpl(MethodImplOptions.AggressiveInlining)] // originally this method wasn't be inlined #endif private static FLOAT PerpendicularDistance(VECTOR a, VECTOR b, FLOAT abDist, FLOAT aCrossB, VECTOR p) { // a profile with the test data showed that originally this was eating up ~44% of the runtime. So, this went through // several iterations of optimization and staring at the disassembly. I tried different methods of using cross // products, doing the computation with larger vector types, etc... this is the best I could do in ~45 minutes // running on 3 hours of sleep, which is all scalar math, but RyuJIT puts it into XMM registers and does // ADDSS/SUBSS/MULSS/DIVSS because that's what it likes to do whenever it sees a vector in a function. FLOAT area = Math.Abs(aCrossB + VectorHelper.GetX(b) * VectorHelper.GetY(p) + VectorHelper.GetX(p) * VectorHelper.GetY(a) - VectorHelper.GetX(p) * VectorHelper.GetY(b) - VectorHelper.GetX(a) * VectorHelper.GetY(p)); FLOAT height = area / abDist; return(height); }
private static void RdpRecursive(List <VECTOR> pts, FLOAT error, int first, int last, List <int> keepIndex) { int nPts = last - first + 1; if (nPts < 3) { return; } VECTOR a = pts[first]; VECTOR b = pts[last]; FLOAT abDist = VectorHelper.Distance(a, b); FLOAT aCrossB = VectorHelper.GetX(a) * VectorHelper.GetY(b) - VectorHelper.GetX(b) * VectorHelper.GetY(a); FLOAT maxDist = error; int split = 0; for (int i = first + 1; i < last - 1; i++) { VECTOR p = pts[i]; FLOAT pDist = PerpendicularDistance(a, b, abDist, aCrossB, p); if (pDist > maxDist) { maxDist = pDist; split = i; } } if (split != 0) { keepIndex.Add(split); RdpRecursive(pts, error, first, split, keepIndex); RdpRecursive(pts, error, split, last, keepIndex); } }
public VECTOR Binormal(FLOAT t) { return(VectorHelper.Normalize(VectorHelper.Cross(Tangent(t), Normal(t)))); }
public Vector3 Tangent(float t) { return(VectorHelper.Normalize(Derivative(t))); }