public void xRad(Vect2 uv, ref Vect3 xyz, ref double k) { Vect3 dxu = new Vect3(), dxv = new Vect3(), ddxu = new Vect3(), ddxv = new Vect3(), dduv = new Vect3(), xnor = new Vect3(); xCvt(uv, ref xyz, ref dxu, ref dxv, ref ddxu, ref ddxv, ref dduv); xNor(uv, ref xyz, ref xnor); //calculate first fundamental form double E = dxu.Norm; double F = dxu.Dot(dxv); double G = dxv.Norm; //double E = BLAS.dot(dxu, dxu); //double F = BLAS.dot(dxu, dxv); //double G = BLAS.dot(dxv, dxv); double detI = E * G - F * F; //calculate second fundamental form double e = xnor.Dot(ddxu); double f = xnor.Dot(dduv); double g = xnor.Dot(ddxv); //double e = BLAS.dot(ddxu, xnor); //double f = BLAS.dot(dduv, xnor); //double g = BLAS.dot(ddxv, xnor); double detII = e * g - f * f; k = detII / detI; }
public static bool xClosest(IMouldCurve c, ref double s, ref Vect2 uv, ref Vect3 xyzTarget, ref double dist, double tol, bool bUseGuess) { Vect3 x = new Vect3(xyzTarget); Vect3 dx = new Vect3(), ddx = new Vect3(); Vect3 h = new Vect3(); Vect2 e = new Vect2(); double deds; //try at each 18th point to ensure global solution double s0 = s; double[] sGuesses;// = new double[] { 0, .125, .25, .375, .5, .625, .75, .875, 1 }; if (bUseGuess) sGuesses = new double[] { s }; else sGuesses = new double[] { 0, .125, .25, .375, .5, .625, .75, .875, 1 }; double sCur = -1, hCur = 1e9; for (int nGuess = 0; nGuess < sGuesses.Length; nGuess++) { s = sGuesses[nGuess];//starting guess int loop = 0, max_loops = 100; while (loop++ < max_loops) { c.xCvt(s, ref uv, ref x, ref dx, ref ddx); h = x - xyzTarget; dist = h.Magnitude; e[0] = s; e[1] = h.Dot(dx); // error, dot product is 0 at pi/2 if (Math.Abs(e[1]) < tol) // error is less than the tolerance { if (dist < hCur)//store best result { sCur = s; hCur = dist; break; } //xyzTarget.Set(x);// return point to caller //return true; } deds = dx.Norm + h.Dot(ddx); deds = e[1] / deds; // calculate a new s (enforce maximum increment) deds = 0.1 > Math.Abs(deds) ? deds : 0.1 * Math.Sign(deds); s = e[0] - deds; //logger.write_format_line("%.5g\t%.5g\t%.5g\t%.5g\t%.5g\t", x[ox], x[oy], e[ox], e[oy], dist); } } if (sCur != -1) //if successful return parameters to caller { c.xVal(sCur, ref uv, ref xyzTarget); dist = hCur; s = sCur; return true; } //s = s0; return false; }
public bool xClosest(ref Vect2 uv, ref Vect3 xyzTarget, ref double dist, double tol) { Vect3 x = new Vect3(xyzTarget); Vect3 dxu = new Vect3(), dxv = new Vect3(); Vect3 ddxu = new Vect3(), ddxv = new Vect3(), dduv = new Vect3(); Vect3 h = new Vect3(); Vect2 c = new Vect2(); Vect2 res = new Vect2(); Vect2 a = new Vect2(), b = new Vect2(); double det, r; Vect2 d = new Vect2(); int loop = 0, max_loops = 150; while (loop++ < max_loops) { xCvt(uv, ref x, ref dxu, ref dxv, ref ddxu, ref ddxv, ref dduv); h = x - xyzTarget; //h = BLAS.subtract(x, xyzTarget); dist = h.Magnitude; //e[0] = s; c[0] = h.Dot(dxu);// BLAS.dot(h, dxu); // error, dot product is 0 at pi/2 c[1] = h.Dot(dxv);// BLAS.dot(h, dxv); // error, dot product is 0 at pi/2 if (Math.Abs(c[0]) < tol && Math.Abs(c[1]) < tol) // error is less than the tolerance { xyzTarget.Set(x);// return point to caller return true; } a[0] = dxu.Norm + h.Dot(ddxu); a[1] = b[0] = dxu.Dot(dxv) + h.Dot(dduv); b[1] = dxv.Norm + h.Dot(ddxv); //a[0] = BLAS.dot(dxu, dxu) + BLAS.dot(h, ddxu); //a[1] = BLAS.dot(dxu, dxv) + BLAS.dot(h, dduv); //b[0] = a[1]; //b[1] = BLAS.dot(dxv, dxv) + BLAS.dot(h, ddxv); det = a.Cross(b); //det = BLAS.cross2d(a, b); d[0] = c.Cross(b) / det; d[1] = a.Cross(c) / det; //d[0] = BLAS.cross2d(c, b) / det; //d[1] = BLAS.cross2d(a, c) / det; c[0] = 0.01 > Math.Abs(d[0]) ? 1 : 0.01 / Math.Abs(d[0]); c[1] = 0.01 > Math.Abs(d[1]) ? 1 : 0.01 / Math.Abs(d[1]); //enforce maximum increment r = Math.Min(c[0], c[1]); //increment uv by scaled residuals //uv = BLAS.subtract(uv, BLAS.scale(d, r)); uv = uv - d * r; //logger.write_format_line("%.5g\t%.5g\t%.5g\t%.5g\t%.5g\t", x[ox], x[oy], e[ox], e[oy], dist); } //s = s0; return false; }
/// <summary> /// Rotate a vector about an axis by a specified angle /// </summary> /// <param name="v">the vector to rotate</param> /// <param name="a">the axis of rotation</param> /// <param name="rad">the angle in radians to rotate</param> /// <returns>a new rotated vector</returns> public static Vect3 Rotate(Vect3 v, Vect3 a, double rad) { Debug.Assert(a.Length == v.Length); //http://inside.mines.edu/~gmurray/ArbitraryAxisRotation/ //rot = a(a*v)(1-cos) + v*cos + (aXv)sin double dot = v.Dot(a); double cos = Math.Cos(rad); double sin = Math.Sin(rad); return new Vect3( a.x * dot * (1 - cos) + v.x * cos + (v.z * a.y - v.y * a.z) * sin, a.y * dot * (1 - cos) + v.y * cos + (v.x * a.z - v.z * a.x) * sin, a.z * dot * (1 - cos) + v.z * cos + (v.y * a.x - v.x * a.y) * sin); }
public void uNor(double s, ref Vect2 uv, ref Vect2 un) { Vect2 ut = new Vect2(); Vect3 xyz = new Vect3(), dxu = new Vect3(), dxv = new Vect3(); uVec(s, ref uv, ref ut); Surface.xVec(uv, ref xyz, ref dxu, ref dxv); //covariant metric tensor components double a11 = dxu.Norm; double a12 = dxu.Dot(dxv); double a22 = dxv.Norm; double det = Math.Sqrt(a11 * a22 - a12 * a12); //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; //return unit normal u components un.Magnitude = 1; }
public virtual List<Entity> CreateEntities(bool bFitPoints, double TolAngle, out double[] sPos) { if (!AllFitPointsValid()) { sPos = null; return new List<Entity>(); } List<double> spos = new List<double>(); const int TEST = 8; int FitLength = FitPoints.Length; double[] stest = new double[(FitLength - 1) * TEST]; Vect3[] xtest = new Vect3[(FitLength - 1) * TEST]; Vect2 uv = new Vect2(); Vect3 xyz = new Vect3(); Vect3 dxp = new Vect3(), dxm = new Vect3(); //initial 8 subdivisions per segment int nFit, nTest = 0; for (nFit = 1; nFit < FitLength; nFit++) { for (int i = 0; i < TEST; i++, nTest++) { stest[nTest] = BLAS.interpolate(i, TEST, FitPoints[nFit].S, FitPoints[nFit - 1].S); xtest[nTest] = new Vect3(); xVal(stest[nTest], ref uv, ref xtest[nTest]); } } //test the midpoint of each subsegment to determine required # of points int[] nAdd = new int[stest.Length]; double cosA; double smid; int nTotal = FitLength; for (nTest = 1; nTest < stest.Length; nTest++) { //midpoint position smid = (stest[nTest] + stest[nTest - 1]) / 2.0; xVal(smid, ref uv, ref xyz); //forward and backward tangents dxp = xtest[nTest] - xyz; dxm = xtest[nTest - 1] - xyz; //change in angle between for and aft tans cosA = -(dxp.Dot(dxm)) / (dxp.Magnitude * dxm.Magnitude); Utilities.LimitRange(-1, ref cosA, 1); cosA = Math.Acos(cosA); //determine additional points and sum total nTotal += nAdd[nTest] = (int)(cosA / TolAngle + 1); } m_Length = 0; Vect3 xprev = new Vect3(); Vect3 dx = new Vect3(); List<Point3D> pnts = new List<Point3D>(); List<Vect3> tans = new List<Vect3>(); xVal(stest[0], ref uv, ref xprev); pnts.Add(new Point3D(xprev.ToArray())); spos.Add(stest[0]); for (nTest = 1; nTest < stest.Length; nTest++) { for (int i = 1; i <= nAdd[nTest]; i++) { smid = ((nAdd[nTest] - i) * stest[nTest - 1] + i * stest[nTest]) / nAdd[nTest]; xVec(smid, ref uv, ref xyz, ref dx); spos.Add(smid); pnts.Add(new Point3D(xyz.ToArray())); tans.Add(new Vect3(dx)); m_Length += xyz.Distance(xprev); xprev.Set(xyz); } } if (EXTENDENTITY) { //add for-cast/back-cast points for (int i = 0; i < 2; i++) { for (nTest = 0; nTest < 10; nTest++) { smid = BLAS.interpolant(nTest, 10) * 0.05;//scale down to .1 cast if (i == 0) smid = -smid; else smid += 1.0; xVal(smid, ref uv, ref xyz); if (i == 0) pnts.Insert(0, new Point3D(xyz.ToArray())); else pnts.Add(new Point3D(xyz.ToArray())); } } } LinearPath lp = new LinearPath(pnts); lp.EntityData = this; //lp.LineWeight = 3.0f; //lp.LineWeightMethod = colorMethodType.byEntity; List<Entity> tanpaths = new List<Entity>(); tanpaths.Add(lp); if (bFitPoints) { //LinearPath path; //int npnt = 0; //foreach (Vect3 pnt in tans) //{ // pnt.Magnitude = 2; // xyz = new Vect3(pnts[npnt].ToArray()); // xyz += pnt; // path = new LinearPath(pnts[npnt], new Point3D(xyz.ToArray())); // path.EntityData = this; // tanpaths.Add(path); // npnt++; // //xVal(pnt.UV, ref xyz); // //pnts.Add(new Point3D(xyz.ToArray())); //} pnts = new List<Point3D>(); foreach (IFitPoint pnt in FitPoints) { xVal(pnt.UV, ref xyz); pnts.Add(new Point3D(xyz.ToArray())); } PointCloud pc = new PointCloud(pnts, 8f); pc.EntityData = this; tanpaths.Add(pc); } sPos = spos.ToArray(); return tanpaths; }
/// <summary> /// inserts additional points into the geo-point arrays to ensure a smooth, linear geo /// </summary> /// <param name="g">the curve object to spline</param> /// <param name="sFits">in: the initial set of geo-points s-positions, out: an interpolated set of geo-point s-pos</param> /// <param name="uFits">in: the initial set of geo-points u-positions, out: an interpolated set of geo-point u-pos</param> static void RefineGeo(MouldCurve g, ref double[] sFits, ref Vect2[] uFits) { int NumFits = sFits.Length; List<double> sPos = new List<double>(NumFits * 10); int uAdd, xAdd, sAdd; double s, dau; Vect2 uv = new Vect2();//, um = new Vect2(), up = new Vect2(); Vect2 dmu = new Vect2(), dpu = new Vect2(); Vect3 xyz = new Vect3(), dmx = new Vect3(), dpx = new Vect3(); sPos.Add(sFits[0]); for (int i = 1; i < NumFits; i++) { s = (sFits[i - 1] + sFits[i]) / 2.0; g.xVal(s, ref uv, ref xyz); dmu = uFits[i - 1] - uv; dpu = uFits[i] - uv; dau = Math.Acos(-(dmu.Dot(dpu)) / dmu.Magnitude / dpu.Magnitude); uAdd = (int)(dau / (Math.PI / 180.0) + 1); g.xVal(uFits[i - 1], ref dmx); g.xVal(uFits[i], ref dpx); dmx = dmx - xyz; dpx = dpx - xyz; dau = Math.Acos(-(dmx.Dot(dpx)) / dmx.Magnitude / dpx.Magnitude); xAdd = (int)(dau / (Math.PI / 180.0) + 1); sAdd = Math.Max(xAdd, uAdd); for (int iS = 1; iS <= sAdd; iS++) { s = BLAS.interpolate((double)iS / (double)sAdd, sFits[i], sFits[i - 1]); sPos.Add(s);//add evenly spaced s points } } uFits = new Vect2[sPos.Count]; sFits = new double[sPos.Count]; int iA = 0; foreach (double sA in sPos) { uFits[iA] = new Vect2(); g.uVal(sA, ref uFits[iA]); sFits[iA] = sA; iA++; } }
/// <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; }