private double CalculateError(int i0, int i1, out Vector3d result)
{
// compute interpolated vertex
var vertices = this.vertices.Data;
Vertex v0 = vertices[i0];
Vertex v1 = vertices[i1];
SymmetricMatrix q = v0.q + v1.q;
bool border = (v0.border & v1.border);
double error = 0.0;
double det = q.Determinant1();
if (det != 0.0 && !border)
{
// q_delta is invertible
result = new Vector3d(
-1.0 / det * q.Determinant2(), // vx = A41/det(q_delta)
1.0 / det * q.Determinant3(), // vy = A42/det(q_delta)
-1.0 / det * q.Determinant4()); // vz = A43/det(q_delta)
error = VertexError(ref q, result.x, result.y, result.z);
}
else
{
// det = 0 -> try to find best result
Vector3d p1 = v0.p;
Vector3d p2 = v1.p;
Vector3d p3 = (p1 + p2) * 0.5f;
double error1 = VertexError(ref q, p1.x, p1.y, p1.z);
double error2 = VertexError(ref q, p2.x, p2.y, p2.z);
double error3 = VertexError(ref q, p3.x, p3.y, p3.z);
error = MathHelper.Min(error1, error2, error3);
if (error == error3)
{
result = p3;
}
else if (error == error2)
{
result = p2;
}
else if (error == error1)
{
result = p1;
}
else
{
result = p3;
}
}
return(error);
}
// Error for one edge
private double CalculateError(int id_v1, int id_v2, ref Vector3d result, ref Vector3d vbuff, ref SymmetricMatrix smbuff)
{
// compute interpolated vertex
SymmetricMatrix.Add(ref vertices.Data[id_v1].q, ref vertices.Data[id_v2].q, ref smbuff);
double error = 0;
double det = smbuff.Determinant1();
if (Math.Abs(det) > 1.0e-10)
{
// q_delta is invertible
result.X = -1.0 / det * smbuff.Determinant2(); // vx = A41/det(q_delta)
result.Y = +1.0 / det * smbuff.Determinant3(); // vy = A42/det(q_delta)
result.Z = -1.0 / det * smbuff.Determinant4(); // vz = A43/det(q_delta)
error = VertexError(ref smbuff, ref result);
}
else
{
// det = 0 -> try to find best result
Vector3d p1 = vertices.Data[id_v1].p;
Vector3d p2 = vertices.Data[id_v2].p;
Vector3d.Average(ref p1, ref p2, ref vbuff);
double error1 = VertexError(ref smbuff, ref p1);
double error2 = VertexError(ref smbuff, ref p2);
double error3 = VertexError(ref smbuff, ref vbuff);
error = Math.Min(error1, Math.Min(error2, error3));
if (error1 == error)
{
result = p1;
}
else if (error2 == error)
{
result = p2;
}
else
{
result = vbuff;
vbuff = new Vector3d();
}
}
return(error);
}