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;
}
