public static Vertex GetInstane(Vector3Double position, Status status = Status.UNKNOWN)
{
return ObjectPool<Vertex>.Create(initVertex, construct);
void initVertex(Vertex c) => c.InitMember(position, status);
Vertex construct() => new Vertex(position, status);
}
public Vertex InitMember(Vector3Double position, Status status = Status.UNKNOWN)
{
Position = position;
this.status = status;
disposedValue = false;
return this;
}
//-------------------------------------PRIVATES---------------------------------//
/**
* Checks if one of the coordinates of a vertex exceed the ones found before
*
* @param vertex vertex to be tested
*/
private void CheckVertex(Vector3Double vertex)
{
if (vertex.x > xMax)
{
xMax = vertex.x;
}
else if (vertex.x < xMin)
{
xMin = vertex.x;
}
if (vertex.y > yMax)
{
yMax = vertex.y;
}
else if (vertex.y < yMin)
{
yMin = vertex.y;
}
if (vertex.z > zMax)
{
zMax = vertex.z;
}
else if (vertex.z < zMin)
{
zMin = vertex.z;
}
}
public double GetArea()
{
Vector3Double p1 = v1.Position;
Vector3Double p2 = v2.Position;
Vector3Double p3 = v3.Position;
return(Vector3Double.Cross(p2 - p1, p3 - p1).magnitude / 2);
}
//---------------------------------CONSTRUCTORS---------------------------------//
/// <summary>
/// 计算面的包围盒
/// </summary>
/// <param name="p1"></param>
/// <param name="p2"></param>
/// <param name="p3"></param>
public Bound(Vector3Double p1, Vector3Double p2, Vector3Double p3)
{
xMax = xMin = p1.x;
yMax = yMin = p1.y;
zMax = zMin = p1.z;
CheckVertex(p2);
CheckVertex(p3);
}
/// <summary>
/// 带公差判等
/// </summary>
/// <param name="vector"></param>
/// <param name="another"></param>
/// <param name="tol"></param>
/// <returns></returns>
public static bool EqualsTol(this Vector3Double vector, Vector3Double another, double tol)
{
if (tol < 0)
{
tol = -tol;
}
var delta = vector - another;
return(Math.Abs(delta.x) < tol && Math.Abs(delta.y) < tol && Math.Abs(delta.z) < tol);
}
/// <summary>
/// 略微抖动射线方向
/// </summary>
public void PerturbDirection()
{
Vector3Double perturbedDirection = Direction;
perturbedDirection.x += 1e-5 * random.NextDouble();
perturbedDirection.y += 1e-5 * random.NextDouble();
perturbedDirection.z += 1e-5 * random.NextDouble();
Direction = perturbedDirection;
}
/// <summary>
/// 平移所有顶点
/// </summary>
public void translate(Vector3Double dv)
{
if (dv.x == 0 && dv.y == 0 && dv.z == 0)
{
return;
}
for (int i = 0; i < vertices.Length; i++)
{
vertices[i] += dv;
}
}
//------------------------------------PRIVATES----------------------------------//
/// <summary>
/// Gets the position of a point relative to a line in the x plane
/// </summary>
/// <param name="point">point to be tested</param>
/// <param name="pointLine1">one of the line ends</param>
/// <param name="pointLine2">one of the line ends</param>
/// <returns>position of the point relative to the line - UP, DOWN, ON, NONE</returns>
private static Side LinePositionInX(Vector3Double point, Vector3Double pointLine1, Vector3Double pointLine2)
{
if ((Math.Abs(pointLine1.y - pointLine2.y) > epsilon) && (((point.y >= pointLine1.y) && (point.y <= pointLine2.y)) || ((point.y <= pointLine1.y) && (point.y >= pointLine2.y))))
{
var a = (pointLine2.z - pointLine1.z) / (pointLine2.y - pointLine1.y);
var b = pointLine1.z - a * pointLine1.y;
var z = a * point.y + b;
return(z > point.z + epsilon ? Side.UP : z < point.z - epsilon ? Side.DOWN : Side.ON);
}
else
{
return(Side.NONE);
}
}
/// <summary>
/// 构造直线，为两平面的交线
/// </summary>
public Line(Face face1, Face face2)
{
Vector3Double normalFace1 = face1.PlaneNormal;
Vector3Double normalFace2 = face2.PlaneNormal;
Direction = Vector3Double.Cross(normalFace1, normalFace2);
if (!(Direction.magnitude < EqualityTolerance)) // 两平面不平行
{
//getting a line point, zero is set to a coordinate whose direction
// 获取线上一点，方向坐标设为零点
//component isn't zero (line intersecting its origin plan)
// 成员不为零（线与第一个平面相交）
var d1 = -Vector3Double.Dot(normalFace1, face1.v1.Position);
var d2 = -Vector3Double.Dot(normalFace2, face2.v1.Position);
if (Math.Abs(Direction.x) > EqualityTolerance)
{
startPoint = new Vector3Double
{
x = 0,
y = (d2 * normalFace1.z - d1 * normalFace2.z) / Direction.x,
z = (d1 * normalFace2.y - d2 * normalFace1.y) / Direction.x
};
}
else if (Math.Abs(Direction.y) > EqualityTolerance)
{
startPoint = new Vector3Double
{
x = (d1 * normalFace2.z - d2 * normalFace1.z) / Direction.y,
y = 0,
z = (d2 * normalFace1.x - d1 * normalFace2.x) / Direction.y
};
}
else
{
startPoint = new Vector3Double
{
x = (d2 * normalFace1.y - d1 * normalFace2.y) / Direction.z,
y = (d1 * normalFace2.x - d2 * normalFace1.x) / Direction.z,
z = 0
};
}
}
else
{
startPoint = default;
}
Direction.Normalize();
}
/// <summary>
/// Gets the position of a point relative to a line in the z plane
/// </summary>
/// <param name="point">point to be tested</param>
/// <param name="pointLine1">one of the line ends</param>
/// <param name="pointLine2">one of the line ends</param>
/// <returns>position of the point relative to the line - UP, DOWN, ON, NONE</returns>
private static Side LinePositionInZ(Vector3Double point, Vector3Double pointLine1, Vector3Double pointLine2)
{
if ((Math.Abs(pointLine1.x - pointLine2.x) > epsilon) &&
(((point.x >= pointLine1.x) && (point.x <= pointLine2.x)) || ((point.x <= pointLine1.x) && (point.x >= pointLine2.x))))
{
var a = (pointLine2.y - pointLine1.y) / (pointLine2.x - pointLine1.x);
var b = pointLine1.y - a * pointLine1.x;
var y = a * point.x + b;
return(y > point.y + epsilon ? Side.UP : y < point.y - epsilon ? Side.DOWN : Side.ON);
}
else
{
return(Side.NONE);
}
}
//-----------------------------------PRIVATES--------------------------------//
/**
* Gets the solid mean
*
* @return point representing the mean
*/
protected Vector3Double getMean()
{
Vector3Double mean = new Vector3Double();
for (int i = 0; i < vertices.Length; i++)
{
mean.x += vertices[i].x;
mean.y += vertices[i].y;
mean.z += vertices[i].z;
}
mean.x /= vertices.Length;
mean.y /= vertices.Length;
mean.z /= vertices.Length;
return(mean);
}
/// <summary>
/// Compute the point resulting from the intersection with a plane
/// </summary>
/// <param name="normal">the plane normal</param>
/// <param name="planePoint">a plane point.</param>
/// <returns>intersection point.If they don't intersect, return null</returns>
public Vector3Double ComputePlaneIntersection(Plane plane)
{
var distanceToStartFromOrigin = Vector3Double.Dot(plane.planeNormal, startPoint);
var distanceFromPlane = distanceToStartFromOrigin - plane.distanceToPlaneFromOrigin;
var denominator = Vector3Double.Dot(plane.planeNormal, Direction);
if (Math.Abs(denominator) < EqualityTolerance) // 射线与平面垂直
{
return(Math.Abs(distanceFromPlane) < EqualityTolerance ? startPoint : Vector3Double.PositiveInfinity);
}
else // 射线被平面拦截
{
return(StartPoint + (Direction * -distanceFromPlane / denominator));
}
}
/**
* Applies a rotation into a solid
*
* @param dx rotation on the x axis
* @param dy rotation on the y axis
*/
public void rotate(double dx, double dy)
{
double cosX = Math.Cos(dx);
double cosY = Math.Cos(dy);
double sinX = Math.Sin(dx);
double sinY = Math.Sin(dy);
if (dx != 0 || dy != 0)
{
//get mean
Vector3Double mean = getMean();
double newX, newY, newZ;
for (int i = 0; i < vertices.Length; i++)
{
vertices[i].x -= mean.x;
vertices[i].y -= mean.y;
vertices[i].z -= mean.z;
//x rotation
if (dx != 0)
{
newY = vertices[i].y * cosX - vertices[i].z * sinX;
newZ = vertices[i].y * sinX + vertices[i].z * cosX;
vertices[i].y = newY;
vertices[i].z = newZ;
}
//y rotation
if (dy != 0)
{
newX = vertices[i].x * cosY + vertices[i].z * sinY;
newZ = -vertices[i].x * sinY + vertices[i].z * cosY;
vertices[i].x = newX;
vertices[i].z = newZ;
}
vertices[i].x += mean.x;
vertices[i].y += mean.y;
vertices[i].z += mean.z;
}
}
}
/// <summary>
/// 设交线的结束点在平面的边上；
/// 如果没有指定过结束点，就指定起点在边上，否则指定结束点在边上
/// </summary>
/// <param name="vertex1">交线线段的其中一点</param>
/// <param name="vertex2">交线线段的其中一点</param>
/// <returns></returns>
private bool SetEdge(Vertex vertex1, Vertex vertex2)
{
Vector3Double point1 = vertex1.Position;
Vector3Double point2 = vertex2.Position;
Vector3Double edgeDirection = new Vector3Double(point2.x - point1.x, point2.y - point1.y, point2.z - point1.z);
Line edgeLine = new Line(edgeDirection, point1);
if (NumEndsSet == 0)
{
StartVertex = vertex1;
StartType = EDGE;
StartPosition = line.ComputeLineIntersection(edgeLine);
StartDistance = line.ComputePointToPointDistance(StartPosition);
IntermediateType = FACE;
NumEndsSet++;
return(true);
}
else if (NumEndsSet == 1)
{
EndVertex = vertex1;
EndType = EDGE;
EndPosition = line.ComputeLineIntersection(edgeLine);
EndDistance = line.ComputePointToPointDistance(EndPosition);
IntermediateType = FACE;
NumEndsSet++;
//the ending point distance should be smaller than starting point distance
if (StartDistance > EndDistance)
{
SwapEnds();
}
return(true);
}
else
{
return(false);
}
}
/** Swaps the starting point and the ending point */
private void SwapEnds()
{
double distTemp = StartDistance;
StartDistance = EndDistance;
EndDistance = distTemp;
int typeTemp = StartType;
StartType = EndType;
EndType = typeTemp;
Vertex vertexTemp = StartVertex;
StartVertex = EndVertex;
EndVertex = vertexTemp;
Vector3Double posTemp = StartPosition;
StartPosition = EndPosition;
EndPosition = posTemp;
}
/// <summary>
/// Checks if the the face contains a point
/// </summary>
/// <param name="point">point to be tested</param>
/// <returns>true if the face contains the point, false otherwise</returns>
private bool ContainsPoint(Vector3Double point)
{
Side result1;
Side result2;
Side result3;
Vector3Double normal = PlaneNormal;
//if x is constant...
if (Math.Abs(normal.x) > epsilon)
{
//tests on the x plane
result1 = LinePositionInX(point, v1.Position, v2.Position);
result2 = LinePositionInX(point, v2.Position, v3.Position);
result3 = LinePositionInX(point, v3.Position, v1.Position);
}
//if y is constant...
else if (Math.Abs(normal.y) > epsilon)
{
//tests on the y plane
result1 = LinePositionInY(point, v1.Position, v2.Position);
result2 = LinePositionInY(point, v2.Position, v3.Position);
result3 = LinePositionInY(point, v3.Position, v1.Position);
}
else
{
//tests on the z plane
result1 = LinePositionInZ(point, v1.Position, v2.Position);
result2 = LinePositionInZ(point, v2.Position, v3.Position);
result3 = LinePositionInZ(point, v3.Position, v1.Position);
}
// if the point is up and down two lines...
// if the point is on of the lines...
return((((result1 == Side.UP) || (result2 == Side.UP) || (result3 == Side.UP)) &&
((result1 == Side.DOWN) || (result2 == Side.DOWN) || (result3 == Side.DOWN))) ||
(result1 == Side.ON) || (result2 == Side.ON) || (result3 == Side.ON));
}
/// <summary>
/// Computes the point resulting from the intersection with another line
/// </summary>
/// <param name="otherLine">the other line to apply the intersection. The lines are supposed to intersect</param>
/// <returns>point resulting from the intersection. If the point coundn't be obtained, return null</returns>
public Vector3Double ComputeLineIntersection(Line otherLine)
{
//x = x1 + a1*t = x2 + b1*s
//y = y1 + a2*t = y2 + b2*s
//z = z1 + a3*t = z2 + b3*s
Vector3Double lineP = otherLine.StartPoint;
Vector3Double lineDir = otherLine.Direction;
double t = 0;
if (Math.Abs(Direction.y * lineDir.x - Direction.x * lineDir.y) > EqualityTolerance)
{
t = (-startPoint.y * lineDir.x + lineP.y * lineDir.x + lineDir.y * startPoint.x - lineDir.y * lineP.x) /
(Direction.y * lineDir.x - Direction.x * lineDir.y);
}
else if (Math.Abs(-Direction.x * lineDir.z + Direction.z * lineDir.x) > EqualityTolerance)
{
t = -(-lineDir.z * startPoint.x + lineDir.z * lineP.x + lineDir.x * startPoint.z - lineDir.x * lineP.z) /
(-Direction.x * lineDir.z + Direction.z * lineDir.x);
}
else if (Math.Abs(-Direction.z * lineDir.y + Direction.y * lineDir.z) > EqualityTolerance)
{
t = (startPoint.z * lineDir.y - lineP.z * lineDir.y - lineDir.z * startPoint.y + lineDir.z * lineP.y) /
(-Direction.z * lineDir.y + Direction.y * lineDir.z);
}
else
{
#if DEBUG
throw new InvalidOperationException();
#else
return(Vector3Double.Zero);
#endif
}
return(StartPoint + (Direction * t));
}
public static bool Equals(this Vector3 vf, Vector3Double vd)
{
return(vd.ToVector3().Equals(vf));
}
public static Vector3 ToVector3(this Vector3Double vector)
{
return(new Vector3((float)vector.x, (float)vector.y, (float)vector.z));
}
/// <summary>
/// 计算从线到另一点的距离
/// </summary>
/// <param name="otherPoint">the point to compute the distance from the line point. The point is supposed to be on the same line.</param>
/// <returns>如果射线起点到另一点的向量与射线方向相反，会得到负数的值</returns>
public double ComputePointToPointDistance(Vector3Double otherPoint)
{
var distance = (otherPoint - startPoint).magnitude;
return((Vector3Double.Dot((otherPoint - startPoint).normalized, Direction) < 0) ? -distance : distance);
}
/// <summary>
/// 用方向和起点构造射线
/// </summary>
/// <param name="direction">射线方向</param>
/// <param name="point">射线起点坐标</param>
public Line(Vector3Double direction, Vector3Double point)
{
Direction = direction;
startPoint = point;
direction.Normalize();
}
/// <summary>
/// 为表面分类，基于光线追踪技术
/// </summary>
/// <param name="obj">计算面状态的 3D 物件</param>
public void RayTraceClassify(Object3D obj)
{
Line ray = new Line(PlaneNormal, Center); // 射线从面的几何中心发出，方向为法线方向
Face closet = default;
bool foundCloset = false;
double closestDistance;
bool success;
do
{
success = true;
closestDistance = double.MaxValue;
for (int faceIndex = 0; faceIndex < obj.GetNumFaces(); faceIndex++)
{
Face face = obj.GetFace(faceIndex);
var intersectionPoint = ray.ComputePlaneIntersection(face.Plane); // 对三角面所在的平面采样
if (intersectionPoint.x < float.PositiveInfinity) // 射线被平面拦截
{
var distance = ray.ComputePointToPointDistance(intersectionPoint);
var dot = Vector3Double.Dot(face.PlaneNormal, ray.Direction);
bool parallel = Math.Abs(dot) < epsilon; // 射线与平面平行
//if ray lies in plane...
if (Math.Abs(distance) < epsilon) // 交点是射线起点
{
if (parallel)
{
// 略微抖动光线方向以免采样到另一平面
ray.PerturbDirection();
success = false;
break;
}
else if (face.ContainsPoint(intersectionPoint))
{
// 面重合
closet = face;
foundCloset = true;
closestDistance = 0;
break;
}
}
else if (!parallel && distance > epsilon) // 射线产生交点
{
if (distance < closestDistance && face.ContainsPoint(intersectionPoint))
{
closestDistance = distance;
closet = face; // 当前的面时最近的平面
foundCloset = true;
}
}
}
}
} while (!success);
if (!foundCloset)
{
Status = Status.OUTSIDE;
} // 没有找到面，自己是外部面
else // 由离自己最近的面，检查方向
{
var dot = Vector3Double.Dot(closet.PlaneNormal, ray.Direction);
if (Math.Abs(closestDistance) < epsilon) // 距离为零，这个面和自己重合
{
if (dot > epsilon)
{
Status = Status.SAME;
}
else if (dot < -epsilon)
{
Status = Status.OPPOSITE;
}
}
else if (dot > epsilon)
{
Status = Status.INSIDE;
} // 不重合，同向，在参数物件内部
else if (dot < -epsilon)
{
Status = Status.OUTSIDE;
} // 不重合，反向，在参数物件外部
}
}
//----------------------------------CONSTRUCTORS--------------------------------//
/// <summary>
/// 构造指定状态的顶点
/// </summary>
/// <param name="position">顶点坐标</param>
/// <param name="status">顶点状态：未知，边界，内部 或 外部</param>
public Vertex(Vector3Double position, Status status = Status.UNKNOWN)
{
InitMember(position, status);
}
