public static void WriteString(string str, Vector p1, Color color)
{
GL.glPushMatrix();
glColor3d(color);
GL.glRasterPos3d(p1.X, p1.Y, p1.Z);
for (int i = 0; i < str.Length; i++)
{
uint tt = (uint)(str[i]);
GL.glCallList(tt);
}
GL.glPopMatrix();
}
public static Vector operator *(Vector p1, double scale)
{
Vector po = new Vector();
po.X = scale * p1.X;
po.Y = scale * p1.Y;
po.Z = scale * p1.Z;
return po;
}
public static void Translated(Vector p1)
{
GL.glTranslated(p1.X, p1.Y, p1.Z);
}
public static void Vertex3d(Vector p1)
{
GL.glVertex3d(p1.X, p1.Y, p1.Z);
}
/// <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();
}
}
}
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 Ellipse(PointD center, Vector majorDirection, Vector minorDirection, Double ra, double rb)
{
this.fCenter = center;
this.fEndPointOfMajorAxis = Normalize(majorDirection) * Ra;
this.fNormal = Cross(majorDirection, minorDirection);
this.fRatio = rb / ra;
}
public Vector(Vector p)
{
this.X = p.X;
this.Y = p.Y;
this.Z = p.Z;
}
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 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;
}
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 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 Vector GetStanderVector(Vector v)
{
return new Vector(
v.X / CubeDimentions[0],
v.Y / CubeDimentions[1],
v.Z / CubeDimentions[2]);
}
public static Vector operator /(Vector p1, double denominator)
{
Vector po = new Vector();
po.X = p1.X / denominator;
po.Y = p1.Y / denominator;
po.Z = p1.Z / denominator;
return po;
}
public Ellipse(PointD center, Vector majorDirection, Vector minorDirection, Double ra, double rb, Color color, LineType lineType, float width)
{
this.fCenter = center;
this.fEndPointOfMajorAxis = Normalize(majorDirection) * ra;
this.fNormal = Cross(majorDirection, minorDirection);
this.fRatio = rb / ra;
this.Color = color;
this.LineType = lineType;
this.LineWidth = width;
}
public PointD(Vector p)
:base(p)
{
}
public Ellipse(PointD center, Vector endPointOfMajorAxis, Vector normal, double ratio, double startAngle, double endAngle)
{
this.fCenter = center;
this.fEndPointOfMajorAxis = endPointOfMajorAxis;
this.fNormal = normal;
this.fRatio = ratio;
this.fStartAngle = startAngle;
this.fEndAngle = endAngle;
}
public static Vector operator -(Vector p1, Vector p2)
{
Vector p3 = new Vector();
p3.X = p1.X - p2.X;
p3.Y = p1.Y - p2.Y;
p3.Z = p1.Z - p2.Z;
return p3;
}
public Ellipse(PointD center, Vector endPointOfMajorAxis, Vector normal, double ratio)
{
this.fCenter = center;
this.fEndPointOfMajorAxis = endPointOfMajorAxis;
this.fNormal = normal;
this.fRatio = ratio;
}
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();
}
}
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;
}
