Skip to content

Commit ed8be8d

Browse files
RiSearcherRiSearcher
committed
Update ConvexPoly.DistanceTo(ConvexPoly).
1 parent 1993a41 commit ed8be8d

File tree

3 files changed

+61
-128
lines changed

3 files changed

+61
-128
lines changed

‎GeometRi.Benchmarks/Program.cs

Lines changed: 3 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -95,7 +95,9 @@ static void Main(string[] args)
9595
{
9696
foreach(ConvexPolyhedron cp2 in list)
9797
{
98-
double dist = cp1.DistanceTo(cp2);
98+
Point3d pp1 = new Point3d();
99+
Point3d pp2 = new Point3d();
100+
double dist = cp1.DistanceTo(cp2, out pp1, out pp2);
99101
}
100102
}
101103
});

‎GeometRi.Tests/ConvexPolyhedronTest.cs

Lines changed: 20 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -349,8 +349,12 @@ public void TetTetDistanceTest_01()
349349
Tetrahedron t2 = new Tetrahedron(new Point3d(0.5, 0, 0), new Point3d(1, -1, -1), new Point3d(1, -1, 1), new Point3d(1, 1, 1));
350350
ConvexPolyhedron cp1 = ConvexPolyhedron.FromTetrahedron(t1);
351351
ConvexPolyhedron cp2 = ConvexPolyhedron.FromTetrahedron(t2);
352-
double dist = cp1.DistanceTo(cp2);
352+
Point3d p1 = new Point3d();
353+
Point3d p2 = new Point3d();
354+
double dist = cp1.DistanceTo(cp2, out p1, out p2);
353355
Assert.IsTrue(GeometRi3D.AlmostEqual(dist, 0.5));
356+
Assert.IsTrue(p1 == new Point3d(0, 0, 0));
357+
Assert.IsTrue(p2 == new Point3d(0.5, 0, 0));
354358
}
355359

356360
[TestMethod]
@@ -361,8 +365,12 @@ public void TetTetDistanceTest_02()
361365
Tetrahedron t2 = new Tetrahedron(new Point3d(0.5, -1, 0), new Point3d(0.5, 1, 0), new Point3d(1, 0, -1), new Point3d(1, 0, 1));
362366
ConvexPolyhedron cp1 = ConvexPolyhedron.FromTetrahedron(t1);
363367
ConvexPolyhedron cp2 = ConvexPolyhedron.FromTetrahedron(t2);
364-
double dist = cp1.DistanceTo(cp2);
368+
Point3d p1 = new Point3d();
369+
Point3d p2 = new Point3d();
370+
double dist = cp1.DistanceTo(cp2, out p1, out p2);
365371
Assert.IsTrue(GeometRi3D.AlmostEqual(dist, 0.5));
372+
Assert.IsTrue(p1 == new Point3d(0, 0, 0));
373+
Assert.IsTrue(p2 == new Point3d(0.5, 0, 0));
366374
}
367375

368376
[TestMethod]
@@ -373,8 +381,12 @@ public void TetTetDistanceTest_03()
373381
Tetrahedron t2 = new Tetrahedron(new Point3d(0.5, -1, -1), new Point3d(0.5, 1, -1), new Point3d(0.5, 0, 1), new Point3d(1, 0, 0));
374382
ConvexPolyhedron cp1 = ConvexPolyhedron.FromTetrahedron(t1);
375383
ConvexPolyhedron cp2 = ConvexPolyhedron.FromTetrahedron(t2);
376-
double dist = cp1.DistanceTo(cp2);
384+
Point3d p1 = new Point3d();
385+
Point3d p2 = new Point3d();
386+
double dist = cp1.DistanceTo(cp2, out p1, out p2);
377387
Assert.IsTrue(GeometRi3D.AlmostEqual(dist, 0.5));
388+
Assert.IsTrue(p1 == new Point3d(0, 0, 0));
389+
Assert.IsTrue(p2 == new Point3d(0.5, 0, 0));
378390
}
379391

