/// <summary>
/// Compute the Thin Plate Spline of the image, return a 2D tab
/// </summary>
/// <param name="control_points">Control points </param>
/// <param name="input">Input image to get the dim xy</param>
public double[,] calc_tps(List<cPoint3D> control_points, cDRC_Region AssociatedRegion, double Regularization)
{
int p = control_points.Count;
if (p < 3) return null;
double[,] grid = new double[AssociatedRegion.SizeX, AssociatedRegion.SizeY];
Matrix mtx_l = new Matrix(p + 3, p + 3);
Matrix mtx_v = new Matrix(p + 3, 1);
Matrix mtx_orig_k = new Matrix(p, p);
double a = 0.0;
for (int i = 0; i < p; ++i)
{
for (int j = i + 1; j < p; ++j)
{
cPoint3D pt_i = new cPoint3D(control_points[i].X, control_points[i].Y, control_points[i].Z);
cPoint3D pt_j = new cPoint3D(control_points[j].X, control_points[j].Y, control_points[j].Z);
pt_i.Y = pt_j.Y = 0;
//double elen = Math.Sqrt((pt_i.X - pt_j.X) * (pt_i.X - pt_j.X) + (pt_i.Z - pt_j.Z) * (pt_i.Z - pt_j.Z));
double elen = pt_i.DistTo(pt_j);
mtx_l[i, j] = mtx_l[j, i] = mtx_orig_k[i, j] = mtx_orig_k[j, i] = tps_base_func(elen);
a += elen * 2; // same for upper & lower tri
}
}
a /= (double)(p * p);
//regularization = 0.3f;
//Fill the rest of L
for (int i = 0; i < p; ++i)
{
//diagonal: reqularization parameters (lambda * a^2)
mtx_l[i, i] = mtx_orig_k[i, i] = Regularization * (a * a);
// P (p x 3, upper right)
mtx_l[i, p + 0] = 1.0;
mtx_l[i, p + 1] = control_points[i].X;
mtx_l[i, p + 2] = control_points[i].Z;
// P transposed (3 x p, bottom left)
mtx_l[p + 0, i] = 1.0;
mtx_l[p + 1, i] = control_points[i].X;
mtx_l[p + 2, i] = control_points[i].Z;
}
// O (3 x 3, lower right)
for (int i = p; i < p + 3; ++i)
for (int j = p; j < p + 3; ++j)
mtx_l[i, j] = 0.0;
// Fill the right hand vector V
for (int i = 0; i < p; ++i)
mtx_v[i, 0] = control_points[i].Y;
mtx_v[p + 0, 0] = mtx_v[p + 1, 0] = mtx_v[p + 2, 0] = 0.0;
// Solve the linear system "inplace"
Matrix mtx_v_res = new Matrix(p + 3, 1);
LuDecomposition ty = new LuDecomposition(mtx_l);
mtx_v_res = ty.Solve(mtx_v);
if (mtx_v_res == null)
{
return null;
}
// Interpolate grid heights
for (int x = 0; x < AssociatedRegion.SizeX; ++x)
{
for (int z = 0; z < AssociatedRegion.SizeY; ++z)
{
//float x = 0f; float z = 0.5f;
double h = mtx_v_res[p + 0, 0] + mtx_v_res[p + 1, 0] * (float)x / (float)AssociatedRegion.SizeX + mtx_v_res[p + 2, 0] * (float)z / (float)AssociatedRegion.SizeY;
//double h = mtx_v[p + 0, 0] + mtx_v[p + 1, 0] * (float)x + mtx_v[p + 2, 0] * (float)z ;
cPoint3D pt_ia;
cPoint3D pt_cur = new cPoint3D((float)x / (float)AssociatedRegion.SizeX, 0, (float)z / (float)AssociatedRegion.SizeY);
//Vector3 pt_cur = new Vector3((float)x , 0, (float)z);
for (int i = 0; i < p; ++i)
{
pt_ia = control_points[i];
pt_ia.Y = 0;
h += mtx_v_res[i, 0] * tps_base_func(pt_ia.DistTo(pt_cur));
}
grid[x, z] = h;
}
}
// Calc bending energy
Matrix w = new Matrix(p, 1);
for (int i = 0; i < p; ++i)
w[i, 0] = mtx_v_res[i, 0];
Matrix be;
be = Matrix.Multiply(Matrix.Multiply(w.Transpose(), mtx_orig_k), w);
bending_energy = be[0, 0];
Console.WriteLine("be= " + be[0, 0]);
return grid;
}
public cFeedBackMessage Run(c3DNewWorld _3DWorld)
{
if (this.Input == null)
{
FeedBackMessage.IsSucceed = false;
FeedBackMessage.Message = "No input data table defined.";
return FeedBackMessage;
}
ListObjects = new cListGeometric3DObject("Axis MetaObject");
cExtendedList ListLenghts = new cExtendedList();
double MinLenght;
cPoint3D Pt1 = null;
cPoint3D Pt2 = null;
cPoint3D Pt3 = null;
if (this.Input.Count == 1)
{
cPoint3D Pt0 = new cPoint3D(this.Input[0][0], 0, 0);
Pt1 = new cPoint3D(this.Input[0][1], 0, 0);
ListLenghts.Add(Pt0.DistTo(Pt1));
MinLenght = ListLenghts.Min();
if (MinLenght == 0) MinLenght = 1;
c3DLine XAxis = new c3DLine(Pt0, Pt1);
XAxis.Tag = this.Input[0].Tag;
XAxis.SetName("Axis - " + this.Input[0].Name);
ListObjects.AddObject(XAxis);
}
else if (this.Input.Count == 2)
{
cPoint3D Pt0 = new cPoint3D(this.Input[0][0], this.Input[1][0], 0);
Pt1 = new cPoint3D(this.Input[0][1], this.Input[1][0], 0);
ListLenghts.Add(Pt0.DistTo(Pt1));
Pt2 = new cPoint3D(this.Input[0][0], this.Input[1][1], 0);
ListLenghts.Add(Pt0.DistTo(Pt2));
MinLenght = ListLenghts.Min();
if (MinLenght == 0) MinLenght = 1;
c3DLine XAxis = new c3DLine(Pt0, Pt1);
XAxis.Tag = this.Input[0].Tag;
XAxis.SetName("Axis - " + this.Input[0].Name);
ListObjects.AddObject(XAxis);
c3DLine YAxis = new c3DLine(Pt0, Pt2);
YAxis.Tag = this.Input[1].Tag;
YAxis.SetName("Axis - " + this.Input[1].Name);
ListObjects.AddObject(YAxis);
}
else
{
cPoint3D Pt0 = new cPoint3D(this.Input[0][0], this.Input[1][0], this.Input[2][0]);
Pt1 = new cPoint3D(this.Input[0][1], this.Input[1][0], this.Input[2][0]);
ListLenghts.Add(Pt0.DistTo(Pt1));
Pt2 = new cPoint3D(this.Input[0][0], this.Input[1][1], this.Input[2][0]);
ListLenghts.Add(Pt0.DistTo(Pt2));
Pt3 = new cPoint3D(this.Input[0][0], this.Input[1][0], this.Input[2][1]);
ListLenghts.Add(Pt0.DistTo(Pt3));
MinLenght = ListLenghts.Min();
if (MinLenght == 0) MinLenght = 1;
c3DLine XAxis = new c3DLine(Pt0, Pt1,Color.Black);
XAxis.Tag = this.Input[0].Tag;
XAxis.SetName("Axis - " + this.Input[0].Name);
ListObjects.AddObject(XAxis);
c3DLine YAxis = new c3DLine(Pt0, Pt2,Color.Black);
YAxis.Tag = this.Input[1].Tag;
YAxis.SetName("Axis - " + this.Input[1].Name);
ListObjects.AddObject(YAxis);
c3DLine ZAxis = new c3DLine(Pt0, Pt3,Color.Black);
ZAxis.Tag = this.Input[2].Tag;
ZAxis.SetName("Axis - " + this.Input[2].Name);
ListObjects.AddObject(ZAxis);
}
#region draw text
double TextScale = MinLenght;
if (MinLenght == 1) TextScale = 1;
cPoint3D PosText = Pt1;
// display the axis name
c3DText TmpText = new c3DText(_3DWorld, this.Input[0].Name, PosText, Color.Black, 0.02 * TextScale);
TmpText.Tag = this.Input[0].Tag;
ListObjects.AddObject(TmpText);
// MinX
PosText = new cPoint3D(0, 0, 0);
PosText.X = this.Input[0][0];
if (this.Input.Count > 1) PosText.Y = this.Input[1][0];
if (this.Input.Count > 2) PosText.Z = this.Input[2][0] - TextScale * 0.03;
ListObjects.AddObject(new c3DText(_3DWorld, this.Input[0][0].ToString("N3"), PosText, Color.DimGray, 0.01 * TextScale));
// MaxX
PosText.X = this.Input[0][1];
if (this.Input.Count > 1) PosText.Y = this.Input[1][0];
if (this.Input.Count > 2) PosText.Z = this.Input[2][0] - TextScale * 0.03;
if (this.Input.Count > 1) ListObjects.AddObject(new c3DText(_3DWorld, this.Input[0][1].ToString("N3"), PosText, Color.DimGray, 0.01 * TextScale));
if (Pt2 != null)
{
PosText = Pt2;
TmpText = new c3DText(_3DWorld, this.Input[1].Name, PosText, Color.Black, 0.02 * TextScale);
TmpText.Tag = this.Input[1].Tag;
ListObjects.AddObject(TmpText);
// MinY
PosText.X = this.Input[0][0] - TextScale * 0.03;// + TextScale*0.02;
if (this.Input.Count > 1) PosText.Y = this.Input[1][0];// -TextScale * 0.03;
if (this.Input.Count > 2) PosText.Z = this.Input[2][0] - TextScale * 0.03;
ListObjects.AddObject(new c3DText(_3DWorld, this.Input[1][0].ToString("N3"), PosText, Color.DimGray, 0.01 * TextScale));
PosText.X = this.Input[0][0] - TextScale * 0.03;// + TextScale*0.02;
if (this.Input.Count > 1) PosText.Y = this.Input[1][1];// -TextScale * 0.03;
if (this.Input.Count > 2) PosText.Z = this.Input[2][0] - TextScale * 0.03;
ListObjects.AddObject(new c3DText(_3DWorld, this.Input[1][1].ToString("N3"), PosText, Color.DimGray, 0.01 * TextScale));
}
if (Pt3 != null)
{
PosText = Pt3;
TmpText = new c3DText(_3DWorld, this.Input[2].Name, /*new cPoint3D(-0.04, -0.04, 0.45)*/ PosText, Color.Black, 0.02 * TextScale);
TmpText.Tag = this.Input[2].Tag;
ListObjects.AddObject(TmpText);
PosText.X = this.Input[0][0] - TextScale * 0.03;// + TextScale*0.02;
PosText.Y = this.Input[1][0];// -TextScale * 0.03;
PosText.Z = this.Input[2][0];
ListObjects.AddObject(new c3DText(_3DWorld, this.Input[2][0].ToString("N3"), PosText, Color.DimGray, 0.01 * TextScale));
PosText.X = this.Input[0][0] - TextScale * 0.03;// + TextScale*0.02;
PosText.Y = this.Input[1][0];// -TextScale * 0.03;
PosText.Z = this.Input[2][1];
ListObjects.AddObject(new c3DText(_3DWorld, this.Input[2][1].ToString("N3"), PosText, Color.DimGray, 0.01 * TextScale));
}
#endregion
return base.FeedBackMessage;
}
