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