380392
[TestMethod]
@@ -385,8 +397,12 @@ public void TetTetDistanceTest_04()
385397
Tetrahedron t2 = new Tetrahedron(new Point3d(0.5, -1, 0), new Point3d(0.5, 1, 0), new Point3d(1, 0, -1), new Point3d(1, 0, 1));
386398
ConvexPolyhedron cp1 = ConvexPolyhedron.FromTetrahedron(t1);
387399
ConvexPolyhedron cp2 = ConvexPolyhedron.FromTetrahedron(t2);
388-
double dist = cp1.DistanceTo(cp2);
400+
Point3d p1 = new Point3d();
401+
Point3d p2 = new Point3d();
402+
double dist = cp1.DistanceTo(cp2, out p1, out p2);
389403
Assert.IsTrue(GeometRi3D.AlmostEqual(dist, 0.5));
404+
Assert.IsTrue(p1 == new Point3d(0, 0, 0));
405+
Assert.IsTrue(p2 == new Point3d(0.5, 0, 0));
390406
}
391407

392408
[TestMethod]

‎GeometRi/ConvexPolyhedron.cs

Lines changed: 38 additions & 123 deletions
Original file line numberDiff line numberDiff line change
@@ -693,21 +693,23 @@ public double DistanceTo(Point3d p)
693693
/// <summary>
694694
/// Distance between two polyhedrons
695695
/// </summary>
696-
public double DistanceTo_old(ConvexPolyhedron cp)
696+
/// <param name="c">Target polyhedron</param>
697+
public double DistanceTo(ConvexPolyhedron c)
697698
{
698-
if (this.Intersects(cp))
699-
{
700-
return 0;
701-
}
702-
Point3d p1, p2;
703-
return _distance_to_not_intersecting(cp, out p1, out p2);
699+
Point3d c1 = new Point3d();
700+
Point3d c2 = new Point3d();
701+
return DistanceTo(c, out c1, out c2);
704702
}
705703

704+
706705
/// <summary>
707706
/// Distance between two polyhedrons
707+
/// <para> The output points are valid only in case of non-intersecting objects.</para>
708708
/// </summary>
709709
/// <param name="c">Target polyhedron</param>
710-
public double DistanceTo(ConvexPolyhedron c)
710+
/// <param name="point_on_this_cp">Closest point on this convex polyhedron</param>
711+
/// <param name="point_on_target_cp">Closest point on target convex polyhedron</param>
712+
public double DistanceTo(ConvexPolyhedron c, out Point3d point_on_this_cp, out Point3d point_on_target_cp)
711713
{
712714
// Use "Method of Separating Axes" to test intersection combined with distance calculation
713715

@@ -717,6 +719,10 @@ public double DistanceTo(ConvexPolyhedron c)
717719

718720
double dist = double.PositiveInfinity;
719721
bool intersecting = true;
722+
Point3d c1 = new Point3d();
723+
Point3d c2 = new Point3d();
724+
point_on_this_cp = c1;
725+
point_on_target_cp = c2;
720726

721727
// Test faces of this CP for separation. Because of the counterclockwise ordering,
722728
// the projection interval for this CP is (-inf, 0].
@@ -732,6 +738,7 @@ public double DistanceTo(ConvexPolyhedron c)
732738
intersecting = false;
733739
double square_proj_dist = double.PositiveInfinity;
734740
Point3d best_proj_point = this.face[i].Vertex[0];
741+
Point3d target_point = this.face[i].Vertex[0];
735742

736743
Plane3d plane = new Plane3d(this.face[i].Vertex[0], this.face[i].normal);
737744
foreach (Point3d point in c.vertex)
@@ -742,6 +749,7 @@ public double DistanceTo(ConvexPolyhedron c)
742749
{
743750
square_proj_dist = tmp_dist;
744751
best_proj_point = projection;
752+
target_point = point;
745753
}
746754
}
747755
// test if best projection of c.vertex is inside the face
@@ -756,7 +764,7 @@ public double DistanceTo(ConvexPolyhedron c)
756764
Vector3d v = new Vector3d(this.face[i].Vertex[l], best_proj_point);
757765
if (edge.Cross(v).Dot(this.face[i].normal) < 0)
758766
{
759-
// projection ourside of face
767+
// projection outside of face
760768
inside = false;
761769
break;
762770
}
@@ -765,7 +773,11 @@ public double DistanceTo(ConvexPolyhedron c)
765773
if (inside)
766774
{
767775
double tmp_dist = Math.Sqrt(square_proj_dist);
768-
if (tmp_dist < dist) { dist = tmp_dist; }
776+
if (tmp_dist < dist) {
777+
dist = tmp_dist;
778+
point_on_this_cp = best_proj_point;
779+
point_on_target_cp = target_point;
780+
}
769781
}
770782
}
771783
}
@@ -784,6 +796,7 @@ public double DistanceTo(ConvexPolyhedron c)
784796
intersecting = false;
785797
double square_proj_dist = double.PositiveInfinity;
786798
Point3d best_proj_point = c.face[i].Vertex[0];
799+
Point3d target_point = c.face[i].Vertex[0];
787800

