/// <summary>
/// 給定STL，量測點及量測方向
/// 計算 從量測點出發、延量測方向距離最近 的位置
/// 若與STL所有三角面都無相交則回傳 null
/// </summary>
/// <param name="aSTL">STL</param>
/// <param name="P">量測點</param>
/// <param name="N">量測方向</param>
/// <returns>最近點</returns>
public override void StartMeasure()
{
if (this.IsBusy == false)
{
//設定狀態
this.FinishedThreadNumber = 0;
this.IsBusy = true;
this.StartTime = DateTime.Now;
//劃分執行緒工作區域
List<int> sliceIndex = new List<int>();
for (int i = 0; i <= this.ThreadNumber; i++)
{
sliceIndex.Add((int)((double)i * (double)MeasurePoints.Count / (double)this.ThreadNumber));
}
//建立量測物件並指定量測範圍
STLMeasurent_Objects = new List<STLMeasuringTool>();
for (int i = 0; i < sliceIndex.Count - 1; i++)
{
PointD[] PartoalPoints = new PointD[sliceIndex[i + 1] - sliceIndex[i]];
for (int j = 0; j < sliceIndex[i + 1] - sliceIndex[i]; j++)
PartoalPoints[j] = this.MeasurePoints[j + sliceIndex[i]];
STLMeasuringTool aSTLMeasurementObject = (STLMeasuringTool)Activator.CreateInstance(this.STLMeasurentReferenceObject.GetType());
aSTLMeasurementObject.STL2Measure = this.STL2Measure;
aSTLMeasurementObject.MeasurePoints = PartoalPoints.ToList();
aSTLMeasurementObject.PercentageChanged += this.OnThreadPercentageChanged;
aSTLMeasurementObject.OnFinish += this.OnThreadFinished;
if (aSTLMeasurementObject is ISTLMeasurement_NeedNormals && this.MeasurePointNormals != null)
{
Vector[] PartoalNormals = new Vector[sliceIndex[i + 1] - sliceIndex[i]];
for (int j = 0; j < sliceIndex[i + 1] - sliceIndex[i]; j++)
PartoalNormals[j] = this.MeasurePointNormals[j + sliceIndex[i]];
(aSTLMeasurementObject as ISTLMeasurement_NeedNormals).MeasurePointNormals = PartoalNormals.ToList();
}
STLMeasurent_Objects.Add(aSTLMeasurementObject);
}
foreach (var aThreadObject in STLMeasurent_Objects)
{
aThreadObject.PercentageChanged += OnThreadPercentageChanged;
aThreadObject.StartMeasure();
}
}
}
protected override void Solve()
{
//提取所有拓樸點
PointD[] allTopoPoints = new PointD[this.TopoFace1.Points.Length + this.TopoFace2.Points.Length];
Vector[] allTopoPointNormals = new Vector[this.TopoFace1.Normals.Length + this.TopoFace2.Normals.Length];
int index;
index = 0;
foreach (var item in this.TopoFace1.Points)
{
allTopoPoints[index] = item;
index++;
}
foreach (var item in this.TopoFace2.Points)
{
allTopoPoints[index] = item;
index++;
}
index = 0;
foreach (var item in this.TopoFace1.Normals)
{
allTopoPointNormals[index] = item;
index++;
}
foreach (var item in this.TopoFace2.Normals)
{
allTopoPointNormals[index] = item;
index++;
}
//劃分計算範圍
List<int> sliceIndex = new List<int>();
for (int i = 0; i < this.ThreadNumber + 1; i++)
sliceIndex.Add((int)(((double)i * (double)allTopoPoints.Length) / (double)this.ThreadNumber));
//建立獨立執行序物件
for (int i = 0; i < this.ThreadNumber; i++)
{
//取出計算範圍內的點
PointD[] PartoalPoints = new PointD[sliceIndex[i + 1] - sliceIndex[i]];
for (int j = 0; j < sliceIndex[i + 1] - sliceIndex[i]; j++)
PartoalPoints[j] = allTopoPoints[j + sliceIndex[i]];
Vector[] PartoalNormals = new Vector[sliceIndex[i + 1] - sliceIndex[i]];
for (int j = 0; j < sliceIndex[i + 1] - sliceIndex[i]; j++)
PartoalNormals[j] = allTopoPointNormals[j + sliceIndex[i]];
//建立物件
STLMeasuringTool_CMM aSTLMeasuringTool_CMM = new STLMeasuringTool_CMM()
{
STL2Measure = this.STL2Measure,
STLCubicClassifier = this.STLCubicClassifier,
MeasurePoints = PartoalPoints.ToList(),
MeasurePointNormals = PartoalNormals.ToList(),
};
aSTLMeasuringTool_CMM.PercentageChanged += this.OnThreadPercentageChanged;
aSTLMeasuringTool_CMM.OnFinish += this.OnThreadFinished;
STLMeasurent_Objects.Add(aSTLMeasuringTool_CMM);
}
this.StartTime = DateTime.Now;
foreach (var aThreadObject in STLMeasurent_Objects)
{
aThreadObject.PercentageChanged += OnThreadPercentageChanged;
aThreadObject.StartMeasure();
}
}
/// <summary>
/// 模擬P40量測類別的建構式
/// </summary>
/// <param name="STL2Measure">欲量測的STL</param>
/// <param name="TopoFace1">第一拓樸面</param>
/// <param name="TopoFace2">第二拓樸面</param>
/// <param name="PitchPoint1">第一拓樸面的PitchPoint</param>
/// <param name="PitchPoint2">第二拓樸面的PitchPoint</param>
/// <param name="ThreadNumber">執行緒數目</param>
/// <param name="OnFinish">計算完成時的後續動作</param>
public STLMeasuringTool_P40_SingleTooth(STL STL2Measure, TopoFace TopoFace1, TopoFace TopoFace2, PointD PitchPoint1, PointD PitchPoint2, int ThreadNumber, EventHandler OnFinish)
{
this.STL2Measure = STL2Measure;
this.TopoFace1 = TopoFace1;
this.TopoFace2 = TopoFace2;
this.PitchPoint1 = PitchPoint1;
this.PitchPoint2 = PitchPoint2;
this.ThreadNumber = ThreadNumber;
this.OnFinish += OnFinish;
}
public HashSet<Entity> GetEntities2(PointD position, Vector direction)
{
HashSet<CubicSpace> IntersectedCubicSpace = GetCubicSpaces2(position, direction);
HashSet<Entity> Entities = new HashSet<Entity>();
foreach (var cubicSpace in IntersectedCubicSpace)
{
foreach (var entity in cubicSpace.Entities)
{
Entities.Add(entity);
}
}
return Entities;
}
protected override void Solve()
{
if (this.IsBusy == false)
{
///進度報告
this.IsBusy = true;
this.Percentage = 0;
this.StartTime = DateTime.Now;
this.TouchPoints = new List<PointD>();
this.TouchedTriangles = new List<Triangle>();
Entity[] AllTriangle = STL2Measure.Entities.Values.ToArray();
for (int pointsIndex = 0; pointsIndex < MeasurePoints.Count; pointsIndex++)
{
//Console.WriteLine(pointsIndex);
PointD P = MeasurePoints[pointsIndex];
bool alreadyFound = false;
//所有通過的三角面及相交的點
Dictionary<PointD, Triangle> Interaction_Points_Triangles = new Dictionary<PointD, Triangle>();
for (int TriangleIndex = 0; TriangleIndex < AllTriangle.Length && !alreadyFound; TriangleIndex++)
{
//進度報告
double currentPercentage = ((double)pointsIndex * this.STL2Measure.Entities.Count + TriangleIndex) /
((double)MeasurePoints.Count * this.STL2Measure.Entities.Count)
* 100.0;
if (currentPercentage > Percentage + 1)
Percentage++;
Triangle aTriangle = AllTriangle[TriangleIndex] as Triangle;
PointD TriangleNormal = Cross(aTriangle.P2 - aTriangle.P1, aTriangle.P3 - aTriangle.P1);
double r = Sqrt(P.X * P.X + P.Y * P.Y);
double L = Sqrt((TriangleNormal.X) * r * (TriangleNormal.X) * r + (TriangleNormal.Y) * r * (TriangleNormal.Y) * r);
double C = (Dot(aTriangle.P1, TriangleNormal) - TriangleNormal.Z * P.Z) / L;
double phi = Atan2(TriangleNormal.Y * r, TriangleNormal.X * r);
double gamma = Acos(C);
double[] thetas = new double[] { gamma + phi, phi - gamma };
for (int i = 0; i < thetas.Length; i++)
{
PointD ans = new PointD(r * Cos(thetas[i]), r * Sin(thetas[i]), P.Z);
PointD v1 = aTriangle.P1 - ans;
PointD v2 = aTriangle.P2 - ans;
PointD v3 = aTriangle.P3 - ans;
PointD n1 = Cross(v1, v2);
PointD n2 = Cross(v2, v3);
PointD n3 = Cross(v3, v1);
if (((n1.Y >= 0 && n2.Y >= 0 && n3.Y >= 0) || (n1.Y <= 0 && n2.Y <= 0 && n3.Y <= 0)) && !(n1.Y == 0 && n2.Y == 0 && n3.Y == 0))
Interaction_Points_Triangles.Add(ans, aTriangle);
}
}
//篩選出最近點
PointD[] Interaction_Points = Interaction_Points_Triangles.Keys.ToArray();
Triangle[] Interaction_Triangles = Interaction_Points_Triangles.Values.ToArray();
if (Interaction_Points.Length > 0)
{
PointD nearestPoint = Interaction_Points[0];
Triangle nearestTriangle = Interaction_Triangles[0];
for (int i = 1; i < Interaction_Points.Length; i++)
{
if (Norm(Interaction_Points[i] - P) < Norm(nearestPoint - P))
{
nearestPoint = Interaction_Points[i];
nearestTriangle = Interaction_Triangles[i];
}
}
this.TouchPoints.Add(nearestPoint);
this.TouchedTriangles.Add(nearestTriangle);
alreadyFound = true;
}
else
{
this.TouchPoints.Add(null);
this.TouchedTriangles.Add(null);
}
}
///進度報告
this.IsBusy = false;
this.Percentage = 100;
this.FinishTime = DateTime.Now;
if (OnFinish != null) OnFinish(this, new EventArgs());
}
;
}
public PolyLine GetSegmentsAsPL(int i, int slice)
{
Console.WriteLine();
PointD[] p = new PointD[slice + 1];
PointD[] cP = new PointD[] { ControlPoints[i], ControlPoints[i + 1], ControlPoints[i + 2], ControlPoints[i + 3] };
//Nci
double[,] Nci = GetNci(i, DataOutput);
for (int j = 0; j < slice + 1; j++)
{
double tou = (double)j * 1.0 / (double)slice;
Double[,] touM = new Double[,] { { 1, tou, tou * tou, tou * tou * tou } };
double[,] cc = MatrixDot(touM, Nci);
if (DataOutput)
{
//Console.WriteLine(i + "tou:" + tou);
//foreach (var item in touM)
//{
// Console.Write(item + ",");
//}
//Console.WriteLine("");
//foreach (var item in cc)
//{
// Console.Write(item + ",");
//}
//Console.WriteLine("");
//Console.WriteLine(i + "th segments sum of Blenging sum:" + (cc[0, 0] + cc[0, 1] + cc[0, 2] + cc[0, 3]));
}
p[j] = (cc[0, 0] * cP[0] + cc[0, 1] * cP[1] + cc[0, 2] * cP[2] + cc[0, 3] * cP[3]);
}
Random rnd = new Random(Guid.NewGuid().GetHashCode());
PolyLine aPlyLine = new PolyLine(p) { Color = System.Drawing.Color.FromArgb(255, rnd.Next(256), rnd.Next(256), rnd.Next(256)), LineWidth = 2 };
//aPlyLine = new PolyLine(ControlPoints) { Color = this.Color };
return aPlyLine;
}
public PointD GetStanderCoordinate(PointD p)
{
PointD p1 = p - this.Boundary[0];
return new PointD(
p1.X / CubeDimentions[0],
p1.Y / CubeDimentions[1],
p1.Z / CubeDimentions[2]);
}
public CubicSpace GetCubicSpace(PointD p)
{
int[] index = GetCubicSpaceIndex(p);
return GetCubicSpace(index);
}
//public HashSet<CubicSpace> GetCubicSpaces(PointD position, Vector direction)
//{
// PointD ssP = GetStanderCoordinate(position);
// Vector ssD = Normalize(GetStanderVector(direction));
// HashSet<CubicSpace> IntersectedCubicSpace = new HashSet<CubicSpace>();
// int[] Dquadrant = Sign(direction.ToArray());
// int[] startIndex = GetCubicSpaceIndex(position);
// int[] nextIndex = GetCubicSpaceIndex(position);
// PointD nextsP = ssP.Clone() as PointD;
// if (!(startIndex[0] < 0 || startIndex[0] >= CubeArrayDimentions[0]
// || startIndex[1] < 0 || startIndex[1] >= CubeArrayDimentions[1]
// || startIndex[2] < 0 || startIndex[2] >= CubeArrayDimentions[2]))
// IntersectedCubicSpace.Add(this.GetCubicSpace(startIndex));
// bool end = false;
// while (!end)
// {
// //List<iv> rate6Org = new List<iv>{
// // new iv(0, nextIndex[0] - nextsP.X),
// // new iv(1, nextIndex[1] - nextsP.Y),
// // new iv(2, nextIndex[2] - nextsP.Z),
// // new iv(3, nextIndex[0] + 1 - nextsP.X),
// // new iv(4, nextIndex[1] + 1 - nextsP.Y),
// // new iv(5, nextIndex[2] + 1 - nextsP.Z)
// //};
// List<iv> rate6 = new List<iv>{
// new iv(0, nextIndex[0] - nextsP.X),
// new iv(1, nextIndex[1] - nextsP.Y),
// new iv(2, nextIndex[2] - nextsP.Z),
// new iv(3, nextIndex[0] + 1 - nextsP.X),
// new iv(4, nextIndex[1] + 1 - nextsP.Y),
// new iv(5, nextIndex[2] + 1 - nextsP.Z)
// };
// rate6[0].V = ssD.X != 0 ? rate6[0].V / ssD.X : double.NegativeInfinity;
// rate6[1].V = ssD.Y != 0 ? rate6[1].V / ssD.Y : double.NegativeInfinity;
// rate6[2].V = ssD.Z != 0 ? rate6[2].V / ssD.Z : double.NegativeInfinity;
// rate6[3].V = ssD.X != 0 ? rate6[3].V / ssD.X : double.NegativeInfinity;
// rate6[4].V = ssD.Y != 0 ? rate6[4].V / ssD.Y : double.NegativeInfinity;
// rate6[5].V = ssD.Z != 0 ? rate6[5].V / ssD.Z : double.NegativeInfinity;
// List<iv> rate26 = new List<iv>{
// new iv(0, nextIndex[0] - 1 - nextsP.X),
// new iv(1, nextIndex[1] - 1 - nextsP.Y),
// new iv(2, nextIndex[2] - 1 - nextsP.Z),
// new iv(3, nextIndex[0] + 2 - nextsP.X),
// new iv(4, nextIndex[1] + 2 - nextsP.Y),
// new iv(5, nextIndex[2] + 2 - nextsP.Z)
// };
// rate26[0].V = ssD.X != 0 ? rate26[0].V / ssD.X : double.NegativeInfinity;
// rate26[1].V = ssD.Y != 0 ? rate26[1].V / ssD.Y : double.NegativeInfinity;
// rate26[2].V = ssD.Z != 0 ? rate26[2].V / ssD.Z : double.NegativeInfinity;
// rate26[3].V = ssD.X != 0 ? rate26[3].V / ssD.X : double.NegativeInfinity;
// rate26[4].V = ssD.Y != 0 ? rate26[4].V / ssD.Y : double.NegativeInfinity;
// rate26[5].V = ssD.Z != 0 ? rate26[5].V / ssD.Z : double.NegativeInfinity;
// var rate =
// (from v in rate6
// where v.V >= 0
// orderby v.V
// select v).ToList();
// var rate2 =
// (from v in rate26
// where v.V >= 0
// orderby v.V
// select v).ToList();
// //int[] lastIndex = (int[])ArrayTake(nextIndex, new int[] { 0 }, new int[] { 2 });
// //PointD lastSP = nextsP.Clone() as PointD;
// nextIndex[rate[0].i % 3] += Dquadrant[rate[0].i % 3];
// double vRate = rate[1].V < rate2[0].V ? rate[1].V : (rate[0].V + rate2[0].V) / 2.0;
// nextsP = nextsP + ssD * vRate;
// //if (!(nextIndex[0] <= nextsP.X && nextIndex[0] + 1 >= nextsP.X
// //&& nextIndex[1] <= nextsP.Y && nextIndex[1] + 1 >= nextsP.Y
// //&& nextIndex[2] <= nextsP.Z && nextIndex[2] + 1 >= nextsP.Z))
// //{
// //}
// if (!(nextIndex[0] < 0 || nextIndex[0] >= CubeArrayDimentions[0]
// || nextIndex[1] < 0 || nextIndex[1] >= CubeArrayDimentions[1]
// || nextIndex[2] < 0 || nextIndex[2] >= CubeArrayDimentions[2]))
// {
// CubicSpace cs = this.GetCubicSpace(nextIndex);
// if (cs.HaveEntity)
// IntersectedCubicSpace.Add(cs);
// }
// else if (((nextIndex[0] < 0 && Dquadrant[0] > 0) || (nextIndex[0] > CubeArrayLastIndex[0] && Dquadrant[0] < 0))
// || ((nextIndex[1] < 0 && Dquadrant[1] > 0) || (nextIndex[1] > CubeArrayLastIndex[1] && Dquadrant[1] < 0))
// || ((nextIndex[2] < 0 && Dquadrant[2] > 0) || (nextIndex[2] > CubeArrayLastIndex[2] && Dquadrant[2] < 0)))
// {
// }
// else
// end = true;
// }
// return IntersectedCubicSpace;
//}
public HashSet<CubicSpace> GetCubicSpaces(PointD position, Vector direction)
{
return GetRay(position, direction);
}
public PointD GetStanderCoordinate(PointD p)
{
PointD p1 = p - this.Boundary[0];
//if (p1.Z / CubeDimentions[2] >= CubeArrayDimentions[2])
//{
// int a = 0;
//}
return new PointD(
p1.X / CubeDimentions[0],
p1.Y / CubeDimentions[1],
p1.Z / CubeDimentions[2]);
}
public int[] GetCubicSpaceIndex(PointD p)
{
PointD p1 = GetStanderCoordinate(p);
int[] index = new int[]{
(int)p1.X,
(int)p1.Y,
(int)p1.Z };
if (p1.X == CubeArrayDimentions[0])
index[0] -= 1;
if (p1.Y == CubeArrayDimentions[1])
index[1] -= 1;
if (p1.Z == CubeArrayDimentions[2])
index[2] -= 1;
return index;
}
public HashSet<CubicSpace> GetCubicSpaces2(PointD position, Vector direction)
{
HashSet<CubicSpace> CubicSpaceInD1 = GetCubicSpaces(position, direction);
HashSet<CubicSpace> CubicSpaceInD2 = GetCubicSpaces(position, -1 * direction);
foreach (var item in CubicSpaceInD2)
{
CubicSpaceInD1.Add(item);
}
return CubicSpaceInD1;
}
public HashSet<CubicSpace> GetCubicSpaces(PointD position, Vector direction)
{
PointD ssP = GetStanderCoordinate(position);
Vector ssD = Normalize(GetStanderVector(direction));
HashSet<CubicSpace> IntersectedCubicSpace = new HashSet<CubicSpace>();
int[] Dquadrant = Sign(direction.ToArray());
int[] startIndex = GetCubicSpaceIndex(position);
int[] nextIndex = GetCubicSpaceIndex(position);
PointD nextsP = ssP.Clone() as PointD;
if (startIndex[0] > 0 && startIndex[0] < CubeArrayDimentions[0]
&& startIndex[1] > 0 && startIndex[1] < CubeArrayDimentions[1]
&& startIndex[2] > 0 && startIndex[2] < CubeArrayDimentions[2])
IntersectedCubicSpace.Add(this.GetCubicSpace(startIndex));
bool end = false;
while (!end)
{
double[] rate6 = new double[]{
(System.Math.Ceiling(nextsP.X) - nextsP.X),
(System.Math.Ceiling(nextsP.Y) - nextsP.Y),
(System.Math.Ceiling(nextsP.Z) - nextsP.Z),
(System.Math.Floor(nextsP.X) - nextsP.X),
(System.Math.Floor(nextsP.Y) - nextsP.Y),
(System.Math.Floor(nextsP.Z) - nextsP.Z)
};
rate6[0] = ssD.X != 0 ? rate6[0] / ssD.X : double.NegativeInfinity;
rate6[1] = ssD.Y != 0 ? rate6[1] / ssD.Y : double.NegativeInfinity;
rate6[2] = ssD.Z != 0 ? rate6[2] / ssD.Z : double.NegativeInfinity;
rate6[3] = ssD.X != 0 ? rate6[3] / ssD.X : double.NegativeInfinity;
rate6[4] = ssD.Y != 0 ? rate6[4] / ssD.Y : double.NegativeInfinity;
rate6[5] = ssD.Z != 0 ? rate6[5] / ssD.Z : double.NegativeInfinity;
double maxRate = rate6[0];
int maxRateIndex = 0;
for (int i = 0; i < 6; i++)
if (rate6[i] > maxRate)
{
maxRate = rate6[i];
maxRateIndex = i;
}
double minRate = maxRate;
int minRateIndex = maxRateIndex;
for (int i = 0; i < 6; i++)
if (rate6[i] < minRate && rate6[i] >= 0)
{
minRate = rate6[i];
minRateIndex = i;
}
if (minRateIndex == 0 || minRateIndex == 3)
nextIndex[0] += Dquadrant[0];
if (minRateIndex == 1 || minRateIndex == 4)
nextIndex[1] += Dquadrant[1];
if (minRateIndex == 2 || minRateIndex == 5)
nextIndex[2] += Dquadrant[2];
nextsP = nextsP + ssD * (maxRate + minRate) / 2.0;
if (nextIndex[0] > 0 && nextIndex[0] < CubeArrayDimentions[0]
&& nextIndex[1] > 0 && nextIndex[1] < CubeArrayDimentions[1]
&& nextIndex[2] > 0 && nextIndex[2] < CubeArrayDimentions[2])
IntersectedCubicSpace.Add(this.GetCubicSpace(nextIndex));//
else if (
((nextIndex[0] < 0 || nextIndex[0] >= CubeArrayDimentions[0]) && nextIndex[0] / Dquadrant[0] < 0)
&& ((nextIndex[1] < 0 || nextIndex[1] >= CubeArrayDimentions[1]) && nextIndex[1] / Dquadrant[1] < 0)
&& ((nextIndex[2] < 0 || nextIndex[2] >= CubeArrayDimentions[2]) && nextIndex[2] / Dquadrant[2] < 0))
{
}
else
end = true;
}
return IntersectedCubicSpace;
}
public CubicSpace GetCubicSpace(PointD p)
{
int[] index = GetCubicSpaceIndex(p);
return CubeSpaces[index[0], index[1], index[2]];
}
public PolyLine[] ToPolyline(int slice)
{
if (DataPoints != null && ControlPoints != null)
{
int n = DataPoints.Length;
PointD[] p = new PointD[(n - 1) * slice + 1];
for (int i = 0; i < n - 1; i++)
{
PointD[] cP = new PointD[] { ControlPoints[i], ControlPoints[i + 1], ControlPoints[i + 2], ControlPoints[i + 3] };
//Nci
double[,] Nci = GetNci(i, DataOutput);
Console.WriteLine();
for (int j = 0; j < slice + 1; j++)
{
double tou = (double)j * 1.0 / (double)slice;
double[,] cc = MatrixDot(new Double[,] { { 1, tou, tou * tou, tou * tou * tou } }, Nci);
//Console.WriteLine(i + "th segments sum of Blenging sum:" + (cc[0, 0] + cc[0, 1] + cc[0, 2] + cc[0, 3]));
p[i * slice + j] = (cc[0, 0] * cP[0] + cc[0, 1] * cP[1] + cc[0, 2] * cP[2] + cc[0, 3] * cP[3]);
}
}
PolyLine aPlyLine = new PolyLine(p) { Color = this.Color, LineWidth = 2 };
//aPlyLine = new PolyLine(ControlPoints) { Color = this.Color };
return new PolyLine[] { aPlyLine };
}
else
return null;
}
public HashSet<CubicSpace> GetRay(PointD position, Vector direction)
{
PointD ssP = GetStanderCoordinate(position);
Vector ssD = Normalize(GetStanderVector(direction));
int[] Dquadrant = Sign(direction.ToArray());
int[] startIndex = GetCubicSpaceIndex(position);
int[] currentIndex = GetCubicSpaceIndex(position);
HashSet<CubicSpace> Ray = new HashSet<CubicSpace>();
#region XYZ射線距離計算
{
double Rate1 = Dot(ssD, new Vector(1, 0, 0));//////////////////
double Rate2 = Dot(ssD, new Vector(0, 1, 0));//////////////////
double Rate3 = Dot(ssD, new Vector(0, 0, 1));//////////////////
double v1 = Dquadrant[0] != 0 ? Rate1 * Abs(currentIndex[0] + Dquadrant[0] - ssP[0]) : double.PositiveInfinity;
double v2 = Dquadrant[1] != 0 ? Rate2 * Abs(currentIndex[1] + Dquadrant[1] - ssP[1]) : double.PositiveInfinity;
double v3 = Dquadrant[2] != 0 ? Rate3 * Abs(currentIndex[2] + Dquadrant[2] - ssP[2]) : double.PositiveInfinity;
while( !((currentIndex[0] < 0 && Dquadrant[0] < 0)|| (currentIndex[0] > CubeArrayLastIndex[0] && Dquadrant[0] > 0))
&& !((currentIndex[1] < 0 && Dquadrant[1] < 0)|| (currentIndex[1] > CubeArrayLastIndex[1] && Dquadrant[1] > 0))
&& !((currentIndex[2] < 0 && Dquadrant[2] < 0)|| (currentIndex[2] > CubeArrayLastIndex[2] && Dquadrant[2] > 0)))
{
CubicSpace cubic = GetCubicSpace(currentIndex);
if (cubic != null)
Ray.Add(cubic);
int Dir = 0;
double minV = v1;
if ( minV > v2)
Dir = 1; minV = v2;
if (minV > v3)
Dir = 2; minV = v3;
currentIndex[Dir] += Dquadrant[Dir];
if (Dir == 0)
v1 = Dquadrant[0] != 0 ?Rate1 * Abs(currentIndex[0] + Dquadrant[0] - ssP[0]) : double.PositiveInfinity;
else if (Dir == 1)
v2 = Dquadrant[1] != 0 ?Rate2 * Abs(currentIndex[1] + Dquadrant[1] - ssP[1]) : double.PositiveInfinity;
else if (Dir == 2)
v3 = Dquadrant[2] != 0 ? Rate3 * Abs(currentIndex[2] + Dquadrant[2] - ssP[2]) : double.PositiveInfinity;
}
}
#endregion
return Ray;
}
public void SetControllPoints(List<PointD> dataPoints)
{
DataPoints = dataPoints.ToArray();
int n = DataPoints.Length;
double[,] M = ZeroMatrix(n + 2, n + 2);
PointD[,] R = new PointD[n + 2, 1];
double[,] invM;
#region 計算弦長
delta_i = new double[n + 3];
delta_i[0] = 0;
delta_i[1] = 0;
delta_i[n+1] = 0;
delta_i[n+1+1] = 0;
for (int i = 0; i < n - 1; i++)
{
delta_i[i + 2] = Norm(DataPoints[i + 1] - DataPoints[i]);
}
#endregion
#region 計算M矩陣
M[0, 0] = -3;
M[0, 1] = 3;
M[1, 0] = 1;
M[n, n+1] = 1;
M[n+1, n] = -3;
M[n+1, n+1] = 3;
for (int i = 1; i < n - 1; i++)
{
int i_d = i + 2;
int i_M = i + 1;
//fi
M[i_M, i] = Pow(delta_i[i_d], 2)
/ (delta_i[i_d - 1] + delta_i[i_d])
/ (delta_i[i_d - 2] + delta_i[i_d - 1] + delta_i[i_d]);
//gi
M[i_M, i + 2] = Pow(delta_i[i_d - 1], 2)
/ (delta_i[i_d - 1] + delta_i[i_d] + delta_i[i_d + 1])
/ (delta_i[i_d - 1] + delta_i[i_d]);
//hi
M[i_M, i + 1] = 1 - M[i_M, i] - M[i_M, i + 2];
}
#endregion
#region 計算R矩陣
if (n == 2)
{
R[0, 0] = (DataPoints[1] - DataPoints[0]) / 3.0;
R[n + 1, 0] = (DataPoints[0] - DataPoints[1]) / 3.0;
}
else
{
PointD a0 = DataPoints[1] - DataPoints[0];
PointD b0 = DataPoints[2] - DataPoints[0];
PointD c0 = Cross(a0, b0);
PointD r0 = (Norm(a0) * Norm(a0) * Cross(b0, c0) + Norm(b0) * Norm(b0) * Cross(c0, a0)) / (2 * Norm(c0) * Norm(c0));
R[0, 0] = Norm(a0) * Cross(r0, c0) / Norm(Cross(r0, c0));
PointD an = DataPoints[n - 2] - DataPoints[n - 1];
PointD bn = DataPoints[n - 3] - DataPoints[n - 1];
PointD cn = Cross(an, bn);
PointD rn = (Norm(an) * Norm(an) * Cross(bn, cn) + Norm(bn) * Norm(bn) * Cross(cn, an)) / (2 * Norm(cn) * Norm(cn));
R[n + 1, 0] = -1.0 * Norm(an) * Cross(rn, cn) / Norm(Cross(rn, cn));
}
for (int i = 0; i < n; i++)
{
int i_M = i + 1;
R[i_M, 0] = DataPoints[i];
}
#endregion
#region 求解控制點
invM = MatrixInverse(M);
if (n == 2)
{
ControlPoints = new PointD[] { DataPoints[0], DataPoints[0], DataPoints[1], DataPoints[1] };
}
else
{
ControlPoints = new PointD[n + 2];
for (int i = 0; i < ControlPoints.GetLength(0); i++)
{
ControlPoints[i] = new PointD();
for (int j = 0; j < ControlPoints.GetLength(0); j++)
{
ControlPoints[i] += invM[i, j] * R[j, 0];
}
}
}
#endregion
#region Debug
if (DataOutput)
{
//Print Knot spans(chord length)
Console.WriteLine("");
Console.WriteLine("Knot spans = ");
foreach (var item in delta_i)
{
Console.WriteLine(item.ToString("0.000000"));
}
//Print M
Console.WriteLine("");
Console.WriteLine("M = ");
Console.WriteLine("{");
for (int i = 0; i < M.GetLength(0); i++)
{
Console.Write("{");
for (int j = 0; j < M.GetLength(1); j++)
{
Console.Write("\t");
Console.Write(M[i, j].ToString("0.000000"));
if (j != (M.GetLength(1) - 1))
{
Console.Write(",");
}
}
Console.Write("}");
if (i != (M.GetLength(0) - 1))
{
Console.WriteLine(",");
}
}
Console.WriteLine("}");
//Print R
Console.WriteLine("");
Console.WriteLine("R = ");
foreach (var item in R)
{
Console.WriteLine("{" + item.X.ToString("0.000000") + ",\t\t" + item.Y.ToString("0.000000") + ",\t\t" + item.Z.ToString("0.000000") + "}");
}
Console.WriteLine("}");
//Print invM
Console.WriteLine("");
Console.WriteLine("invM = ");
Console.WriteLine("{");
for (int i = 0; i < invM.GetLength(0); i++)
{
Console.Write("{");
for (int j = 0; j < invM.GetLength(1); j++)
{
Console.Write("\t");
Console.Write(invM[i, j].ToString("0.000000"));
if (j != (invM.GetLength(1) - 1))
{
Console.Write(",");
}
}
Console.Write("}");
if (i != (invM.GetLength(0) - 1))
{
Console.WriteLine(",");
}
}
Console.WriteLine("");
Console.WriteLine("}");
//Print Control Points
Console.WriteLine("");
Console.WriteLine("Control Points = ");
foreach (var item in ControlPoints)
{
Console.WriteLine("{" + item.X.ToString("0.000000") + ",\t\t" + item.Y.ToString("0.000000") + ", \t\t" + item.Z.ToString("0.000000") + "}");
}
}
#endregion Debug
}
public HashSet<CubicSpace> GetCubicSpaces2(PointD position, Vector direction)
{
HashSet<CubicSpace> CubicSpaceInD1 = GetCubicSpaces(position, direction);
HashSet<CubicSpace> CubicSpaceInD2 = GetCubicSpaces(position, -1 * direction);
foreach (var item in CubicSpaceInD2)
{
CubicSpaceInD1.Add(item);
}
//foreach (var item in CubicSpaceInD1)
//{
// if (item.HaveEntity)
// item.Visible = Visibility.Visible;
//}
return CubicSpaceInD1;
}
public PolyLine ToControlPointLine(System.Drawing.Color color)
{
PointD[] p = new PointD[ControlPoints.Length];
for (int i = 0; i < p.Length; i++)
{
p[i] = ControlPoints[i];
}
return new PolyLine(p) { Color = color, LineType = PTL.Definitions.LineType.DotDashed };
}
public static STL ReadSTL_ASCII(String fileName, Predicate<Triangle> TriangleFilter = null)
{
System.IO.StreamReader stream = new System.IO.StreamReader(fileName, Encoding.Default);
string strLine;
string[] strArray;
List<String> strList;
STL newSTL;
//aSTL.Name = myFile.ReadLine();
try
{
STL aSTL = new STL() { Color = System.Drawing.Color.Gray };
while (stream.Peek() != -1)
{
Triangle aTriangle = new Triangle() { Color = System.Drawing.Color.Transparent };
//1 法向量
strLine = stream.ReadLine();
strArray = strLine.Split(' ');
strList = new List<string>();
foreach (var item in strArray)
{
if (item != "")
strList.Add(item);
}
if (strList[0].ToLower() == "facet")
{
PointD N = new PointD()
{
X = Convert.ToDouble(strList[2]),
Y = Convert.ToDouble(strList[3]),
Z = Convert.ToDouble(strList[4])
};
aTriangle.N1 = N;
//捨棄
stream.ReadLine();
//點
strLine = stream.ReadLine();
strArray = strLine.Split(' ');
strList = new List<string>();
foreach (var item in strArray)
{
if (item != "")
strList.Add(item);
}
aTriangle.P1 = new PointD()
{
X = Convert.ToDouble(strList[1]),
Y = Convert.ToDouble(strList[2]),
Z = Convert.ToDouble(strList[3])
};
//點
strLine = stream.ReadLine();
strArray = strLine.Split(' ');
strList = new List<string>();
foreach (var item in strArray)
{
if (item != "")
strList.Add(item);
}
aTriangle.P2 = new PointD()
{
X = Convert.ToDouble(strList[1]),
Y = Convert.ToDouble(strList[2]),
Z = Convert.ToDouble(strList[3])
};
//點
strLine = stream.ReadLine();
strArray = strLine.Split(' ');
strList = new List<string>();
foreach (var item in strArray)
{
if (item != "")
strList.Add(item);
}
aTriangle.P3 = new PointD()
{
X = Convert.ToDouble(strList[1]),
Y = Convert.ToDouble(strList[2]),
Z = Convert.ToDouble(strList[3])
};
//捨棄
stream.ReadLine();
//捨棄
stream.ReadLine();
if (TriangleFilter != null)
{
if (TriangleFilter(aTriangle))
aSTL.AddEntity(aTriangle);
}
else
{
aSTL.AddEntity(aTriangle);
}
}
}
newSTL = aSTL;
}
catch
{
newSTL = null;
}
stream.Dispose();
stream.Close();
if (newSTL.Entities.Count == 0)
newSTL = null;
return newSTL;
}