public void TrimLineShouldWork
(double[] p0
, double[] p1)
{
CreatePartDoc(modelDoc =>
{
var v0 = new Vector3(p0[0], p0[1], p0[2]);
var v1 = new Vector3(p1[0], p1[1], p1[2]);
var edge = new Edge3(v0, v1);
var limitedLine = Modeler.CreateTrimmedLine(edge);
var tol = 1e-9;
var startPoint = limitedLine.StartPoint(0)[0];
var endPoint = limitedLine.EndPoint(0)[0];
v0.X.Should().BeApproximately(startPoint.X, tol);
v0.Y.Should().BeApproximately(startPoint.Y, tol);
v0.Z.Should().BeApproximately(startPoint.Z, tol);
v1.X.Should().BeApproximately(endPoint.X, tol);
v1.Y.Should().BeApproximately(endPoint.Y, tol);
v1.Z.Should().BeApproximately(endPoint.Z, tol);
//######################################################
var wbv = limitedLine.CreateWireBody();
var d = wbv.DisplayUndoable(modelDoc, Color.Blue);
return d;
});
}
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 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);
}
/// <summary>
/// Create a line from p0 to p1.
/// </summary>
/// <param name="modeler"></param>
/// <param name="edge"></param>
/// <returns></returns>
public static ICurve CreateTrimmedLine(this IModeler modeler, Edge3 edge)
{
Debug.Assert(edge.Delta.Length() > double.Epsilon);
var startPoint = new[] {(double)edge.A.X, (double)edge.A.Y,(double)edge.A.Z};
var dir = edge.Delta.Unit();
var direction = new[] {(double)dir.X,(double)dir.Y,(double)dir.Z};
var line = (ICurve)modeler.CreateLine(startPoint, direction);
Debug.Assert(line != null, "line != null");
var pp0 = line.GetClosestPointOn(edge.A.X, edge.A.Y, edge.A.Z).CastArray<double>();
var pp1 = line.GetClosestPointOn(edge.B.X, edge.B.Y, edge.B.Z).CastArray<double>();
//line = line.CreateTrimmedCurve2(pp0[0], pp0[1], pp0[2], pp1[0], pp1[1], pp1[2]);
var trimmedLine = line.CreateTrimmedCurve(pp0[3], pp1[3]);
Debug.Assert(trimmedLine != null, "line != null");
return trimmedLine;
}
public static IDisposable DisplayUndoable
(
this Edge3 line
,
IModelDoc2 doc
,
Color color
,
float thickness = 1
,
int layer = 0
)
{
var wire = new OpenWire(new [] { line.A, line.B }.ToList(), thickness, color);
return(wire.DisplayUndoable(doc, layer));
}
public static EdgeDistance ClosestDistanceBetweenTwoCurves(IMathUtility m, ICurve curve0, ICurve curve1)
{
var curveDomain = curve1.Domain();
var solver = new BrentSearch
(t =>
{
var pt = curve1.PointAt(t);
return((curve0.ClosestPointOn(pt).Point - pt).Length());
}
, curveDomain[0]
, curveDomain[1]
);
solver.Minimize();
var param = solver.Solution;
var pt1 = curve1.PointAt(param);
var pt0 = curve0.ClosestPointOn(pt1).Point;
var edge = new Edge3(pt1, pt0);
return(new EdgeDistance(edge, solver.Value));
}
public static Gen<Triangle> Triangle(this Gen<double> floats)
{
return floats
.Vector3()
.Triangle()
.Where
(v =>
{
var e0 = new Edge3(v.A, v.B);
var e1 = new Edge3(v.A, v.C);
var e2 = new Edge3(v.B, v.C);
var minDist = 1e-2;
var maxDist = 1;
if (List(e0, e1, e2).Any(e => e.Length < minDist))
return false;
if (List(e0, e1, e2).XPaired().Any(t => t.Map((a, b) => a.AngleBetween(b) < 5d/180*Math.PI)))
return false;
return true;
});
}
public static Matrix4x4 CreateFromEdgeAngleOrigin(Edge3 p, double angle)
{
return(CreateFromAxisAngleOrigin(new PointDirection3(p.A, p.Delta), angle));
}
public static Matrix4x4 CreateFromEdgeAngleOrigin(Edge3 p, double angle)
{
return CreateFromAxisAngleOrigin(new PointDirection3(p.A,p.Delta),angle);
}
public static EdgeDistance ClosestDistanceBetweenTwoCurves(IMathUtility m,ICurve curve0, ICurve curve1)
{
var curveDomain = curve1.Domain();
var solver = new BrentSearch
(t =>
{
var pt = curve1.PointAt(t);
return (curve0.ClosestPointOn(pt).Point - pt).Length();
}
, curveDomain[0]
, curveDomain[1]
);
solver.Minimize();
var param = solver.Solution;
var pt1 = curve1.PointAt(param);
var pt0 = curve0.ClosestPointOn(pt1).Point;
var edge = new Edge3(pt1, pt0);
return new EdgeDistance(edge, solver.Value);
}
public Edge3 ShortestEdgeJoining(Edge3 other, double tol = 1e-9)
{
return(ShortestConnectingEdge(this, other, tol));
}
public double AngleBetween(Edge3 other) => this.Delta.AngleBetweenVectors(other.Delta);
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);
}
ShortestConnectingEdge
(Edge3 e0, Edge3 e1, double tol = 1e-9)
{
var u = e0.B - e0.A;
var v = e1.B - e1.A;
var w = e0.A - e1.A;
var a = Vector3.Dot(u, u); // always >= 0
var b = Vector3.Dot(u, v);
var c = Vector3.Dot(v, v); // always >= 0
var d = Vector3.Dot(u, w);
var e = Vector3.Dot(v, w);
var det = a * c - b * b;
double sN, sD = det; // sc = sN / sD, default sD = D >= 0
double tN, tD = 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
sN = 0.0; // force using point P0 on segment S1
sD = 1.0; // to prevent possible division by 0.0 later
tN = e;
tD = c;
}
else
{
// get the closest points on the infinite lines
sN = b * e - c * d;
tN = a * e - b * d;
if (sN < 0.0)
{
// sc < 0 => the s=0 edge is visible
sN = 0.0;
tN = e;
tD = c;
}
else if (sN > sD)
{
// sc > 1 => the s=1 edge is visible
sN = sD;
tN = e + b;
tD = c;
}
}
if (tN < 0.0)
{
// tc < 0 => the t=0 edge is visible
tN = 0.0;
// recompute sc for this edge
if (-d < 0.0)
{
sN = 0.0;
}
else if (-d > a)
{
sN = sD;
}
else
{
sN = -d;
sD = a;
}
}
else if (tN > tD)
{ // tc > 1 => the t=1 edge is visible
tN = tD;
// recompute sc for this edge
if ((-d + b) < 0.0)
{
sN = 0;
}
else if ((-d + b) > a)
{
sN = sD;
}
else
{
sN = -d + b;
sD = a;
}
}
// finally do the division to get sc and tc
var sc = Math.Abs(sN) < tol ? 0.0 : sN / sD;
var tc = Math.Abs(tN) < tol ? 0.0 : tN / tD;
// get the difference of the two closest points
var dP = w + (sc * u) - (tc * v); // = S1(sc) - e2(tc)
var p1 = sc * u + e0.A;
var p2 = tc * v + e1.A;
return(new Edge3(p1, p2));
}
public double AngleBetween(Edge3 other) => this.Delta.AngleBetweenVectors(other.Delta);
public void ShouldGetTheShortestLine()
{
var edge0 = new Edge3(vector(0,-1,-1), vector(0,1,-1));
var edge1 = new Edge3(vector(-1,0,1), vector(1,0,1));
var connect = edge0.ShortestEdgeJoining(edge1);
connect.A.X.Should().BeApproximately(0, 1e-6);
connect.A.Y.Should().BeApproximately(0, 1e-6);
connect.A.Z.Should().BeApproximately(-1, 1e-6);
connect.B.X.Should().BeApproximately(0, 1e-6);
connect.B.Y.Should().BeApproximately(0, 1e-6);
connect.B.Z.Should().BeApproximately(1, 1e-6);
}
public Edge3 ShortestEdgeJoining(Edge3 other, double tol = 1e-9)
{
return ShortestConnectingEdge(this, other, tol);
}
ShortestConnectingEdge
(Edge3 e0, Edge3 e1, double tol = 1e-9)
{
var u = e0.B - e0.A;
var v = e1.B - e1.A;
var w = e0.A - e1.A;
var a = Vector3.Dot(u, u); // always >= 0
var b = Vector3.Dot(u, v);
var c = Vector3.Dot(v, v); // always >= 0
var d = Vector3.Dot(u, w);
var e = Vector3.Dot(v, w);
var det = a * c - b * b;
double sN, sD = det; // sc = sN / sD, default sD = D >= 0
double tN, tD = 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
sN = 0.0; // force using point P0 on segment S1
sD = 1.0; // to prevent possible division by 0.0 later
tN = e;
tD = c;
}
else
{
// get the closest points on the infinite lines
sN = b * e - c * d;
tN = a * e - b * d;
if (sN < 0.0)
{
// sc < 0 => the s=0 edge is visible
sN = 0.0;
tN = e;
tD = c;
}
else if (sN > sD)
{
// sc > 1 => the s=1 edge is visible
sN = sD;
tN = e + b;
tD = c;
}
}
if (tN < 0.0)
{
// tc < 0 => the t=0 edge is visible
tN = 0.0;
// recompute sc for this edge
if (-d < 0.0)
sN = 0.0;
else if (-d > a)
sN = sD;
else
{
sN = -d;
sD = a;
}
}
else if (tN > tD)
{ // tc > 1 => the t=1 edge is visible
tN = tD;
// recompute sc for this edge
if ((-d + b) < 0.0)
sN = 0;
else if ((-d + b) > a)
sN = sD;
else
{
sN = -d + b;
sD = a;
}
}
// finally do the division to get sc and tc
var sc = Math.Abs(sN) < tol ? 0.0 : sN / sD;
var tc = Math.Abs(tN) < tol ? 0.0 : tN / tD;
// get the difference of the two closest points
var dP = w + (sc * u) - (tc * v); // = S1(sc) - e2(tc)
var p1 = sc*u + e0.A;
var p2 = tc*v + e1.A;
return new Edge3(p1,p2);
}
public EdgeDistance(Edge3 edge, double distance)
{
Edge = edge;
Distance = distance;
}
public void LinspaceForVector3ShouldWork()
{
CreatePartDoc(modelDoc =>
{
var v0 = new Vector3(0, 0, 0);
var v1 = new Vector3(1, 0, 0);
var intV = Sequences.LinSpace(v0, v1, 5).ToList();
var interpolated = intV
.Buffer(2, 1)
.Where(p => p.Count == 2)
.Select(ps =>
{
var edge = new Edge3(ps[0], ps[1]);
var limitedLine = Modeler.CreateTrimmedLine(edge);
var wB = limitedLine.CreateWireBody();
return wB.DisplayUndoable(modelDoc, Color.Blue);
})
.ToCompositeDisposable();
return new CompositeDisposable(interpolated);
});
}