788801
Plane3d plane = new Plane3d(c.face[i].Vertex[0], c.face[i].normal);
789802
foreach (Point3d point in this.vertex)
@@ -794,6 +807,7 @@ public double DistanceTo(ConvexPolyhedron c)
794807
{
795808
square_proj_dist = tmp_dist;
796809
best_proj_point = projection;
810+
target_point = point;
797811
}
798812
}
799813
// test if best projection of c.vertex is inside the face
@@ -808,7 +822,7 @@ public double DistanceTo(ConvexPolyhedron c)
808822
Vector3d v = new Vector3d(c.face[i].Vertex[l], best_proj_point);
809823
if (edge.Cross(v).Dot(c.face[i].normal) < 0)
810824
{
811-
// projection ourside of face
825+
// projection outside of face
812826
inside = false;
813827
break;
814828
}
@@ -817,7 +831,12 @@ public double DistanceTo(ConvexPolyhedron c)
817831
if (inside)
818832
{
819833
double tmp_dist = Math.Sqrt(square_proj_dist);
820-
if (tmp_dist < dist) { dist = tmp_dist; }
834+
if (tmp_dist < dist)
835+
{
836+
dist = tmp_dist;
837+
point_on_this_cp = best_proj_point;
838+
point_on_target_cp = target_point;
839+
}
821840
}
822841

823842
}
@@ -854,8 +873,13 @@ public double DistanceTo(ConvexPolyhedron c)
854873
{
855874
// The projections of this CP and 'c' onto the line P + t * N are on opposite sides of the projection of P.
856875
intersecting = false;
857-
double tmp_dist = s1.DistanceTo(s2);
858-
if (tmp_dist < dist) { dist = tmp_dist; }
876+
double tmp_dist = s1.DistanceTo(s2, out c1, out c2);
877+
if (tmp_dist < dist)
878+
{
879+
dist = tmp_dist;
880+
point_on_this_cp = c1;
881+
point_on_target_cp = c2;
882+
}
859883
}
860884
}
861885
}
@@ -871,115 +895,6 @@ public double DistanceTo(ConvexPolyhedron c)
871895
}
872896
}
873897

