public Matrix PseudoInverse()
{
var sVD = new SVD(this);
Matrix matrix = new SquareMatrix(Columns);
for (var i = 0; i < sVD.W.Rows; i++)
{
if (sVD.W[i] > 1E-05)
{
matrix[i, i] = 1.0/sVD.W[i];
}
}
return sVD.V*matrix*sVD.U.Transpose();
}
public Plane3D FitPlaneToPoints(Collection<Point3D> points)
{
if (points == null)
{
throw new NullReferenceException();
}
if (points.Count < 3)
{
throw new MatrixException("Not enough points to fit a plane");
}
var point3D = Centroid(points);
var num = 0.0;
var num2 = 0.0;
var num3 = 0.0;
var num4 = 0.0;
var num5 = 0.0;
var num6 = 0.0;
foreach (var current in points)
{
var num7 = current.X - point3D.X;
var num8 = current.Y - point3D.Y;
var num9 = current.Z - point3D.Z;
num += num7*num7;
num2 += num8*num8;
num3 += num9*num9;
num4 += num7*num8;
num5 += num8*num9;
num6 += num7*num9;
}
var mat = new SquareMatrix(3, new[]
{
num,
num4,
num6,
num4,
num2,
num5,
num6,
num5,
num3
});
var sVD = new SVD(mat);
var normal = new Vector3D(sVD.U.GetColumn(sVD.SmallestSingularIndex));
var plane3D = new Plane3D(point3D, normal);
CalculateErrors(points, plane3D);
return plane3D;
}
public double ConditionNumber()
{
var sVD = new SVD(this);
return sVD.ConditionNumber;
}
public Line3D FitLineToPoints(Collection<Point3D> points)
{
if (points == null)
{
throw new MatrixNullReference();
}
var point3D = Centroid(points);
var num = 0.0;
var num2 = 0.0;
var num3 = 0.0;
var num4 = 0.0;
var num5 = 0.0;
var num6 = 0.0;
foreach (var current in points)
{
var num7 = current.X - point3D.X;
var num8 = current.Y - point3D.Y;
var num9 = current.Z - point3D.Z;
num += num7*num7;
num2 += num8*num8;
num3 += num9*num9;
num4 += num7*num8;
num5 += num8*num9;
num6 += num7*num9;
}
var mat = new SquareMatrix(3, new[]
{
num2 + num3,
-num4,
-num6,
-num4,
num3 + num,
-num5,
-num6,
-num5,
num + num2
});
var sVD = new SVD(mat);
var direction = new Vector3D(sVD.U.GetColumn(sVD.SmallestSingularIndex));
var line3D = new Line3D(point3D, direction);
CalculateErrors(points, line3D);
return line3D;
}
