/// <summary> /// using the start, end and passed geo fit points will slide the points along their normals to achieve a geodesic /// </summary> /// <param name="g">the curve object to spline</param> /// <param name="start">the starting point of this segment</param> /// <param name="end">the ending point of this segment</param> /// <param name="sFits">the s-positons of each geo-point</param> /// <param name="uFits">the u-positons of each geo-point</param> /// <returns>true if successful, false otherwise</returns> static bool FitGeo(MouldCurve g, IFitPoint start, IFitPoint end, double[] sFits, Vect2[] uFits) { int NumFits = sFits.Length; int INC = NumFits - 1; int i; Vect2[] uNor = new Vect2[NumFits]; Vect3 xyz = new Vect3(), dxu = new Vect3(), dxv = new Vect3(); Vect2 ut = new Vect2(), un = new Vect2(); double a11, a12, a22, det; //calculate insurface normals at each fitpoint uNor[0] = new Vect2();//fixed endpoint doesnt need normal for (i = 1; i < INC; i++) { g.Surface.xVec(uFits[i], ref xyz, ref dxu, ref dxv); //covariant metric tensor components a11 = dxu.Norm; a12 = dxu.Dot(dxv); a22 = dxv.Norm; det = Math.Sqrt(a11 * a22 - a12 * a12); //tangent(secant) vector u components ut = uFits[i + 1] - uFits[i - 1]; //contravariant normal vector components in the surface plane un[0] = (a12 * ut[0] + a22 * ut[1]) / det; un[1] = -(a11 * ut[0] + a12 * ut[1]) / det; //store unit normal u components un.Magnitude = 1; uNor[i] = new Vect2(un); } uNor[INC] = new Vect2();//fixed endpoint doesnt need normal Vect3 xPrev = new Vect3(); Vect3 ddxu = new Vect3(), ddxv = new Vect3(), dduv = new Vect3(); Vect3[] xNor = new Vect3[NumFits]; Vect3[] dxNor = new Vect3[NumFits]; Vect3[] xTan = new Vect3[NumFits]; double[] xLen = new double[NumFits]; bool Conver = false; int nNwt; Vector x; DenseVector sNor; for (nNwt = 0; nNwt < 50; nNwt++) { xNor[0] = new Vect3(); //update startpoint slide position if (start is SlidePoint) { dxNor[0] = new Vect3(); (start as SlidePoint).Curve.xCvt((start as SlidePoint).SCurve, ref uFits[0], ref xPrev, ref xNor[0], ref dxNor[0]); //uFits[0][0] = FitPoints[0][1]; //uFits[0][1] = FitPoints[0][2]; } else g.xVal(uFits[0], ref xPrev); //update endpoint slide position if (end is SlidePoint) { uFits[INC][0] = end[1]; uFits[INC][1] = end[2]; } xLen[0] = 0; //calc internal point vectors for (i = 1; i < NumFits; i++) { g.Surface.xCvt(uFits[i], ref xyz, ref dxu, ref dxv, ref ddxu, ref ddxv, ref dduv); a11 = uNor[i][0] * uNor[i][0]; a12 = uNor[i][0] * uNor[i][1] * 2; a22 = uNor[i][1] * uNor[i][1]; // insurface normal x components dxu.Scale(uNor[i][0]); dxv.Scale(uNor[i][1]); xNor[i] = dxu + dxv; //insurface normal x derivatives ddxu.Scale(a11); dduv.Scale(a12); ddxv.Scale(a22); dxNor[i] = ddxu + dduv + ddxv; //forward facing tangent vector xTan[i] = xyz - xPrev; xPrev.Set(xyz); xLen[i] = xTan[i].Magnitude;//segment length xLen[0] += xLen[i];//accumulate total length xTan[i].Magnitude = 1;//unit tangent vector } //update endpoint slide position if (end is SlidePoint) { (end as SlidePoint).Curve.xCvt((end as SlidePoint).SCurve, ref uFits[INC], ref xyz, ref xNor[INC], ref dxNor[INC]); } DenseMatrix A = new DenseMatrix(NumFits); sNor = new DenseVector(NumFits); double p0, pp, d, d0, dp, gm, g0, gp; int ix; //slide startpoint if (start is SlidePoint) { //mid point normal vector dotted with end point tangent vectors pp = xNor[0].Dot(xTan[1]); for (g0 = gp = 0, ix = 0; ix < 3; ix++) { //midpoint tangent and curavture variantion dp = (xNor[0][ix] - pp * xTan[1][ix]) / xLen[1]; //mid and top point gradients g0 += -dp * xNor[0][ix] + xTan[1][ix] * dxNor[0][ix]; gp += dp * xNor[1][ix]; } //geodesic residual and gradients A[0, 0] = g0; A[0, 1] = gp; sNor[0] = pp; } else//fixed start point { A[0, 0] = 1; sNor[0] = 0; } for (i = 1; i < INC; i++)//internal points { //midpoint normal dotted with tangents p0 = xNor[i].Dot(xTan[i]);// BLAS.dot(xNor[i], xTan[i]); pp = xNor[i].Dot(xTan[i + 1]);//BLAS.dot(xNor[i], xTan[i + 1]); for (gm = g0 = gp = 0, ix = 0; ix < 3; ix++) { //midpoint curvature vector d = xTan[i + 1][ix] - xTan[i][ix]; //midpoint tangent and curavture variantion d0 = (xNor[i][ix] - p0 * xTan[i][ix]) / xLen[i]; dp = (xNor[i][ix] - pp * xTan[i + 1][ix]) / xLen[i + 1]; //bottom, mid and top point gradients gm += d0 * xNor[i - 1][ix]; g0 += (-d0 - dp) * xNor[i][ix] + d * dxNor[i][ix]; gp += dp * xNor[i + 1][ix]; } A[i, i - 1] = gm; A[i, i] = g0; A[i, i + 1] = gp; sNor[i] = -p0 + pp; } if (end is SlidePoint)//slide endpoint { p0 = xNor[i].Dot(xTan[i]); for (gm = g0 = 0, ix = 0; ix < 3; ix++) { //midpoint tangent and curavture variantion d0 = (xNor[i][ix] - p0 * xTan[i][ix]) / xLen[i]; //bottom, mid and top point gradients gm += d0 * xNor[i - 1][ix]; g0 += -d0 * xNor[i][ix] - xTan[i][ix] * dxNor[i][ix]; } A[i, i - 1] = gm; A[i, i] = g0; sNor[i] = -p0; } else//fixed endpoint { A[i, i] = 1; sNor[i] = 0; } LU decomp = A.LU(); x = (Vector)decomp.Solve(sNor); double Reduce = Math.Min(1, .05 / x.AbsoluteMaximum()); if( start is SlidePoint) (start as SlidePoint).SCurve -= x[0] * Reduce; if( end is SlidePoint) (end as SlidePoint).SCurve -= x[INC] * Reduce; for (i = 1; i < NumFits; i++)//increment uv points { uFits[i][0] -= x[i] * uNor[i][0] * Reduce; uFits[i][1] -= x[i] * uNor[i][1] * Reduce; } if (nNwt < 5) { //keep initial (s)-increments within bounds if (start is SlidePoint) (start as SlidePoint).SCurve = Utilities.LimitRange(0, (start as SlidePoint).SCurve, 1); if (end is SlidePoint) (end as SlidePoint).SCurve = Utilities.LimitRange(0, (end as SlidePoint).SCurve, 1); // keep initial (u)-increments within bounds for (i = 1; i < NumFits - 1; i++) { uFits[i][0] = Utilities.LimitRange(0, uFits[i][0], 1); uFits[i][1] = Utilities.LimitRange(-.125, uFits[i][1], 1.125); } } double xmax = x.AbsoluteMaximum(); double smax = sNor.AbsoluteMaximum(); if (Conver = (x.AbsoluteMaximum() < 1e-8 && sNor.AbsoluteMaximum() < 1e-7)) break; } if (!Conver) return false; g.Length = xLen[0];//store length //calculate unit length (s)-parameter values sFits[0] = 0; for (i = 1; i < NumFits; i++) sFits[i] = sFits[i - 1] + xLen[i] / xLen[0]; //g.m_uvs = uFits; g.ReSpline(sFits, uFits); return true; }