public static void ComputeMeanCovariance(IList<Vector2> pts, out Vector2 mean, out Matrix2 cov)
{
// for better numerical stability, use a two-pass method where we first compute the mean
double sum_x = 0, sum_y = 0;
for (int i = 0; i < pts.Count; i++) {
sum_x += pts[i].X;
sum_y += pts[i].Y;
}
double avg_x = sum_x/(double)pts.Count;
double avg_y = sum_y/(double)pts.Count;
// now compute variances and covariances
double sum_x2 = 0, sum_y2 = 0, sum_xy = 0;
for (int i = 0; i < pts.Count; i++) {
double dx = pts[i].X - avg_x;
double dy = pts[i].Y - avg_y;
sum_x2 += dx*dx;
sum_y2 += dy*dy;
sum_xy += dx*dy;
}
double var_x = sum_x2/(double)pts.Count;
double var_y = sum_y2/(double)pts.Count;
double cov_xy = sum_xy/(double)pts.Count;
mean = new Vector2(avg_x, avg_y);
cov = new Matrix2(var_x, cov_xy, cov_xy, var_y);
}
public Matrix4 Inverse()
{
Matrix2 invA = SubmatrixA().Inverse();
Matrix2 B = SubmatrixB();
Matrix2 C = SubmatrixC();
Matrix2 invSchurComp = (SubmatrixD() - C * invA * B).Inverse();
return(Matrix4.FromSubmatrix(
invA + invA * B * invSchurComp * C * invA, -invA * B * invSchurComp,
-invSchurComp * C * invA, invSchurComp));
}
public static Matrix4 FromSubmatrix(
Matrix2 A, Matrix2 B,
Matrix2 C, Matrix2 D)
{
Matrix4 ret = new Matrix4();
for (int i = 0; i < 2; i++)
{
for (int j = 0; j < 2; j++)
{
ret[i, j] = A[i, j];
ret[i + 2, j] = C[i, j];
ret[i, j + 2] = B[i, j];
ret[i + 2, j + 2] = D[i, j];
}
}
return(ret);
}
public bool Intersect(Line l, out Vector2 pt, out Vector2 K)
{
Vector2 P = P1 - P0;
Vector2 S = l.P1 - l.P0;
double cross = S.Cross(P);
if (Math.Abs(cross) < 1e-10) {
pt = default(Vector2);
K = default(Vector2);
return false;
}
Matrix2 A = new Matrix2(-S.Y, S.X, -P.Y, P.X);
K = A*(l.P0 - P0)/cross;
if (K.X >= 0 && K.X <= 1) {
pt = P0 + P*K.X;
return true;
}
else {
pt = default(Vector2);
return false;
}
}
public bool Intersect(LineSegment l, out Vector2 pt, out Vector2 K)
{
Vector2 P = P1 - P0;
Vector2 S = l.P1 - l.P0;
Matrix2 A = new Matrix2(P.X, -S.X, P.Y, -S.Y);
if (Math.Abs(A.Determinant()) < 1e-10) {
pt = default(Vector2);
K = default(Vector2);
return false;
}
K = A.Inverse()*(l.P0 - P0);
if (K.X >= 0 && K.X <= 1 && K.Y >= 0 && K.Y <= 1) {
pt = P0 + P*K.X;
return true;
}
else {
pt = default(Vector2);
return false;
}
}
public HRIPDF(Vector2 mean, Matrix2 sigma)
{
this.mean = mean;
this.sigma = sigma;
}
public static Matrix4 FromSubmatrix(
Matrix2 A, Matrix2 B,
Matrix2 C, Matrix2 D)
{
Matrix4 ret = new Matrix4();
for (int i = 0; i < 2; i++)
{
for (int j = 0; j < 2; j++)
{
ret[i, j] = A[i, j];
ret[i + 2, j] = C[i, j];
ret[i, j + 2] = B[i, j];
ret[i + 2, j + 2] = D[i, j];
}
}
return ret;
}
