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