public static decimal incl(decimal[] p) { decimal incl; /*if (90 - 360 / (2 * MathDecimal.PI) * * MathDecimal.ACos(p[2]/ MathDecimal.Norm2(p)) > 60) * { * incl = 90 - 360 / (2 * MathDecimal.PI) * * MathDecimal.ASin(MathDecimal.Sqrt((p[0] * p[0] + p[1] * p[1]) * / MathDecimal.Norm2(p))); * } * else * { * incl = 90 - 360 / (2 * MathDecimal.PI) * * MathDecimal.ACos(p[2] * / MathDecimal.Norm2(p)); * }*/ if (p[2] == 0) { incl = 0; } else { incl = 90 - 360 / (2 * MathDecimal.PI) * MathDecimal.ACos(p[2] / MathDecimal.Norm2(p)); } return(incl); }
public static decimal[] GaussNewton(Func <decimal[], decimal[, ]> J, Func <decimal[], decimal[]> r, decimal[] p0) { decimal[] p = p0; decimal a = 1; int iter = 0; while (iter < 100) //TODO: while error is large { decimal[] pAdd = LinearLeastSquares(J(p), MathDecimal.Negative(r(p))); int innerIter = 0; a = 1; while (MathDecimal.Pow2(MathDecimal.Norm2(r(p))) - MathDecimal.Pow2(MathDecimal.Norm2(MathDecimal.Sum(p, MathDecimal.Prod(a, pAdd)))) < 1M / 2M * a * MathDecimal.Pow2(MathDecimal.Norm2(MathDecimal.Prod(J(p), p))) && innerIter < 50) { a /= 2; innerIter++; } int s = 0; p = MathDecimal.Sum(p, MathDecimal.Prod(a, pAdd)); iter++; } return(p); }