874-
/// <summary>
875-
/// Distance between two polyhedrons
876-
/// <para> The output points may be not unique in case of intersecting objects.</para>
877-
/// </summary>
878-
/// <param name="cp">Target polyhedron</param>
879-
/// <param name="point_on_this_cp">Closest point on this convex polyhedron</param>
880-
/// <param name="point_on_target_cp">Closest point on target convex polyhedron</param>
881-
public double DistanceTo(ConvexPolyhedron cp, out Point3d point_on_this_cp, out Point3d point_on_target_cp)
882-
{
883-
if (this.Intersects(cp))
884-
{
885-
// check if one CP is inside other CP
886-
Point3d c1 = this.Center;
887-
point_on_this_cp = c1;
888-
point_on_target_cp = c1;
889-
if (c1.BelongsTo(cp))
890-
{
891-
return 0;
892-
}
893-
Point3d c2 = cp.Center;
894-
if (c2.BelongsTo(this))
895-
{
896-
point_on_this_cp = c2;
897-
point_on_target_cp = c2;
898-
return 0;
899-
}
900-
901-
// If CPs are partially intersecting, return any common point
902-
Point3d common_point = _get_common_point(cp);
903-
if (common_point != null)
904-
{
905-
point_on_this_cp = common_point;
906-
point_on_target_cp = common_point;
907-
return 0;
908-
}
909-
}
910-
911-
return _distance_to_not_intersecting(cp, out point_on_this_cp, out point_on_target_cp);
912-
}
913-
914-
internal double _distance_to_not_intersecting(ConvexPolyhedron cp, out Point3d point_on_this_cp, out Point3d point_on_target_cp)
915-
{
916-
double dist = double.PositiveInfinity;
917-
Point3d c1 = new Point3d();
918-
Point3d c2 = new Point3d();
919-
point_on_this_cp = c1;
920-
point_on_target_cp = c1;
921-
922-
// test vertices of this cp
923-
for (int i = 0; i < numVertices; i++)
924-
{
925-
for (int k = 0; k < cp.numFaces; k++)
926-
{
927-
for (int l = 0; l < cp.face[k].vertex.Length - 2; l++)
928-
{
929-
Triangle t2 = new Triangle(cp.face[k].Vertex[0], cp.face[k].Vertex[l + 1], cp.face[k].Vertex[l + 2]);
930-
931-
double tmp_dist = vertex[i].DistanceTo(t2, out c1);
932-
if (tmp_dist < dist)
933-
{
934-
point_on_this_cp = vertex[i];
935-
point_on_target_cp = c1;
936-
dist = tmp_dist;
937-
}
938-
}
939-
940-
}
941-
}
942-
943-
// test vertices of target cp
944-
for (int i = 0; i < cp.numVertices; i++)
945-
{
946-
for (int k = 0; k < this.numFaces; k++)
947-
{
948-
for (int l = 0; l < this.face[k].vertex.Length - 2; l++)
949-
{
950-
Triangle t2 = new Triangle(this.face[k].Vertex[0], this.face[k].Vertex[l + 1], this.face[k].Vertex[l + 2]);
951-
952-
double tmp_dist = cp.vertex[i].DistanceTo(t2, out c1);
953-
if (tmp_dist < dist)
954-
{
955-
point_on_this_cp = c1;
956-
point_on_target_cp = cp.vertex[i];
957-
dist = tmp_dist;
958-
}
959-
}
960-
961-
}
962-
}
963-
964-
// test edges
965-
for (int i = 0; i < this.numEdges; i++)
966-
{
967-
Segment3d s1 = new Segment3d(this.vertex[this.edge[i].p1], this.vertex[this.edge[i].p2]);
968-
for (int j = 0; j < cp.numEdges; j++)
969-
{
970-
Segment3d s2 = new Segment3d(cp.vertex[cp.edge[j].p1], cp.vertex[cp.edge[j].p2]);
971-
double tmp_dist = s1.DistanceTo(s2, out c1, out c2);
972-
if (tmp_dist < dist)
973-
{
974-
point_on_this_cp = c1;
975-
point_on_target_cp = c2;
976-
dist = tmp_dist;
977-
}
978-
}
979-
}
980-
981-
return dist;
982-
}
983898

984899
internal Point3d _get_common_point(ConvexPolyhedron cp)
985900
{

0 commit comments

Comments
 (0)