/// <summary>
/// 判断点是否在矩形内
/// </summary>
/// <param name="pt"></param>
/// <returns></returns>
public override bool CheckIn(Double3 pt)
{
// 首先判断是在矩形所在的平面内。
if (base.CheckIn(pt) == false)
{
return(false);
}
// pt 在矩形的2条边之间
Double3 diff = pt - this.Pt;
if (Double3.Dot(Double3.Cross(diff, Aixs1), Double3.Cross(diff, Aixs2)) > 0)
{
return(false);
}
// diff 在2个轴上的分量小于轴的长度
double length1 = Double3.Dot(diff, Aixs1);
if (length1 < 0 || length1 > Aixs1.sqrMagnitude)
{
return(false);
}
length1 = Double3.Dot(diff, Aixs2);
if (length1 < 0 || length1 > Aixs2.sqrMagnitude)
{
return(false);
}
return(true);
}
/// <summary>
/// 获取交点
/// </summary>
/// <param name="line"></param>
/// <param name="intersectPoint"></param>
/// <returns></returns>
public virtual bool GetIntersectPoint(Line3D line, ref Double3 intersectPoint)
{
if (line == null)
{
return(false);
}
Double3 aixsVector = AixsVector(line.StartPoint);
double distance = aixsVector.magnitude;
if (distance == 0)
{
intersectPoint = line.StartPoint;
return(true);
}
else
{
double dot = Double3.Dot(aixsVector.normalized, line.NormalizedDir);
if (dot == 0)
{
return(false);
}
else
{
intersectPoint = line.StartPoint - distance / dot * line.NormalizedDir;
return(true);
}
}
}
/// <summary>
/// 与线的关系
/// </summary>
/// <param name="line"></param>
/// <returns></returns>
public virtual LineRelation CheckLineRelation(Line3D line)
{
if (line == null)
{
return(LineRelation.None);
}
if (Double3.Dot(line.NormalizedDir, this.NormalizedNormal) == 0)
{
Double3 diff = line.StartPoint - this.Pt;
if (Double3.Dot(diff, this.NormalizedNormal) == 0)
{
// 线在平面上
return(LineRelation.Coincide);
}
else
{
return(LineRelation.Parallel);
}
}
else
{
Double3 intersectPoint = Double3.zero;
if (GetIntersectPoint(line, ref intersectPoint) == true)
{
return(LineRelation.Intersect);
}
else
{
return(LineRelation.Detach);
}
}
}
/// <summary>
/// 获取交点
/// </summary>
/// <param name="line"></param>
/// <param name="intersectPoint"></param>
/// <returns></returns>
public virtual bool GetIntersectPoint(Line3D line, ref Double3 intersectPoint)
{
if (line == null)
{
return(false);
}
Double3 aixsVector = AixsVector(line.StartPoint);
double distance = aixsVector.magnitude;
if (distance == 0)
{
intersectPoint = line.StartPoint;
return(true);
}
else
{
double dot = Double3.Dot(aixsVector.normalized, line.NormalizedDir);
if (dot == 0)
{
return(false);
}
else
{
Double3 point = line.StartPoint - distance / dot * line.NormalizedDir;
if (line is Rays3D)
{
if (Double3.Dot(line.NormalizedDir, point - line.StartPoint) >= 0)
{
intersectPoint = point;
return(true);
}
else
{
return(false);
}
}
else if (line is LineSegment3D)
{
if (Double3.Dot(point - line.StartPoint, point - (line as LineSegment3D).EndPoint) <= 0)
{
intersectPoint = point;
return(true);
}
else
{
return(false);
}
}
else
{
intersectPoint = point;
return(true);
}
}
}
}
/// <summary>
/// 判断是否在相同的平面
/// </summary>
/// <param name="line1"></param>
/// <param name="line2"></param>
/// <returns></returns>
public static bool CheckInSamePlane(Line3D line1, Line3D line2)
{
if (line1 == null || line2 == null)
{
return(false);
}
// 判断是否共面
Double3 diff = line2.StartPoint - line1.StartPoint;
Double3 VerticalAxis = Line3D.GetVerticalAxis(line1, line2);
return(Double3.Dot(diff, VerticalAxis) == 0 ? true : false);
}
private static PointMapping GetMapping(Double3 point, IList <Double3> lowResPoints,
IList <Double3> lowResNormals, IList <Triangle> lowResTriangles, Sphere[] spheres)
{
//Find the closest triangle in the low resoltuion mesh
int l = spheres.Length;
int indexOfClosest = -1;
double closestDist = double.MaxValue * 1e-8;
Triangle tri;
Double3 a, b, c;
double uBest = 1.0 / 3.0, vBest = 1.0 / 3.0;
for (int i = 0; i < l; ++i)
{
Sphere s = spheres[i];
double d2 = (point - spheres[i].Center).LengthSquared;
double sum = closestDist + s.Radius;
if (sum * sum > d2) //The solution can only improve if this condition is met
{
tri = lowResTriangles[i];
a = lowResPoints[tri.A];
b = lowResPoints[tri.B];
c = lowResPoints[tri.C];
double u, v, w;
d2 = DistancePointTriangleSquared(a, b, c, point, out u, out v, out w);
if (d2 < closestDist * closestDist)
{
closestDist = Math.Sqrt(d2);
indexOfClosest = i;
uBest = u;
vBest = v;
}
}
}
//Compute barycentricCoordinates
tri = lowResTriangles[indexOfClosest];
a = lowResPoints[tri.A];
b = lowResPoints[tri.B];
c = lowResPoints[tri.C];
// Optimization reduces artifacts, unfortunately cannot fully avoid them
Double3 uvw = PatternSearch(uBest, vBest, point, lowResPoints[tri.A], lowResPoints[tri.B], lowResPoints[tri.C], lowResNormals[tri.A], lowResNormals[tri.B], lowResNormals[tri.C]);
Double3 dir = point - (uvw.X * a + uvw.Y * b + uvw.Z * c);
Double3 normal = uvw.X * lowResNormals[tri.A] + uvw.Y * lowResNormals[tri.B] + uvw.Z * lowResNormals[tri.C];
double offset = dir.Length * Math.Sign(Double3.Dot(dir, normal));
Double3 p = uvw.X * a + uvw.Y * b + uvw.Z * c;
p = p + offset * normal;
return(new PointMapping(tri, uvw, offset)); //uvw are barycentric Coordinates
}
/// <summary>
/// 判断点是否在直线上
/// </summary>
/// <param name="pt"></param>
/// <returns></returns>
public virtual bool CheckIn(Double3 pt)
{
Double3 diff = pt - this.Pt;
if (Double3.Dot(diff, this.NormalizedNormal) == 0)
{
return(true);
}
else
{
return(false);
}
}
/// <summary>
/// 获取交点
/// </summary>
/// <param name="line"></param>
/// <param name="intersectPoint"></param>
/// <returns></returns>
public override bool GetIntersectPoint(Line3D line, ref Double3 intersectPoint)
{
Double3 point = Double3.zero;
if (base.GetIntersectPoint(line, ref point) == true)
{
if (Double3.Dot(point - line.StartPoint, point - (line as LineSegment3D).EndPoint) <= 0)
{
intersectPoint = point;
return(true);
}
else
{
return(false);
}
}
return(false);
}
/// <summary>
/// 点导视线的距离
/// </summary>
/// <param name="pt"></param>
/// <returns></returns>
public override double CalcDistance(Double3 pt)
{
Double3 diff = pt - this.StartPoint;
if (diff == Double3.zero)
{
return(0);
}
if (Double3.Dot(diff, this.NormalizedDir) <= 0)
{
return(diff.magnitude);
}
else
{
Double3 verticalAxis = this.AixsVector(pt);
return(verticalAxis.magnitude);
}
}
/// <summary>
/// 判断点是否在射线上
/// </summary>
/// <param name="pt"></param>
/// <returns></returns>
public override bool CheckIn(Double3 pt)
{
bool ret = base.CheckIn(pt);
if (ret == true)
{
Double3 diff = pt - this.StartPoint;
if (Double3.Dot(diff, this.NormalizedDir) < 0)
{
return(false);
}
else
{
return(true);
}
}
return(false);
}
/// <summary>
/// 获取交点
/// </summary>
/// <param name="line"></param>
/// <param name="intersectPoint"></param>
/// <returns></returns>
public override bool GetIntersectPoint(Line3D line, ref Double3 intersectPoint)
{
Double3 point = Double3.zero;
if (base.GetIntersectPoint(line, ref point) == true)
{
if (Double3.Dot(line.NormalizedDir, point - line.StartPoint) >= 0)
{
intersectPoint = point;
return(true);
}
else
{
return(false);
}
}
return(false);
}
/// <summary>
/// 判断点是否在直线上
/// </summary>
/// <param name="pt"></param>
/// <returns></returns>
public override bool CheckIn(Double3 pt)
{
bool ret = base.CheckIn(pt);
if (ret == true)
{
Double3 diff1 = pt - this.StartPoint;
Double3 diff2 = pt - this.EndPoint;
if (Double3.Dot(diff1, diff2) > 0)
{
return(false);
}
else
{
return(true);
}
}
return(false);
}
/// <summary>
/// 点导几何元素的距离
/// </summary>
/// <param name="pt"></param>
/// <returns></returns>
public override double CalcDistance(Double3 pt)
{
Double3 diff1 = pt - this.StartPoint;
Double3 diff2 = pt - this.EndPoint;
double ret1 = Double3.Dot(diff1, this.NormalizedDir);
double ret2 = Double3.Dot(diff2, this.NormalizedDir);
if (ret1 * ret2 <= 0)
{
Double3 aixsVector = this.AixsVector(pt);
return(aixsVector.magnitude);
}
else if (ret1 < 0)
{
return(diff1.magnitude);
}
else
{
return(diff2.magnitude);
}
}
/// <summary>
/// 与线的关系
/// </summary>
/// <param name="line"></param>
/// <returns></returns>
public override LineRelation CheckLineRelation(Line3D line)
{
LineRelation lr = base.CheckLineRelation(line);
if (lr == LineRelation.Intersect)
{
Double3 aixsVector = line.AixsVector(this.StartPoint);
if (Double3.Dot(aixsVector, this.NormalizedDir) > 0)
{
return(LineRelation.Detach);
}
else
{
return(LineRelation.Intersect);
}
}
else
{
return(lr);
}
}
/// <summary>
/// 与线的关系
/// </summary>
/// <param name="line"></param>
/// <returns></returns>
public override LineRelation CheckLineRelation(Line3D line)
{
LineRelation lr = base.CheckLineRelation(line);
if (lr == LineRelation.Intersect)
{
// 计算轴向量
Double3 aixsVector1 = line.AixsVector(this.StartPoint);
Double3 aixsVector2 = line.AixsVector(this.EndPoint);
// 方向相反相交
if (Double3.Dot(aixsVector1, aixsVector2) <= 0)
{
return(LineRelation.Intersect);
}
else
{
return(LineRelation.Detach);
}
}
else
{
return(lr);
}
}
//http://www.geometrictools.com/GTEngine/Include/Mathematics/GteDistPointTriangleExact.h
public static double DistancePointTriangleSquared(Double3 triA, Double3 triB, Double3 triC, Double3 point, out double u, out double v, out double w)
{
Double3 diff = point - triA;
Double3 edge0 = triB - triA;
Double3 edge1 = triC - triA;
double a00 = Double3.Dot(edge0, edge0);
double a01 = Double3.Dot(edge0, edge1);
double a11 = Double3.Dot(edge1, edge1);
double b0 = -Double3.Dot(diff, edge0);
double b1 = -Double3.Dot(diff, edge1);
double det = a00 * a11 - a01 * a01;
double t0 = a01 * b1 - a11 * b0;
double t1 = a01 * b0 - a00 * b1;
if (t0 + t1 <= det)
{
if (t0 < 0)
{
if (t1 < 0) // region 4
{
if (b0 < 0)
{
t1 = 0;
if (-b0 >= a00) // V0
{
t0 = 1;
}
else // E01
{
t0 = -b0 / a00;
}
}
else
{
t0 = 0;
if (b1 >= 0) // V0
{
t1 = 0;
}
else if (-b1 >= a11) // V2
{
t1 = 1;
}
else // E20
{
t1 = -b1 / a11;
}
}
}
else // region 3
{
t0 = 0;
if (b1 >= 0) // V0
{
t1 = 0;
}
else if (-b1 >= a11) // V2
{
t1 = 1;
}
else // E20
{
t1 = -b1 / a11;
}
}
}
else if (t1 < 0) // region 5
{
t1 = 0;
if (b0 >= 0) // V0
{
t0 = 0;
}
else if (-b0 >= a00) // V1
{
t0 = 1;
}
else // E01
{
t0 = -b0 / a00;
}
}
else // region 0, interior
{
double invDet = 1 / det;
t0 *= invDet;
t1 *= invDet;
}
}
else
{
double tmp0, tmp1, numer, denom;
if (t0 < 0) // region 2
{
tmp0 = a01 + b0;
tmp1 = a11 + b1;
if (tmp1 > tmp0)
{
numer = tmp1 - tmp0;
denom = a00 - ((double)2) * a01 + a11;
if (numer >= denom) // V1
{
t0 = 1;
t1 = 0;
}
else // E12
{
t0 = numer / denom;
t1 = 1 - t0;
}
}
else
{
t0 = 0;
if (tmp1 <= 0) // V2
{
t1 = 1;
}
else if (b1 >= 0) // V0
{
t1 = 0;
}
else // E20
{
t1 = -b1 / a11;
}
}
}
else if (t1 < 0) // region 6
{
tmp0 = a01 + b1;
tmp1 = a00 + b0;
if (tmp1 > tmp0)
{
numer = tmp1 - tmp0;
denom = a00 - ((double)2) * a01 + a11;
if (numer >= denom) // V2
{
t1 = 1;
t0 = 0;
}
else // E12
{
t1 = numer / denom;
t0 = 1 - t1;
}
}
else
{
t1 = 0;
if (tmp1 <= 0) // V1
{
t0 = 1;
}
else if (b0 >= 0) // V0
{
t0 = 0;
}
else // E01
{
t0 = -b0 / a00;
}
}
}
else // region 1
{
numer = a11 + b1 - a01 - b0;
if (numer <= 0) // V2
{
t0 = 0;
t1 = 1;
}
else
{
denom = a00 - ((double)2) * a01 + a11;
if (numer >= denom) // V1
{
t0 = 1;
t1 = 0;
}
else // 12
{
t0 = numer / denom;
t1 = 1 - t0;
}
}
}
}
u = 1 - t0 - t1;
v = t0;
w = t1;
double dx = diff.X - (t0 * edge0.X + t1 * edge1.X);
double dy = diff.Y - (t0 * edge0.Y + t1 * edge1.Y);
double dz = diff.Z - (t0 * edge0.Z + t1 * edge1.Z);
return(dx * dx + dy * dy + dz * dz);
}
/// <summary>
/// Berechnet die Kp-Matrix für die Ebenen aller Facetten.
/// </summary>
/// <param name="strict">
/// true, um eine <c>InvalidOperationException</c> zu werfen, falls ein
/// degeneriertes Face entdeckt wird.
/// </param>
/// <returns>
/// Eine Liste von Kp-Matrizen.
/// </returns>
/// <exception cref="InvalidOperationException">
/// Es wurde ein degeneriertes Face gefunden, dessen Vertices kollinear sind.
/// </exception>
IList <Double4x4> ComputeKpForEachPlane(bool strict)
{
var kp = new List <Double4x4>();
var degenerate = new List <int[]>();
foreach (var f in faces)
{
var points = new[] {
vertices[f[0]],
vertices[f[1]],
vertices[f[2]]
};
// Ebene aus den 3 Ortsvektoren konstruieren.
var dir1 = points[1] - points[0];
var dir2 = points[2] - points[0];
var n = Double3.Cross(dir1, dir2);
// Wenn das Kreuzprodukt der Nullvektor ist, sind die Richtungsvektoren
// kollinear, d.h. die Vertices liegen auf einer Geraden und die Facette
// ist degeneriert.
if (n == Double3.Zero)
{
degenerate.Add(f);
if (strict)
{
var msg = new StringBuilder()
.AppendFormat("Encountered degenerate face ({0} {1} {2})",
f[0], f[1], f[2])
.AppendLine()
.AppendFormat("Vertex 1: {0}\n", points[0])
.AppendFormat("Vertex 2: {0}\n", points[1])
.AppendFormat("Vertex 3: {0}\n", points[2])
.ToString();
throw new InvalidOperationException(msg);
}
}
else
{
n.Normalize();
var a = n.X;
var b = n.Y;
var c = n.Z;
var d = -Double3.Dot(n, points[0]);
// Siehe [Gar97], Abschnitt 5 ("Deriving Error Quadrics").
var m = new Double4x4()
{
M11 = a * a,
M12 = a * b,
M13 = a * c,
M14 = a * d,
M21 = a * b,
M22 = b * b,
M23 = b * c,
M24 = b * d,
M31 = a * c,
M32 = b * c,
M33 = c * c,
M34 = c * d,
M41 = a * d,
M42 = b * d,
M43 = c * d,
M44 = d * d
};
kp.Add(m);
}
}
if (degenerate.Count > 0)
{
System.Diagnostics.Debug.WriteLine("Warning: {0} degenerate faces found.", degenerate.Count);
}
foreach (var d in degenerate)
{
faces.Remove(d);
}
return(kp);
}