public static Matrix4x4 CreateFromAxisAngleOrigin(PointDirection3 p, double angle)
{
return
(Matrix4x4.CreateTranslation(-p.Point)
* Matrix4x4.CreateFromAxisAngle(p.Direction.Unit(), angle)
* Matrix4x4.CreateTranslation(p.Point));
}
public static Matrix4x4 CreateFromAxisAngleOrigin(PointDirection3 p, double angle)
{
return
Matrix4x4.CreateTranslation(-p.Point)
*Matrix4x4.CreateFromAxisAngle(p.Direction.Unit(), angle)
*Matrix4x4.CreateTranslation(p.Point);
}
ShortestConnectingEdge
(Edge3 e1, PointDirection3 e2, double tol = 1e-9)
{
var u1 = e1.B - e1.A;
var u2 = e2.Direction;
var w = e1.A - e2.Point;
var a = Vector3.Dot(u1, u1); // always >= 0
var b = Vector3.Dot(u1, u2);
var c = Vector3.Dot(u2, u2); // always >= 0
var d = Vector3.Dot(u1, w);
var e = Vector3.Dot(u2, w);
var det = a * c - b * b;
double t1, d1 = det; // sc = sN / sD, default sD = D >= 0
double t2, d2 = det; // tc = tN / tD, default tD = D >= 0
// compute the line parameters of the two closest points
if (det < tol)
{
// the lines are almost parallel
t1 = 0.0; // force using point P0 on segment S1
d1 = 1.0; // to prevent possible division by 0.0 later
t2 = e;
d2 = c;
}
else
{
// get the closest points on the infinite lines
t1 = b * e - c * d;
t2 = a * e - b * d;
if (t1 < 0.0)
{
// sc < 0 => the s=0 edge is visible
t1 = 0.0;
t2 = e;
d2 = c;
}
else if (t1 > d1)
{
// sc > 1 => the s=1 edge is visible
t1 = d1;
t2 = e + b;
d2 = c;
}
}
// finally do the division to get sc and tc
var c1 = Math.Abs(t1) < tol ? 0.0 : t1 / d1;
var c2 = Math.Abs(t2) < tol ? 0.0 : t2 / d2;
var p1 = c1 * u1 + e1.A;
var p2 = c2 * u2 + e2.Point;
return(new Edge3(p1, p2));
}
public void NonIntersectingLinesShouldReturnNone_v2(Vector3 position, Vector3 forward, Vector3 up)
{
var line = new Edge3(new Vector3(-1, -2, -3) * 0.001, new Vector3(-1, -2, -3));
var plane = new PointDirection3(Vector3.Zero, Vector3.UnitX);
var transform = Matrix4x4.CreateWorld(position, forward, up);
var tline = line.ApplyTransform(transform);
var tplane = plane.ApplyTransform(transform);
tline.IntersectPlane(tplane).IsSome.Should().BeFalse();
}
public Option<Vector3> IntersectPlane(PointDirection3 plane)
{
var t = (plane.Point - A).Dot(plane.Direction)
/(B - A).Dot(plane.Direction);
if(t<=0)
return Option<Vector3>.None;
if(t>=1)
return Option<Vector3>.None;
return A + t*(B - A);
}
public void IntersectingLineWithPlaneShouldReturnIntersectionPoint(Vector3 position, Vector3 forward, Vector3 up)
{
var line = new Edge3(new Vector3(1,2,3), new Vector3(-1,-2,-3));
var plane = new PointDirection3(Vector3.Zero, Vector3.UnitX);
var transform = Matrix4x4.CreateWorld(position, forward, up);
var tline = line.ApplyTransform(transform);
var tplane = plane.ApplyTransform(transform);
var intersectPlane = tline.IntersectPlane(tplane).__Value__();
var intersectPlaneExpectedValue = Vector3.Transform(Vector3.Zero, transform);
intersectPlane.Should().BeApproximately(intersectPlaneExpectedValue, 1e-6);
}
public Option<Edge3> IntersectPlane(PointDirection3 plane)
{
var d = plane.IsAbovePlane(A);
if (d == plane.IsAbovePlane(B) && d == plane.IsAbovePlane(C))
return Option<Edge3>.None;
var p0 = EdgeAB.IntersectPlane(plane);
var p1 = EdgeBC.IntersectPlane(plane);
var p2 = EdgeCA.IntersectPlane(plane);
return Match(p0, p1)
.BindNone(() => Match(p0, p2))
.BindNone (() => Match(p1, p2));
}
public Option <Vector3> IntersectPlane(PointDirection3 plane)
{
var t = (plane.Point - A).Dot(plane.Direction)
/ (B - A).Dot(plane.Direction);
if (t <= 0)
{
return(Option <Vector3> .None);
}
if (t >= 1)
{
return(Option <Vector3> .None);
}
return(A + t * (B - A));
}
public Option <Edge3> IntersectPlane(PointDirection3 plane)
{
var d = plane.IsAbovePlane(A);
if (d == plane.IsAbovePlane(B) && d == plane.IsAbovePlane(C))
{
return(Option <Edge3> .None);
}
var p0 = EdgeAB.IntersectPlane(plane);
var p1 = EdgeBC.IntersectPlane(plane);
var p2 = EdgeCA.IntersectPlane(plane);
return(Match(p0, p1)
.BindNone(() => Match(p0, p2))
.BindNone(() => Match(p1, p2)));
}
public TriangleWithNormals(Vector3 a, Vector3 b, Vector3 c)
{
A = new PointDirection3(a, default(Vector3));
B = new PointDirection3(b, default(Vector3));
C = new PointDirection3(c, default(Vector3));
}
public TriangleWithNormals(PointDirection3 a, PointDirection3 b, PointDirection3 c)
{
A = a;
B = b;
C = c;
}
public void IntersectingShouldBeMatched(Matrix4x4 transform, Triangle triangle)
{
var plane = new PointDirection3(Vector3.Zero, Vector3.UnitX);
var ttriangle = triangle.ApplyTransform(transform);
var pplane = plane.ApplyTransform(transform);
var intersectionO = ttriangle.IntersectPlane(pplane);
intersectionO.IsSome.Should().Be(true);
}
public Edge3 ShortestEdgeJoining(PointDirection3 other, double tol = 1e-9)
{
return ShortestConnectingEdge(this, other, tol);
}
public static PointParamWithRayProjection ClosestPointToRay(this ICurve curve, PointDirection3 ray, double tol = 1e-9)
{
var bound = curve.Domain();
int numOfRadius = 0;
double[] radius = null;
MathPoint location = null;
var radiusResult = curve.FindMinimumRadius();
var domain = new RangeDouble(curve.Domain());
var tessTol = radiusResult.Radius / 10;
var closestPointOnEdge = Vector3.Zero;
for (var i = 0; i < 1; i++)
{
var tessPoints = Sequences
.LinSpace(domain.Min, domain.Max, 100)
.Select(curve.PointParamAt).ToList();
var edges = tessPoints.Buffer(2, 1).Where(buf => buf.Count == 2)
.ToList();
var closestEdge
= edges
.Select(edge => new { edge, connection = MakeEdge(edge).ShortestEdgeJoining(ray, tol) })
.MinBy(o => o.connection.LengthSquared)[0];
var a = closestEdge.edge[0].T;
var b = closestEdge.edge[1].T;
domain = new RangeDouble(a, b);
tessTol = tessTol / 10;
}
Func <Vector3, Vector3> projectOnRay = p => (p - ray.Point).ProjectOn(ray.Direction) + ray.Point;
var solver = new BrentSearch(t =>
{
var p = curve.PointAt(t);
var proj = projectOnRay(p);
return((p - proj).LengthSquared());
}, domain.Min, domain.Max);
solver.Minimize();
var minT = solver.Solution;
var pointParam = curve.PointParamAt(minT);
return(new PointParamWithRayProjection(pointParam, projectOnRay(pointParam.Point)));
}
public static PointParamWithRayProjection ClosestPointToRay(this ICurve curve, PointDirection3 ray, double tol = 1e-9)
{
var bound = curve.Domain();
int numOfRadius = 0;
double[] radius = null;
MathPoint location = null;
var radiusResult = curve.FindMinimumRadius();
var domain = new RangeDouble(curve.Domain());
var tessTol = radiusResult.Radius/10;
var closestPointOnEdge = Vector3.Zero;
for (var i = 0; i < 1; i++)
{
var tessPoints = Sequences
.LinSpace(domain.Min, domain.Max, 100)
.Select(curve.PointParamAt).ToList();
var edges = tessPoints.Buffer(2, 1).Where(buf => buf.Count == 2)
.ToList();
var closestEdge
= edges
.Select(edge => new {edge, connection= MakeEdge(edge).ShortestEdgeJoining(ray, tol)})
.MinBy(o=>o.connection.LengthSquared)[0];
var a = closestEdge.edge[0].T;
var b = closestEdge.edge[1].T;
domain = new RangeDouble(a, b);
tessTol = tessTol/10;
}
Func<Vector3, Vector3> projectOnRay = p => (p - ray.Point).ProjectOn(ray.Direction) + ray.Point;
var solver = new BrentSearch(t =>
{
var p = curve.PointAt(t);
var proj = projectOnRay(p);
return (p - proj).LengthSquared();
}, domain.Min, domain.Max);
solver.Minimize();
var minT = solver.Solution;
var pointParam = curve.PointParamAt(minT);
return new PointParamWithRayProjection(pointParam, projectOnRay(pointParam.Point));
}
ShortestConnectingEdge
(Edge3 e1, PointDirection3 e2, double tol = 1e-9)
{
var u1 = e1.B - e1.A;
var u2 = e2.Direction;
var w = e1.A - e2.Point;
var a = Vector3.Dot(u1, u1); // always >= 0
var b = Vector3.Dot(u1, u2);
var c = Vector3.Dot(u2, u2); // always >= 0
var d = Vector3.Dot(u1, w);
var e = Vector3.Dot(u2, w);
var det = a * c - b * b;
double t1, d1 = det; // sc = sN / sD, default sD = D >= 0
double t2, d2 = det; // tc = tN / tD, default tD = D >= 0
// compute the line parameters of the two closest points
if (det < tol)
{
// the lines are almost parallel
t1 = 0.0; // force using point P0 on segment S1
d1 = 1.0; // to prevent possible division by 0.0 later
t2 = e;
d2 = c;
}
else
{
// get the closest points on the infinite lines
t1 = b * e - c * d;
t2 = a * e - b * d;
if (t1 < 0.0)
{
// sc < 0 => the s=0 edge is visible
t1 = 0.0;
t2 = e;
d2 = c;
}
else if (t1 > d1)
{
// sc > 1 => the s=1 edge is visible
t1 = d1;
t2 = e + b;
d2 = c;
}
}
// finally do the division to get sc and tc
var c1 = Math.Abs(t1) < tol ? 0.0 : t1 / d1;
var c2 = Math.Abs(t2) < tol ? 0.0 : t2 / d2;
var p1 = c1*u1 + e1.A;
var p2 = c2*u2 + e2.Point;
return new Edge3(p1,p2);
}
public TriangleWithNormals(PointDirection3 a, PointDirection3 b, PointDirection3 c)
{
A = a;
B = b;
C = c;
}
/// <summary>
/// Create a surface from a plane definition
/// </summary>
/// <param name="activeModeler"></param>
/// <param name="plane"></param>
/// <returns></returns>
public static ISurface CreatePlanarSurface(this IModeler activeModeler, PointDirection3 plane)
{
var planeDirection = plane.Direction;
var vRef = planeDirection.Orthogonal();
var mathVector = planeDirection;
var vRootPoint = plane.Point;
return (ISurface) activeModeler.CreatePlanarSurface2(vRootPoint.ToDoubles(), mathVector.ToDoubles(), vRef.ToDoubles());
}
public TriangleWithNormals(Vector3 a, Vector3 b, Vector3 c)
{
A = new PointDirection3( a, default(Vector3));
B = new PointDirection3(b, default(Vector3) );
C = new PointDirection3(c, default(Vector3) );
}
public Edge3 ShortestEdgeJoining(PointDirection3 other, double tol = 1e-9)
{
return(ShortestConnectingEdge(this, other, tol));
}