From 4e01070736b34f7f3eca9106dc3fa22dccf6ff5f Mon Sep 17 00:00:00 2001 From: Peter LoVerso Date: Fri, 20 Dec 2024 11:44:13 -0800 Subject: [PATCH 1/8] Make mesh/triangle public. Add assorted geometry helper functions for triangles. --- spatialmath/box.go | 10 +- spatialmath/capsule.go | 4 +- spatialmath/geometry_utils.go | 4 +- spatialmath/mesh.go | 116 ++++---------------- spatialmath/mesh_test.go | 8 +- spatialmath/triangle.go | 197 ++++++++++++++++++++++++++++++++++ spatialmath/triangle_test.go | 157 +++++++++++++++++++++++++++ 7 files changed, 387 insertions(+), 109 deletions(-) create mode 100644 spatialmath/triangle.go create mode 100644 spatialmath/triangle_test.go diff --git a/spatialmath/box.go b/spatialmath/box.go index 9a56662c192..ef5b8a99d57 100644 --- a/spatialmath/box.go +++ b/spatialmath/box.go @@ -56,7 +56,7 @@ type box struct { halfSize [3]float64 boundingSphereR float64 label string - mesh *mesh + mesh *Mesh rotMatrix *RotationMatrix once sync.Once } @@ -241,13 +241,13 @@ func (b *box) vertices() []r3.Vector { } // vertices returns the vertices defining the box. -func (b *box) toMesh() *mesh { +func (b *box) toMesh() *Mesh { if b.mesh == nil { - m := &mesh{pose: b.pose} - triangles := make([]*triangle, 0, 12) + m := &Mesh{pose: b.pose} + triangles := make([]*Triangle, 0, 12) verts := b.vertices() for _, tri := range boxTriangles { - triangles = append(triangles, newTriangle(verts[tri[0]], verts[tri[1]], verts[tri[2]])) + triangles = append(triangles, NewTriangle(verts[tri[0]], verts[tri[1]], verts[tri[2]])) } m.triangles = triangles b.mesh = m diff --git a/spatialmath/capsule.go b/spatialmath/capsule.go index 9c88e6d7207..0b7d274dc48 100644 --- a/spatialmath/capsule.go +++ b/spatialmath/capsule.go @@ -261,7 +261,7 @@ func capsuleVsBoxDistance(c *capsule, other *box) float64 { // IMPORTANT: meshes are not considered solid. A mesh is not guaranteed to represent an enclosed area. This will measure ONLY the distance // to the closest triangle in the mesh. -func capsuleVsMeshDistance(c *capsule, other *mesh) float64 { +func capsuleVsMeshDistance(c *capsule, other *Mesh) float64 { lowDist := math.Inf(1) for _, t := range other.triangles { // Measure distance to each mesh triangle @@ -273,7 +273,7 @@ func capsuleVsMeshDistance(c *capsule, other *mesh) float64 { return lowDist } -func capsuleVsTriangleDistance(c *capsule, other *triangle) float64 { +func capsuleVsTriangleDistance(c *capsule, other *Triangle) float64 { capPt, triPt := closestPointsSegmentTriangle(c.segA, c.segB, other) return capPt.Sub(triPt).Norm() - c.radius } diff --git a/spatialmath/geometry_utils.go b/spatialmath/geometry_utils.go index 5dcbfad6a6d..66f40315b1e 100644 --- a/spatialmath/geometry_utils.go +++ b/spatialmath/geometry_utils.go @@ -100,7 +100,7 @@ func BoundingSphere(geometry Geometry) (Geometry, error) { } // closestSegmentTrianglePoints takes a line segment and a triangle, and returns the point on each closest to the other. -func closestPointsSegmentTriangle(ap1, ap2 r3.Vector, t *triangle) (bestSegPt, bestTriPt r3.Vector) { +func closestPointsSegmentTriangle(ap1, ap2 r3.Vector, t *Triangle) (bestSegPt, bestTriPt r3.Vector) { // The closest triangle point is either on the edge or within the triangle. // First, handle the case where the closest triangle point is inside the @@ -109,7 +109,7 @@ func closestPointsSegmentTriangle(ap1, ap2 r3.Vector, t *triangle) (bestSegPt, b // If the line overlaps the triangle and is parallel to the triangle plane, // the chosen triangle point is arbitrary. segPt, _ := closestPointsSegmentPlane(ap1, ap2, t.p0, t.normal) - triPt, inside := t.closestInsidePoint(segPt) + triPt, inside := t.ClosestInsidePoint(segPt) if inside { // If inside is false, then these will not be the best points, because they are based on the segment-plane intersection return segPt, triPt diff --git a/spatialmath/mesh.go b/spatialmath/mesh.go index bcb46c10fa0..bf45f4a4314 100644 --- a/spatialmath/mesh.go +++ b/spatialmath/mesh.go @@ -1,115 +1,39 @@ package spatialmath -import ( - "github.com/golang/geo/r3" -) + +//~ import ( +//~ "github.com/golang/geo/r3" +//~ ) // This file incorporates work covered by the Brax project -- https://github.com/google/brax/blob/main/LICENSE. // Copyright 2021 The Brax Authors, which is licensed under the Apache License Version 2.0 (the “License”). // You may obtain a copy of the license at http://www.apache.org/licenses/LICENSE-2.0. // mesh is a collision geometry that represents a set of triangles that represent a mesh. -type mesh struct { +type Mesh struct { pose Pose - triangles []*triangle + triangles []*Triangle } -type triangle struct { - p0 r3.Vector - p1 r3.Vector - p2 r3.Vector - - normal r3.Vector -} - -func newTriangle(p0, p1, p2 r3.Vector) *triangle { - return &triangle{ - p0: p0, - p1: p1, - p2: p2, - normal: PlaneNormal(p0, p1, p2), +func NewMesh(pose Pose, triangles []*Triangle) *Mesh { + return &Mesh{ + pose: pose, + triangles: triangles, } } -// closestPointToCoplanarPoint takes a point, and returns the closest point on the triangle to the given point -// The given point *MUST* be coplanar with the triangle. If it is known ahead of time that the point is coplanar, this is faster. -func (t *triangle) closestPointToCoplanarPoint(pt r3.Vector) r3.Vector { - // Determine whether point is inside all triangle edges: - c0 := pt.Sub(t.p0).Cross(t.p1.Sub(t.p0)) - c1 := pt.Sub(t.p1).Cross(t.p2.Sub(t.p1)) - c2 := pt.Sub(t.p2).Cross(t.p0.Sub(t.p2)) - inside := c0.Dot(t.normal) <= 0 && c1.Dot(t.normal) <= 0 && c2.Dot(t.normal) <= 0 - - if inside { - return pt - } - - // Edge 1: - refPt := ClosestPointSegmentPoint(t.p0, t.p1, pt) - bestDist := pt.Sub(refPt).Norm2() - - // Edge 2: - point2 := ClosestPointSegmentPoint(t.p1, t.p2, pt) - if distsq := pt.Sub(point2).Norm2(); distsq < bestDist { - refPt = point2 - bestDist = distsq - } - - // Edge 3: - point3 := ClosestPointSegmentPoint(t.p2, t.p0, pt) - if distsq := pt.Sub(point3).Norm2(); distsq < bestDist { - return point3 - } - return refPt +func (m *Mesh) Pose() Pose { + return m.pose } -// closestPointToPoint takes a point, and returns the closest point on the triangle to the given point, as well as whether the point -// is on the edge of the triangle. -// This is slower than closestPointToCoplanarPoint. -func (t *triangle) closestPointToPoint(point r3.Vector) r3.Vector { - closestPtInside, inside := t.closestInsidePoint(point) - if inside { - return closestPtInside - } - - // If the closest point is outside the triangle, it must be on an edge, so we - // check each triangle edge for a closest point to the point pt. - closestPt := ClosestPointSegmentPoint(t.p0, t.p1, point) - bestDist := point.Sub(closestPt).Norm2() - - newPt := ClosestPointSegmentPoint(t.p1, t.p2, point) - if newDist := point.Sub(newPt).Norm2(); newDist < bestDist { - closestPt = newPt - bestDist = newDist - } - - newPt = ClosestPointSegmentPoint(t.p2, t.p0, point) - if newDist := point.Sub(newPt).Norm2(); newDist < bestDist { - return newPt - } - return closestPt +func (m *Mesh) Triangles() []*Triangle { + return m.triangles } -// closestInsidePoint returns the closest point on a triangle IF AND ONLY IF the query point's projection overlaps the triangle. -// Otherwise it will return the query point. -// To visualize this- if one draws a tetrahedron using the triangle and the query point, all angles from the triangle to the query point -// must be <= 90 degrees. -func (t *triangle) closestInsidePoint(point r3.Vector) (r3.Vector, bool) { - // Parametrize the triangle s.t. a point inside the triangle is - // Q = p0 + u * e0 + v * e1, when 0 <= u <= 1, 0 <= v <= 1, and - // 0 <= u + v <= 1. Let e0 = (p1 - p0) and e1 = (p2 - p0). - // We analytically minimize the distance between the point pt and Q. - e0 := t.p1.Sub(t.p0) - e1 := t.p2.Sub(t.p0) - a := e0.Norm2() - b := e0.Dot(e1) - c := e1.Norm2() - d := point.Sub(t.p0) - // The determinant is 0 only if the angle between e1 and e0 is 0 - // (i.e. the triangle has overlapping lines). - det := (a*c - b*b) - u := (c*e0.Dot(d) - b*e1.Dot(d)) / det - v := (-b*e0.Dot(d) + a*e1.Dot(d)) / det - inside := (0 <= u) && (u <= 1) && (0 <= v) && (v <= 1) && (u+v <= 1) - return t.p0.Add(e0.Mul(u)).Add(e1.Mul(v)), inside +func (m *Mesh) Transform(pose Pose) *Mesh { + // Triangle points are in frame of mesh, like the corners of a box, so no need to transform them + return &Mesh{ + pose: Compose(pose, m.pose), + triangles: m.triangles, + } } diff --git a/spatialmath/mesh_test.go b/spatialmath/mesh_test.go index 4dd30946eec..bfc3570c294 100644 --- a/spatialmath/mesh_test.go +++ b/spatialmath/mesh_test.go @@ -11,12 +11,12 @@ func TestClosestPoint(t *testing.T) { p0 := r3.Vector{0, 0, 0} p1 := r3.Vector{1, 0, 0} p2 := r3.Vector{0, 0, 2} - tri := newTriangle(p0, p1, p2) + tri := NewTriangle(p0, p1, p2) qp1 := r3.Vector{-1, 0, 0} - cp1 := tri.closestPointToCoplanarPoint(qp1) - cp2 := tri.closestPointToPoint(qp1) - cp3, inside := tri.closestInsidePoint(qp1) + cp1 := tri.ClosestPointToCoplanarPoint(qp1) + cp2 := tri.ClosestPointToPoint(qp1) + cp3, inside := tri.ClosestInsidePoint(qp1) test.That(t, inside, test.ShouldBeFalse) test.That(t, cp3.ApproxEqual(qp1), test.ShouldBeTrue) test.That(t, cp1.ApproxEqual(cp2), test.ShouldBeTrue) diff --git a/spatialmath/triangle.go b/spatialmath/triangle.go new file mode 100644 index 00000000000..f9554b6cb91 --- /dev/null +++ b/spatialmath/triangle.go @@ -0,0 +1,197 @@ +package spatialmath + +import ( + "math" + + "github.com/golang/geo/r3" +) + +type Triangle struct { + p0 r3.Vector + p1 r3.Vector + p2 r3.Vector + + normal r3.Vector +} + +func NewTriangle(p0, p1, p2 r3.Vector) *Triangle { + return &Triangle{ + p0: p0, + p1: p1, + p2: p2, + normal: PlaneNormal(p0, p1, p2), + } +} + +// closestPointToCoplanarPoint takes a point, and returns the closest point on the triangle to the given point +// The given point *MUST* be coplanar with the triangle. If it is known ahead of time that the point is coplanar, this is faster. +func (t *Triangle) ClosestPointToCoplanarPoint(pt r3.Vector) r3.Vector { + // Determine whether point is inside all triangle edges: + c0 := pt.Sub(t.p0).Cross(t.p1.Sub(t.p0)) + c1 := pt.Sub(t.p1).Cross(t.p2.Sub(t.p1)) + c2 := pt.Sub(t.p2).Cross(t.p0.Sub(t.p2)) + inside := c0.Dot(t.normal) <= 0 && c1.Dot(t.normal) <= 0 && c2.Dot(t.normal) <= 0 + + if inside { + return pt + } + + // Edge 1: + refPt := ClosestPointSegmentPoint(t.p0, t.p1, pt) + bestDist := pt.Sub(refPt).Norm2() + + // Edge 2: + point2 := ClosestPointSegmentPoint(t.p1, t.p2, pt) + if distsq := pt.Sub(point2).Norm2(); distsq < bestDist { + refPt = point2 + bestDist = distsq + } + + // Edge 3: + point3 := ClosestPointSegmentPoint(t.p2, t.p0, pt) + if distsq := pt.Sub(point3).Norm2(); distsq < bestDist { + return point3 + } + return refPt +} + +// closestPointToPoint takes a point, and returns the closest point on the triangle to the given point. +// This is slower than closestPointToCoplanarPoint. +func (t *Triangle) ClosestPointToPoint(point r3.Vector) r3.Vector { + closestPtInside, inside := t.ClosestInsidePoint(point) + if inside { + return closestPtInside + } + + // If the closest point is outside the triangle, it must be on an edge, so we + // check each triangle edge for a closest point to the point pt. + closestPt := ClosestPointSegmentPoint(t.p0, t.p1, point) + bestDist := point.Sub(closestPt).Norm2() + + newPt := ClosestPointSegmentPoint(t.p1, t.p2, point) + if newDist := point.Sub(newPt).Norm2(); newDist < bestDist { + closestPt = newPt + bestDist = newDist + } + + newPt = ClosestPointSegmentPoint(t.p2, t.p0, point) + if newDist := point.Sub(newPt).Norm2(); newDist < bestDist { + return newPt + } + return closestPt +} + +// closestInsidePoint returns the closest point on a triangle IF AND ONLY IF the query point's projection overlaps the triangle. +// Otherwise it will return the query point. +// To visualize this- if one draws a tetrahedron using the triangle and the query point, all angles from the triangle to the query point +// must be <= 90 degrees. +func (t *Triangle) ClosestInsidePoint(point r3.Vector) (r3.Vector, bool) { + eps := 1e-6 + + // Parametrize the triangle s.t. a point inside the triangle is + // Q = p0 + u * e0 + v * e1, when 0 <= u <= 1, 0 <= v <= 1, and + // 0 <= u + v <= 1. Let e0 = (p1 - p0) and e1 = (p2 - p0). + // We analytically minimize the distance between the point pt and Q. + e0 := t.p1.Sub(t.p0) + e1 := t.p2.Sub(t.p0) + a := e0.Norm2() + b := e0.Dot(e1) + c := e1.Norm2() + d := point.Sub(t.p0) + // The determinant is 0 only if the angle between e1 and e0 is 0 + // (i.e. the triangle has overlapping lines). + det := (a*c - b*b) + u := (c*e0.Dot(d) - b*e1.Dot(d)) / det + v := (-b*e0.Dot(d) + a*e1.Dot(d)) / det + inside := (0 <= u+eps) && (u <= 1+eps) && (0 <= v+eps) && (v <= 1+eps) && (u+v <= 1+eps) + return t.p0.Add(e0.Mul(u)).Add(e1.Mul(v)), inside +} + +func (t *Triangle) Points() []r3.Vector { + return []r3.Vector{t.p0, t.p1, t.p2} +} + +func (t *Triangle) Normal() r3.Vector { + return t.normal +} + +// IntersectsPlane determines if the triangle intersects with a plane defined by a point and normal vector. +// Returns true if the triangle intersects with or lies on the plane. +func (t *Triangle) IntersectsPlane(planePt, planeNormal r3.Vector) bool { + // Calculate signed distances from each triangle vertex to the plane + d0 := planeNormal.Dot(t.p0.Sub(planePt)) + d1 := planeNormal.Dot(t.p1.Sub(planePt)) + d2 := planeNormal.Dot(t.p2.Sub(planePt)) + + // If all points are on the same side of the plane (all distances positive or all negative), + // there is no intersection + if (d0 > floatEpsilon && d1 > floatEpsilon && d2 > floatEpsilon) || + (d0 < -floatEpsilon && d1 < -floatEpsilon && d2 < -floatEpsilon) { + return false + } + + // If any point is very close to the plane or points lie on different sides, + // there is an intersection + return true +} + +// TrianglePlaneIntersectingSegment determines the line segment where a triangle intersects with a plane. +// Returns the two points defining the intersection line segment and whether an intersection exists. +// If the triangle only touches the plane at a point, both returned points will be the same. +// If the triangle lies in the plane, it returns two points representing the longest edge of the triangle. +func (t *Triangle) TrianglePlaneIntersectingSegment(planePt, planeNormal r3.Vector) (r3.Vector, r3.Vector, bool) { + // First check if there's an intersection + if !t.IntersectsPlane(planePt, planeNormal) { + return r3.Vector{}, r3.Vector{}, false + } + + // Calculate signed distances from each vertex to the plane + d0 := planeNormal.Dot(t.p0.Sub(planePt)) + d1 := planeNormal.Dot(t.p1.Sub(planePt)) + d2 := planeNormal.Dot(t.p2.Sub(planePt)) + + // If triangle lies in plane (all distances are approximately zero) + if math.Abs(d0) < floatEpsilon && math.Abs(d1) < floatEpsilon && math.Abs(d2) < floatEpsilon { + // Return the longest edge of the triangle + e1 := t.p1.Sub(t.p0).Norm2() + e2 := t.p2.Sub(t.p1).Norm2() + e3 := t.p0.Sub(t.p2).Norm2() + if e1 >= e2 && e1 >= e3 { + return t.p0, t.p1, true + } else if e2 >= e1 && e2 >= e3 { + return t.p1, t.p2, true + } + return t.p2, t.p0, true + } + + // Find the two edges that intersect with the plane + var intersections []r3.Vector + edges := [][2]r3.Vector{ + {t.p0, t.p1}, + {t.p1, t.p2}, + {t.p2, t.p0}, + } + dists := []float64{d0, d1, d2} + + for i := 0; i < 3; i++ { + j := (i + 1) % 3 + // If edge crosses plane (distances have different signs) + if (dists[i] * dists[j]) < 0 { + // Calculate intersection point using linear interpolation + t := dists[i] / (dists[i] - dists[j]) + edge := edges[i] + intersection := edge[0].Add(edge[1].Sub(edge[0]).Mul(t)) + intersections = append(intersections, intersection) + } else if math.Abs(dists[i]) < floatEpsilon { + // Vertex lies on plane + intersections = append(intersections, edges[i][0]) + } + } + + // We should have exactly two intersection points + if len(intersections) < 2 { + // Handle degenerate case where triangle only touches plane at a point + return intersections[0], intersections[0], true + } + return intersections[0], intersections[1], true +} diff --git a/spatialmath/triangle_test.go b/spatialmath/triangle_test.go new file mode 100644 index 00000000000..e483b71ad4a --- /dev/null +++ b/spatialmath/triangle_test.go @@ -0,0 +1,157 @@ +package spatialmath + +import ( + "testing" + + "github.com/golang/geo/r3" + "go.viam.com/test" +) + +func TestTriangleIntersectsPlane(t *testing.T) { + tests := []struct { + name string + triangle *Triangle + planePt r3.Vector + planeNormal r3.Vector + want bool + }{ + { + name: "triangle intersects plane", + triangle: NewTriangle( + r3.Vector{X: -1, Y: 0, Z: -1}, + r3.Vector{X: 1, Y: 0, Z: -1}, + r3.Vector{X: 0, Y: 0, Z: 1}, + ), + planePt: r3.Vector{X: 0, Y: 0, Z: 0}, + planeNormal: r3.Vector{X: 0, Y: 0, Z: 1}, + want: true, + }, + { + name: "triangle above plane", + triangle: NewTriangle( + r3.Vector{X: 0, Y: 0, Z: 1}, + r3.Vector{X: 1, Y: 0, Z: 1}, + r3.Vector{X: 0, Y: 1, Z: 1}, + ), + planePt: r3.Vector{X: 0, Y: 0, Z: 0}, + planeNormal: r3.Vector{X: 0, Y: 0, Z: 1}, + want: false, + }, + { + name: "triangle below plane", + triangle: NewTriangle( + r3.Vector{X: 0, Y: 0, Z: -1}, + r3.Vector{X: 1, Y: 0, Z: -1}, + r3.Vector{X: 0, Y: 1, Z: -1}, + ), + planePt: r3.Vector{X: 0, Y: 0, Z: 0}, + planeNormal: r3.Vector{X: 0, Y: 0, Z: 1}, + want: false, + }, + { + name: "triangle lies in plane", + triangle: NewTriangle( + r3.Vector{X: 0, Y: 0, Z: 0}, + r3.Vector{X: 1, Y: 0, Z: 0}, + r3.Vector{X: 0, Y: 1, Z: 0}, + ), + planePt: r3.Vector{X: 0, Y: 0, Z: 0}, + planeNormal: r3.Vector{X: 0, Y: 0, Z: 1}, + want: true, + }, + { + name: "triangle touches plane at vertex", + triangle: NewTriangle( + r3.Vector{X: 0, Y: 0, Z: 0}, + r3.Vector{X: 1, Y: 0, Z: 1}, + r3.Vector{X: 0, Y: 1, Z: 1}, + ), + planePt: r3.Vector{X: 0, Y: 0, Z: 0}, + planeNormal: r3.Vector{X: 0, Y: 0, Z: 1}, + want: true, + }, + } + + for _, tt := range tests { + t.Run(tt.name, func(t *testing.T) { + got := tt.triangle.IntersectsPlane(tt.planePt, tt.planeNormal) + test.That(t, tt.want, test.ShouldEqual, got) + }) + } +} + +func TestTrianglePlaneIntersectingSegment(t *testing.T) { + tests := []struct { + name string + triangle *Triangle + planePt r3.Vector + planeNormal r3.Vector + wantP1 r3.Vector + wantP2 r3.Vector + wantExists bool + }{ + { + name: "triangle intersects plane - simple case", + triangle: NewTriangle( + r3.Vector{X: -1, Y: 0, Z: -1}, + r3.Vector{X: 1, Y: 0, Z: -1}, + r3.Vector{X: 0, Y: 0, Z: 1}, + ), + planePt: r3.Vector{X: 0, Y: 0, Z: 0}, + planeNormal: r3.Vector{X: 0, Y: 0, Z: 1}, + wantP1: r3.Vector{X: 0.5, Y: 0, Z: 0}, + wantP2: r3.Vector{X: -0.5, Y: 0, Z: 0}, + wantExists: true, + }, + { + name: "triangle lies in plane", + triangle: NewTriangle( + r3.Vector{X: 0, Y: 0, Z: 0}, + r3.Vector{X: 1, Y: 0, Z: 0}, + r3.Vector{X: 0, Y: 1, Z: 0}, + ), + planePt: r3.Vector{X: 0, Y: 0, Z: 0}, + planeNormal: r3.Vector{X: 0, Y: 0, Z: 1}, + wantP1: r3.Vector{X: 1, Y: 0, Z: 0}, + wantP2: r3.Vector{X: 0, Y: 1, Z: 0}, + wantExists: true, + }, + { + name: "no intersection", + triangle: NewTriangle( + r3.Vector{X: 0, Y: 0, Z: 1}, + r3.Vector{X: 1, Y: 0, Z: 1}, + r3.Vector{X: 0, Y: 1, Z: 1}, + ), + planePt: r3.Vector{X: 0, Y: 0, Z: 0}, + planeNormal: r3.Vector{X: 0, Y: 0, Z: 1}, + wantP1: r3.Vector{}, + wantP2: r3.Vector{}, + wantExists: false, + }, + { + name: "triangle touches plane at vertex", + triangle: NewTriangle( + r3.Vector{X: 0, Y: 0, Z: 0}, + r3.Vector{X: 1, Y: 0, Z: 1}, + r3.Vector{X: 0, Y: 1, Z: 1}, + ), + planePt: r3.Vector{X: 0, Y: 0, Z: 0}, + planeNormal: r3.Vector{X: 0, Y: 0, Z: 1}, + wantP1: r3.Vector{X: 0, Y: 0, Z: 0}, + wantP2: r3.Vector{X: 0, Y: 0, Z: 0}, + wantExists: true, + }, + } + + for _, tt := range tests { + t.Run(tt.name, func(t *testing.T) { + gotP1, gotP2, gotExists := tt.triangle.TrianglePlaneIntersectingSegment(tt.planePt, tt.planeNormal) + test.That(t, tt.wantExists, test.ShouldEqual, gotExists) + if tt.wantExists { + test.That(t, tt.wantP1.ApproxEqual(gotP1), test.ShouldBeTrue) + test.That(t, tt.wantP2.ApproxEqual(gotP2), test.ShouldBeTrue) + } + }) + } +} From f8c68efbfff3bad0f50240053e0bd7d060f416e0 Mon Sep 17 00:00:00 2001 From: Peter LoVerso Date: Mon, 30 Dec 2024 08:20:56 -0800 Subject: [PATCH 2/8] Add area coverage --- motionplan/coverage/coverage.go | 255 +++++++++++++++++++++++++++ motionplan/coverage/coverage_test.go | 36 ++++ motionplan/motionPlanner.go | 12 ++ motionplan/planManager.go | 38 ++-- services/motion/builtin/builtin.go | 1 + 5 files changed, 316 insertions(+), 26 deletions(-) create mode 100644 motionplan/coverage/coverage.go create mode 100644 motionplan/coverage/coverage_test.go diff --git a/motionplan/coverage/coverage.go b/motionplan/coverage/coverage.go new file mode 100644 index 00000000000..c37e5bec0a3 --- /dev/null +++ b/motionplan/coverage/coverage.go @@ -0,0 +1,255 @@ +package coverage + +import ( + "bufio" + "errors" + "os" + "math" + "sort" + "fmt" + //~ "context" + //~ "errors" + + //~ "go.viam.com/rdk/motionplan/ik" + "go.viam.com/rdk/referenceframe" + "go.viam.com/rdk/spatialmath" + + "github.com/chenzhekl/goply" + "github.com/golang/geo/r3" +) + +type plane struct { + pt, normal r3.Vector +} + +func ReadPLY(path string) (*spatialmath.Mesh, error) { + readerRaw, err := os.Open(path) + if err != nil { + return nil, err + } + reader := bufio.NewReader(readerRaw) + ply := goply.New(reader) + vertices := ply.Elements("vertex") + faces := ply.Elements("face") + triangles := []*spatialmath.Triangle{} + for _, face := range faces { + + pts := []r3.Vector{} + idxIface := face["vertex_indices"] + for _, i := range idxIface.([]interface{}) { + pts = append(pts, r3.Vector{ + 1000*vertices[int(i.(uint32))]["x"].(float64), + 1000*vertices[int(i.(uint32))]["y"].(float64), + 1000*vertices[int(i.(uint32))]["z"].(float64)}) + } + if len(pts) != 3 { + return nil, errors.New("triangle did not have three points") + } + tri := spatialmath.NewTriangle(pts[0], pts[1], pts[2]) + triangles = append(triangles, tri) + } + return spatialmath.NewMesh(spatialmath.NewZeroPose(), triangles), nil +} + +// Naive algorithm to find waypoints on a mesh +func MeshWaypoints( + mesh *spatialmath.Mesh, + step, trim float64, // Step is how far apart the up/down strokes will be. Trim will stay that far back from the edges of the mesh + source spatialmath.Pose, // Used to determine which side of mesh to write on +) ([]spatialmath.Pose, error) { + meshNorm := calcMeshNormal(mesh) + normPose := spatialmath.NewPoseFromOrientation(&spatialmath.OrientationVector{OX: meshNorm.X, OY: meshNorm.Y, OZ: meshNorm.Z}) + if step <= 0 { + return nil, errors.New("step must be >0") + } + + // Rotate mesh to project onto X-Y plane, and determine limits of rotated mesh + xLim := referenceframe.Limit{Min: math.Inf(1), Max: math.Inf(-1)} + yLim := referenceframe.Limit{Min: math.Inf(1), Max: math.Inf(-1)} + rotTriangles := []*spatialmath.Triangle{} + vMap := map[r3.Vector][]*spatialmath.Triangle{} + for _, t := range mesh.Triangles() { + pts := []r3.Vector{} + for _, pt := range t.Points() { + rotPt := spatialmath.Compose(spatialmath.PoseInverse(normPose), spatialmath.NewPoseFromPoint(pt)).Point() + if rotPt.Y > yLim.Max { + yLim.Max = rotPt.Y + } + if rotPt.Y < yLim.Min { + yLim.Min = rotPt.Y + } + if rotPt.X > xLim.Max { + xLim.Max = rotPt.X + } + if rotPt.X < xLim.Min { + xLim.Min = rotPt.X + } + pts = append(pts, rotPt) + } + tri := spatialmath.NewTriangle(pts[0], pts[1], pts[2]) + vMap[pts[0]] = append(vMap[pts[0]], tri) + vMap[pts[1]] = append(vMap[pts[1]], tri) + vMap[pts[2]] = append(vMap[pts[2]], tri) + rotTriangles = append(rotTriangles, tri) + } + strokes := [][]spatialmath.Pose{} + for xVal := xLim.Min + step + trim ; xVal < xLim.Max - trim; xVal += step { + stroke := planePoses( + rotTriangles, + plane{pt: r3.Vector{xVal,0,0}, normal: r3.Vector{1,0,0}}, + source, + ) + // Sort by Y value + sort.Slice(stroke, func(i, j int) bool { + return stroke[i].Point().Y > stroke[j].Point().Y + }) + strokes = append(strokes, stroke) + } + // Invert every other stroke to make a contiguous zigzag + finalPath := []spatialmath.Pose{} + flip := false + // Un-rotate poses back to original mesh position + fmt.Println("got ", len(strokes), " strokes") + for _, stroke := range strokes { + if flip { + for i := len(stroke)-1; i >= 0; i-- { + if stroke[i].Point().Y > xLim.Max - trim || stroke[i].Point().Y < xLim.Min + trim { + continue + } + finalPath = append(finalPath, spatialmath.Compose(normPose, stroke[i])) + } + } else { + for _, pose := range stroke { + if pose.Point().Y > xLim.Max - trim || pose.Point().Y < xLim.Min + trim { + continue + } + finalPath = append(finalPath, spatialmath.Compose(normPose, pose)) + } + } + flip = !flip + } + + return finalPath, nil +} + +func planePoses(triangles []*spatialmath.Triangle, p plane, approachPose spatialmath.Pose) []spatialmath.Pose { + normalPoses := []spatialmath.Pose{} + approachVec := approachPose.Orientation().OrientationVectorRadians().Vector() + + for _, t := range triangles { + tp1, tp2, intersects := t.TrianglePlaneIntersectingSegment(p.pt, p.normal) + if !intersects || tp1.ApproxEqual(tp2) { + continue + } + // There are two normals to each triangle; get both and use the one closest to the approach orientation + triVec := t.Normal() + dist := approachVec.Sub(triVec).Norm2() + tNormInv := triVec.Mul(-1) + if approachVec.Sub(tNormInv).Norm2() < dist { + triVec = tNormInv + } + approachOrient := &spatialmath.OrientationVector{OX: triVec.X, OY: triVec.Y, OZ: triVec.Z} + // 5% point + pose1 := spatialmath.NewPose( + tp1.Sub(tp1.Sub(tp2).Mul(0.05)), + approachOrient, + ) + // 95% point + pose2 := spatialmath.NewPose( + tp1.Sub(tp1.Sub(tp2).Mul(0.95)), + approachOrient, + ) + normalPoses = append(normalPoses, pose1, pose2) + } + return normalPoses +} + +// Won't work on e.g. a sphere. Has to be "facing" somewhere. +func calcMeshNormal(mesh *spatialmath.Mesh) r3.Vector { + normal := r3.Vector{} + // Find mean normal of mesh + // TODO: Scale by triangle area? + for _, t := range mesh.Triangles() { + tNorm := t.Normal() + //~ fmt.Println("tNorm", tNorm) + // Ensure same hemisphere + if tNorm.Z < 0 { + normal = normal.Add(tNorm.Mul(-1)) + }else { + normal = normal.Add(tNorm) + } + } + normal = normal.Normalize() + return normal +} + +// CalculateNeighbors returns a map of triangle indices to their neighboring triangle indices. +// Two triangles are considered neighbors if they share an edge. +func CalculateNeighbors(triangles []*spatialmath.Triangle) map[int][]int { + neighbors := make(map[int][]int) + + // Helper function to check if two line segments overlap + segmentsOverlap := func(a0, a1, b0, b1 r3.Vector) bool { + const epsilon = 1e-10 // Small value for floating-point comparisons + + // Check if segments are collinear + dir := a1.Sub(a0) + if dir.Norm2() < epsilon { + return false // Degenerate segment + } + + // Check if b0 and b1 lie on the line of a0-a1 + crossB0 := b0.Sub(a0).Cross(dir) + crossB1 := b1.Sub(a0).Cross(dir) + if crossB0.Norm2() > epsilon || crossB1.Norm2() > epsilon { + return false // Not collinear + } + + // Project onto the line + dirNorm := dir.Norm() + t0 := b0.Sub(a0).Dot(dir) / dirNorm + t1 := b1.Sub(a0).Dot(dir) / dirNorm + + // Check overlap + return (t0 >= -epsilon && t0 <= dirNorm+epsilon) || + (t1 >= -epsilon && t1 <= dirNorm+epsilon) || + (t0 <= -epsilon && t1 >= dirNorm+epsilon) + } + + // Helper function to check if two triangles share an edge + sharesEdge := func(t1, t2 *spatialmath.Triangle) bool { + t1p := t1.Points() + t2p := t2.Points() + t1Edges := [][2]r3.Vector{ + {t1p[0], t1p[1]}, + {t1p[1], t1p[2]}, + {t1p[2], t1p[0]}, + } + t2Edges := [][2]r3.Vector{ + {t2p[0], t2p[1]}, + {t2p[1], t2p[2]}, + {t2p[2], t2p[0]}, + } + + for _, e1 := range t1Edges { + for _, e2 := range t2Edges { + if segmentsOverlap(e1[0], e1[1], e2[0], e2[1]) { + return true + } + } + } + return false + } + + // Check each triangle against all other triangles + for i := range triangles { + neighbors[i] = make([]int, 0) + for j := range triangles { + if i != j && sharesEdge(triangles[i], triangles[j]) { + neighbors[i] = append(neighbors[i], j) + } + } + } + + return neighbors +} diff --git a/motionplan/coverage/coverage_test.go b/motionplan/coverage/coverage_test.go new file mode 100644 index 00000000000..6c3c72d5390 --- /dev/null +++ b/motionplan/coverage/coverage_test.go @@ -0,0 +1,36 @@ +package coverage + +import ( + "testing" + "fmt" + + "go.viam.com/test" + "go.viam.com/rdk/spatialmath" + "github.com/golang/geo/r3" +) + +func TestReadPLY(t *testing.T) { + m, err := ReadPLY("/home/peter/Downloads/lod_100_ascii.ply") + test.That(t, err, test.ShouldBeNil) + //~ MeshWaypoints(m, n, 0, nil) + + tri := spatialmath.NewTriangle( + r3.Vector{100, 0, 100}, + r3.Vector{0, 100, 50}, + r3.Vector{0, -100, 150}, + ) + + //~ cameraPose := spatialmath.NewPose( + //~ r3.Vector{0, -100, 250}, + //~ &spatialmath.OrientationVector{OX:1}, + //~ ) + + fmt.Println(tri.ClosestInsidePoint(r3.Vector{100, 0, 100.001})) + //~ mesh1 := spatialmath.NewMesh(spatialmath.NewZeroPose(), []*spatialmath.Triangle{tri}) + wps, err := MeshWaypoints(m, 10, spatialmath.NewZeroPose()) + test.That(t, err, test.ShouldBeNil) + test.That(t, len(wps), test.ShouldBeGreaterThan, 0) + //~ for _, wp := range wps { + //~ fmt.Println(spatialmath.PoseToProtobuf(spatialmath.Compose(cameraPose, wp))) + //~ } +} diff --git a/motionplan/motionPlanner.go b/motionplan/motionPlanner.go index ce8efd27b16..53113cd20f1 100644 --- a/motionplan/motionPlanner.go +++ b/motionplan/motionPlanner.go @@ -463,6 +463,18 @@ IK: // good solution, stopping early break IK } + for _, oldSol := range solutions { + similarity := &ik.SegmentFS{ + StartConfiguration: oldSol, + EndConfiguration: step, + FS: mp.fs, + } + simscore := mp.planOpts.configurationDistanceFunc(similarity) + if simscore < 0.1 { + fmt.Println("Skipping similar") + continue IK + } + } solutions[score] = step if len(solutions) >= nSolutions { diff --git a/motionplan/planManager.go b/motionplan/planManager.go index 9433e060980..d2524012e5a 100644 --- a/motionplan/planManager.go +++ b/motionplan/planManager.go @@ -63,11 +63,6 @@ type atomicWaypoint struct { // planMultiWaypoint plans a motion through multiple waypoints, using identical constraints for each // Any constraints, etc, will be held for the entire motion. func (pm *planManager) planMultiWaypoint(ctx context.Context, request *PlanRequest, seedPlan Plan) (Plan, error) { - startPoses, err := request.StartState.ComputePoses(request.FrameSystem) - if err != nil { - return nil, err - } - opt, err := pm.plannerSetupFromMoveRequest( request.StartState, request.Goals[0], @@ -101,26 +96,13 @@ func (pm *planManager) planMultiWaypoint(ctx context.Context, request *PlanReque waypoints := []atomicWaypoint{} - runningStart := startPoses - for i, goal := range request.Goals { - goalPoses, err := goal.ComputePoses(request.FrameSystem) - if err != nil { - return nil, err - } - - // Log each requested motion - // TODO: PlanRequest.String() could begin to exist - for frame, stepgoal := range goalPoses { - request.Logger.CInfof(ctx, - "setting up motion for frame %s\nGoal: %v\nstartPose %v\nworldstate: %v\n", - frame, - referenceframe.PoseInFrameToProtobuf(stepgoal), - referenceframe.PoseInFrameToProtobuf(runningStart[frame]), - request.WorldState.String(), - ) + for i := range request.Goals { + request.Logger.Info("i", i) + select { + case <-ctx.Done(): + return nil, ctx.Err() + default: } - runningStart = goalPoses - // Solving highly constrained motions by breaking apart into small pieces is much more performant goalWaypoints, err := pm.generateWaypoints(request, seedPlan, i) if err != nil { @@ -156,13 +138,14 @@ func (pm *planManager) planAtomicWaypoints( var seed map[string][]referenceframe.Input // try to solve each goal, one at a time - for _, wp := range waypoints { + for i, wp := range waypoints { // Check if ctx is done between each waypoint select { case <-ctx.Done(): return nil, ctx.Err() default: } + pm.logger.Info("planning step ", i, " of ", len(waypoints)) var maps *rrtMaps if seedPlan != nil { @@ -227,7 +210,10 @@ func (pm *planManager) planSingleAtomicWaypoint( wp atomicWaypoint, maps *rrtMaps, ) (map[string][]referenceframe.Input, *resultPromise, error) { - pm.logger.Debug("start planning for ", wp.goalState.configuration, wp.goalState.poses) + fromPoses, _ := wp.startState.ComputePoses(pm.fs) + toPoses, _ := wp.goalState.ComputePoses(pm.fs) + + pm.logger.Debug("start planning from\n", fromPoses, "\nto\n", toPoses) if _, ok := wp.mp.(rrtParallelPlanner); ok { // rrtParallelPlanner supports solution look-ahead for parallel waypoint solving diff --git a/services/motion/builtin/builtin.go b/services/motion/builtin/builtin.go index 447a2db3722..ee3738c0026 100644 --- a/services/motion/builtin/builtin.go +++ b/services/motion/builtin/builtin.go @@ -417,6 +417,7 @@ func (ms *builtIn) plan(ctx context.Context, req motion.MoveReq) (motionplan.Pla if len(waypoints) == 0 { return nil, errors.New("could not find any waypoints to plan for in MoveRequest. Fill in Destination or goal_state") } + req.Extra["waypoints"] = nil // re-evaluate goal poses to be in the frame of World // TODO (RSDK-8847) : this is a workaround to help account for us not yet being able to properly synchronize simultaneous motion across From 02b5ce14ac35a365daa7730bd7aba85cb8af08cb Mon Sep 17 00:00:00 2001 From: Peter LoVerso Date: Wed, 15 Jan 2025 14:42:10 -0800 Subject: [PATCH 3/8] Add some debug, delete coverage files --- motionplan/cBiRRT.go | 5 + motionplan/coverage/coverage.go | 255 --------------------------- motionplan/coverage/coverage_test.go | 36 ---- motionplan/planManager.go | 7 +- 4 files changed, 10 insertions(+), 293 deletions(-) delete mode 100644 motionplan/coverage/coverage.go delete mode 100644 motionplan/coverage/coverage_test.go diff --git a/motionplan/cBiRRT.go b/motionplan/cBiRRT.go index 5e5672d1fc7..13fc0b7ae62 100644 --- a/motionplan/cBiRRT.go +++ b/motionplan/cBiRRT.go @@ -143,6 +143,11 @@ func (mp *cBiRRTMotionPlanner) rrtBackgroundRunner( } } mp.logger.CInfof(ctx, "goal node: %v\n", rrt.maps.optNode.Q()) + for n, _ := range rrt.maps.startMap { + mp.logger.CInfof(ctx, "start node: %v\n", n.Q()) + break + } + mp.logger.Info("DOF", mp.lfs.dof) interpConfig, err := referenceframe.InterpolateFS(mp.fs, seed, rrt.maps.optNode.Q(), 0.5) if err != nil { rrt.solutionChan <- &rrtSolution{err: err} diff --git a/motionplan/coverage/coverage.go b/motionplan/coverage/coverage.go deleted file mode 100644 index c37e5bec0a3..00000000000 --- a/motionplan/coverage/coverage.go +++ /dev/null @@ -1,255 +0,0 @@ -package coverage - -import ( - "bufio" - "errors" - "os" - "math" - "sort" - "fmt" - //~ "context" - //~ "errors" - - //~ "go.viam.com/rdk/motionplan/ik" - "go.viam.com/rdk/referenceframe" - "go.viam.com/rdk/spatialmath" - - "github.com/chenzhekl/goply" - "github.com/golang/geo/r3" -) - -type plane struct { - pt, normal r3.Vector -} - -func ReadPLY(path string) (*spatialmath.Mesh, error) { - readerRaw, err := os.Open(path) - if err != nil { - return nil, err - } - reader := bufio.NewReader(readerRaw) - ply := goply.New(reader) - vertices := ply.Elements("vertex") - faces := ply.Elements("face") - triangles := []*spatialmath.Triangle{} - for _, face := range faces { - - pts := []r3.Vector{} - idxIface := face["vertex_indices"] - for _, i := range idxIface.([]interface{}) { - pts = append(pts, r3.Vector{ - 1000*vertices[int(i.(uint32))]["x"].(float64), - 1000*vertices[int(i.(uint32))]["y"].(float64), - 1000*vertices[int(i.(uint32))]["z"].(float64)}) - } - if len(pts) != 3 { - return nil, errors.New("triangle did not have three points") - } - tri := spatialmath.NewTriangle(pts[0], pts[1], pts[2]) - triangles = append(triangles, tri) - } - return spatialmath.NewMesh(spatialmath.NewZeroPose(), triangles), nil -} - -// Naive algorithm to find waypoints on a mesh -func MeshWaypoints( - mesh *spatialmath.Mesh, - step, trim float64, // Step is how far apart the up/down strokes will be. Trim will stay that far back from the edges of the mesh - source spatialmath.Pose, // Used to determine which side of mesh to write on -) ([]spatialmath.Pose, error) { - meshNorm := calcMeshNormal(mesh) - normPose := spatialmath.NewPoseFromOrientation(&spatialmath.OrientationVector{OX: meshNorm.X, OY: meshNorm.Y, OZ: meshNorm.Z}) - if step <= 0 { - return nil, errors.New("step must be >0") - } - - // Rotate mesh to project onto X-Y plane, and determine limits of rotated mesh - xLim := referenceframe.Limit{Min: math.Inf(1), Max: math.Inf(-1)} - yLim := referenceframe.Limit{Min: math.Inf(1), Max: math.Inf(-1)} - rotTriangles := []*spatialmath.Triangle{} - vMap := map[r3.Vector][]*spatialmath.Triangle{} - for _, t := range mesh.Triangles() { - pts := []r3.Vector{} - for _, pt := range t.Points() { - rotPt := spatialmath.Compose(spatialmath.PoseInverse(normPose), spatialmath.NewPoseFromPoint(pt)).Point() - if rotPt.Y > yLim.Max { - yLim.Max = rotPt.Y - } - if rotPt.Y < yLim.Min { - yLim.Min = rotPt.Y - } - if rotPt.X > xLim.Max { - xLim.Max = rotPt.X - } - if rotPt.X < xLim.Min { - xLim.Min = rotPt.X - } - pts = append(pts, rotPt) - } - tri := spatialmath.NewTriangle(pts[0], pts[1], pts[2]) - vMap[pts[0]] = append(vMap[pts[0]], tri) - vMap[pts[1]] = append(vMap[pts[1]], tri) - vMap[pts[2]] = append(vMap[pts[2]], tri) - rotTriangles = append(rotTriangles, tri) - } - strokes := [][]spatialmath.Pose{} - for xVal := xLim.Min + step + trim ; xVal < xLim.Max - trim; xVal += step { - stroke := planePoses( - rotTriangles, - plane{pt: r3.Vector{xVal,0,0}, normal: r3.Vector{1,0,0}}, - source, - ) - // Sort by Y value - sort.Slice(stroke, func(i, j int) bool { - return stroke[i].Point().Y > stroke[j].Point().Y - }) - strokes = append(strokes, stroke) - } - // Invert every other stroke to make a contiguous zigzag - finalPath := []spatialmath.Pose{} - flip := false - // Un-rotate poses back to original mesh position - fmt.Println("got ", len(strokes), " strokes") - for _, stroke := range strokes { - if flip { - for i := len(stroke)-1; i >= 0; i-- { - if stroke[i].Point().Y > xLim.Max - trim || stroke[i].Point().Y < xLim.Min + trim { - continue - } - finalPath = append(finalPath, spatialmath.Compose(normPose, stroke[i])) - } - } else { - for _, pose := range stroke { - if pose.Point().Y > xLim.Max - trim || pose.Point().Y < xLim.Min + trim { - continue - } - finalPath = append(finalPath, spatialmath.Compose(normPose, pose)) - } - } - flip = !flip - } - - return finalPath, nil -} - -func planePoses(triangles []*spatialmath.Triangle, p plane, approachPose spatialmath.Pose) []spatialmath.Pose { - normalPoses := []spatialmath.Pose{} - approachVec := approachPose.Orientation().OrientationVectorRadians().Vector() - - for _, t := range triangles { - tp1, tp2, intersects := t.TrianglePlaneIntersectingSegment(p.pt, p.normal) - if !intersects || tp1.ApproxEqual(tp2) { - continue - } - // There are two normals to each triangle; get both and use the one closest to the approach orientation - triVec := t.Normal() - dist := approachVec.Sub(triVec).Norm2() - tNormInv := triVec.Mul(-1) - if approachVec.Sub(tNormInv).Norm2() < dist { - triVec = tNormInv - } - approachOrient := &spatialmath.OrientationVector{OX: triVec.X, OY: triVec.Y, OZ: triVec.Z} - // 5% point - pose1 := spatialmath.NewPose( - tp1.Sub(tp1.Sub(tp2).Mul(0.05)), - approachOrient, - ) - // 95% point - pose2 := spatialmath.NewPose( - tp1.Sub(tp1.Sub(tp2).Mul(0.95)), - approachOrient, - ) - normalPoses = append(normalPoses, pose1, pose2) - } - return normalPoses -} - -// Won't work on e.g. a sphere. Has to be "facing" somewhere. -func calcMeshNormal(mesh *spatialmath.Mesh) r3.Vector { - normal := r3.Vector{} - // Find mean normal of mesh - // TODO: Scale by triangle area? - for _, t := range mesh.Triangles() { - tNorm := t.Normal() - //~ fmt.Println("tNorm", tNorm) - // Ensure same hemisphere - if tNorm.Z < 0 { - normal = normal.Add(tNorm.Mul(-1)) - }else { - normal = normal.Add(tNorm) - } - } - normal = normal.Normalize() - return normal -} - -// CalculateNeighbors returns a map of triangle indices to their neighboring triangle indices. -// Two triangles are considered neighbors if they share an edge. -func CalculateNeighbors(triangles []*spatialmath.Triangle) map[int][]int { - neighbors := make(map[int][]int) - - // Helper function to check if two line segments overlap - segmentsOverlap := func(a0, a1, b0, b1 r3.Vector) bool { - const epsilon = 1e-10 // Small value for floating-point comparisons - - // Check if segments are collinear - dir := a1.Sub(a0) - if dir.Norm2() < epsilon { - return false // Degenerate segment - } - - // Check if b0 and b1 lie on the line of a0-a1 - crossB0 := b0.Sub(a0).Cross(dir) - crossB1 := b1.Sub(a0).Cross(dir) - if crossB0.Norm2() > epsilon || crossB1.Norm2() > epsilon { - return false // Not collinear - } - - // Project onto the line - dirNorm := dir.Norm() - t0 := b0.Sub(a0).Dot(dir) / dirNorm - t1 := b1.Sub(a0).Dot(dir) / dirNorm - - // Check overlap - return (t0 >= -epsilon && t0 <= dirNorm+epsilon) || - (t1 >= -epsilon && t1 <= dirNorm+epsilon) || - (t0 <= -epsilon && t1 >= dirNorm+epsilon) - } - - // Helper function to check if two triangles share an edge - sharesEdge := func(t1, t2 *spatialmath.Triangle) bool { - t1p := t1.Points() - t2p := t2.Points() - t1Edges := [][2]r3.Vector{ - {t1p[0], t1p[1]}, - {t1p[1], t1p[2]}, - {t1p[2], t1p[0]}, - } - t2Edges := [][2]r3.Vector{ - {t2p[0], t2p[1]}, - {t2p[1], t2p[2]}, - {t2p[2], t2p[0]}, - } - - for _, e1 := range t1Edges { - for _, e2 := range t2Edges { - if segmentsOverlap(e1[0], e1[1], e2[0], e2[1]) { - return true - } - } - } - return false - } - - // Check each triangle against all other triangles - for i := range triangles { - neighbors[i] = make([]int, 0) - for j := range triangles { - if i != j && sharesEdge(triangles[i], triangles[j]) { - neighbors[i] = append(neighbors[i], j) - } - } - } - - return neighbors -} diff --git a/motionplan/coverage/coverage_test.go b/motionplan/coverage/coverage_test.go deleted file mode 100644 index 6c3c72d5390..00000000000 --- a/motionplan/coverage/coverage_test.go +++ /dev/null @@ -1,36 +0,0 @@ -package coverage - -import ( - "testing" - "fmt" - - "go.viam.com/test" - "go.viam.com/rdk/spatialmath" - "github.com/golang/geo/r3" -) - -func TestReadPLY(t *testing.T) { - m, err := ReadPLY("/home/peter/Downloads/lod_100_ascii.ply") - test.That(t, err, test.ShouldBeNil) - //~ MeshWaypoints(m, n, 0, nil) - - tri := spatialmath.NewTriangle( - r3.Vector{100, 0, 100}, - r3.Vector{0, 100, 50}, - r3.Vector{0, -100, 150}, - ) - - //~ cameraPose := spatialmath.NewPose( - //~ r3.Vector{0, -100, 250}, - //~ &spatialmath.OrientationVector{OX:1}, - //~ ) - - fmt.Println(tri.ClosestInsidePoint(r3.Vector{100, 0, 100.001})) - //~ mesh1 := spatialmath.NewMesh(spatialmath.NewZeroPose(), []*spatialmath.Triangle{tri}) - wps, err := MeshWaypoints(m, 10, spatialmath.NewZeroPose()) - test.That(t, err, test.ShouldBeNil) - test.That(t, len(wps), test.ShouldBeGreaterThan, 0) - //~ for _, wp := range wps { - //~ fmt.Println(spatialmath.PoseToProtobuf(spatialmath.Compose(cameraPose, wp))) - //~ } -} diff --git a/motionplan/planManager.go b/motionplan/planManager.go index d2524012e5a..23c8c6070c5 100644 --- a/motionplan/planManager.go +++ b/motionplan/planManager.go @@ -145,7 +145,10 @@ func (pm *planManager) planAtomicWaypoints( return nil, ctx.Err() default: } - pm.logger.Info("planning step ", i, " of ", len(waypoints)) + pm.logger.Info("planning step ", i, " of ", len(waypoints), ":", wp.goalState) + for k, v := range wp.goalState.Poses() { + pm.logger.Info(k, v) + } var maps *rrtMaps if seedPlan != nil { @@ -212,7 +215,7 @@ func (pm *planManager) planSingleAtomicWaypoint( ) (map[string][]referenceframe.Input, *resultPromise, error) { fromPoses, _ := wp.startState.ComputePoses(pm.fs) toPoses, _ := wp.goalState.ComputePoses(pm.fs) - + pm.logger.Debug("start configuration", wp.startState.Configuration()) pm.logger.Debug("start planning from\n", fromPoses, "\nto\n", toPoses) if _, ok := wp.mp.(rrtParallelPlanner); ok { From 564da80273c6df851f81791b3ec07aa7d2ccb9e2 Mon Sep 17 00:00:00 2001 From: Peter LoVerso Date: Wed, 15 Jan 2025 14:56:11 -0800 Subject: [PATCH 4/8] Appease linter --- motionplan/cBiRRT.go | 2 +- motionplan/motionPlanner.go | 1 - motionplan/planManager.go | 10 ++++++++-- spatialmath/mesh.go | 11 +++++------ spatialmath/triangle.go | 12 ++++++++---- 5 files changed, 22 insertions(+), 14 deletions(-) diff --git a/motionplan/cBiRRT.go b/motionplan/cBiRRT.go index 13fc0b7ae62..0d84bed3f58 100644 --- a/motionplan/cBiRRT.go +++ b/motionplan/cBiRRT.go @@ -143,7 +143,7 @@ func (mp *cBiRRTMotionPlanner) rrtBackgroundRunner( } } mp.logger.CInfof(ctx, "goal node: %v\n", rrt.maps.optNode.Q()) - for n, _ := range rrt.maps.startMap { + for n := range rrt.maps.startMap { mp.logger.CInfof(ctx, "start node: %v\n", n.Q()) break } diff --git a/motionplan/motionPlanner.go b/motionplan/motionPlanner.go index 53113cd20f1..10c5e05e1e0 100644 --- a/motionplan/motionPlanner.go +++ b/motionplan/motionPlanner.go @@ -471,7 +471,6 @@ IK: } simscore := mp.planOpts.configurationDistanceFunc(similarity) if simscore < 0.1 { - fmt.Println("Skipping similar") continue IK } } diff --git a/motionplan/planManager.go b/motionplan/planManager.go index 23c8c6070c5..ad079ebcc59 100644 --- a/motionplan/planManager.go +++ b/motionplan/planManager.go @@ -213,8 +213,14 @@ func (pm *planManager) planSingleAtomicWaypoint( wp atomicWaypoint, maps *rrtMaps, ) (map[string][]referenceframe.Input, *resultPromise, error) { - fromPoses, _ := wp.startState.ComputePoses(pm.fs) - toPoses, _ := wp.goalState.ComputePoses(pm.fs) + fromPoses, err := wp.startState.ComputePoses(pm.fs) + if err != nil { + return nil, nil, err + } + toPoses, err := wp.goalState.ComputePoses(pm.fs) + if err != nil { + return nil, nil, err + } pm.logger.Debug("start configuration", wp.startState.Configuration()) pm.logger.Debug("start planning from\n", fromPoses, "\nto\n", toPoses) diff --git a/spatialmath/mesh.go b/spatialmath/mesh.go index bf45f4a4314..43ab03d58f2 100644 --- a/spatialmath/mesh.go +++ b/spatialmath/mesh.go @@ -1,20 +1,16 @@ package spatialmath - -//~ import ( -//~ "github.com/golang/geo/r3" -//~ ) - // This file incorporates work covered by the Brax project -- https://github.com/google/brax/blob/main/LICENSE. // Copyright 2021 The Brax Authors, which is licensed under the Apache License Version 2.0 (the “License”). // You may obtain a copy of the license at http://www.apache.org/licenses/LICENSE-2.0. -// mesh is a collision geometry that represents a set of triangles that represent a mesh. +// Mesh is a set of triangles at some pose. Triangle points are in the frame of the mesh. type Mesh struct { pose Pose triangles []*Triangle } +// NewMesh creates a mesh from the given triangles and pose. func NewMesh(pose Pose, triangles []*Triangle) *Mesh { return &Mesh{ pose: pose, @@ -22,14 +18,17 @@ func NewMesh(pose Pose, triangles []*Triangle) *Mesh { } } +// Pose returns the pose of the mesh. func (m *Mesh) Pose() Pose { return m.pose } +// Triangles returns the triangles associated with the mesh. func (m *Mesh) Triangles() []*Triangle { return m.triangles } +// Transform transforms the mesh. As triangles are in the mesh's frame, they are unchanged. func (m *Mesh) Transform(pose Pose) *Mesh { // Triangle points are in frame of mesh, like the corners of a box, so no need to transform them return &Mesh{ diff --git a/spatialmath/triangle.go b/spatialmath/triangle.go index f9554b6cb91..1a8f9d4192e 100644 --- a/spatialmath/triangle.go +++ b/spatialmath/triangle.go @@ -6,6 +6,7 @@ import ( "github.com/golang/geo/r3" ) +// Triangle is three points and a normal vector. type Triangle struct { p0 r3.Vector p1 r3.Vector @@ -14,6 +15,7 @@ type Triangle struct { normal r3.Vector } +// NewTriangle creates a Triangle from three points. The Normal is computed; directionality is random. func NewTriangle(p0, p1, p2 r3.Vector) *Triangle { return &Triangle{ p0: p0, @@ -23,7 +25,7 @@ func NewTriangle(p0, p1, p2 r3.Vector) *Triangle { } } -// closestPointToCoplanarPoint takes a point, and returns the closest point on the triangle to the given point +// ClosestPointToCoplanarPoint takes a point, and returns the closest point on the triangle to the given point // The given point *MUST* be coplanar with the triangle. If it is known ahead of time that the point is coplanar, this is faster. func (t *Triangle) ClosestPointToCoplanarPoint(pt r3.Vector) r3.Vector { // Determine whether point is inside all triangle edges: @@ -55,8 +57,8 @@ func (t *Triangle) ClosestPointToCoplanarPoint(pt r3.Vector) r3.Vector { return refPt } -// closestPointToPoint takes a point, and returns the closest point on the triangle to the given point. -// This is slower than closestPointToCoplanarPoint. +// ClosestPointToPoint takes a point, and returns the closest point on the triangle to the given point. +// This is slower than ClosestPointToCoplanarPoint. func (t *Triangle) ClosestPointToPoint(point r3.Vector) r3.Vector { closestPtInside, inside := t.ClosestInsidePoint(point) if inside { @@ -81,7 +83,7 @@ func (t *Triangle) ClosestPointToPoint(point r3.Vector) r3.Vector { return closestPt } -// closestInsidePoint returns the closest point on a triangle IF AND ONLY IF the query point's projection overlaps the triangle. +// ClosestInsidePoint returns the closest point on a triangle IF AND ONLY IF the query point's projection overlaps the triangle. // Otherwise it will return the query point. // To visualize this- if one draws a tetrahedron using the triangle and the query point, all angles from the triangle to the query point // must be <= 90 degrees. @@ -107,10 +109,12 @@ func (t *Triangle) ClosestInsidePoint(point r3.Vector) (r3.Vector, bool) { return t.p0.Add(e0.Mul(u)).Add(e1.Mul(v)), inside } +// Points returns the three points associated with the triangle. func (t *Triangle) Points() []r3.Vector { return []r3.Vector{t.p0, t.p1, t.p2} } +// Normal returns the triangle's normal vector. func (t *Triangle) Normal() r3.Vector { return t.normal } From 46fa02bd4de11a1149aeec91a247ee0e062c1ba5 Mon Sep 17 00:00:00 2001 From: Peter LoVerso Date: Mon, 27 Jan 2025 09:53:36 -0800 Subject: [PATCH 5/8] Assorted PR feedback --- motionplan/cBiRRT.go | 6 +++--- motionplan/motionPlanner.go | 6 +++++- motionplan/plan.go | 2 +- motionplan/planManager.go | 1 - motionplan/plannerOptions.go | 4 ++-- services/motion/builtin/builtin.go | 3 +++ spatialmath/box.go | 2 +- 7 files changed, 15 insertions(+), 9 deletions(-) diff --git a/motionplan/cBiRRT.go b/motionplan/cBiRRT.go index 0d84bed3f58..baebae4740b 100644 --- a/motionplan/cBiRRT.go +++ b/motionplan/cBiRRT.go @@ -142,12 +142,12 @@ func (mp *cBiRRTMotionPlanner) rrtBackgroundRunner( break } } - mp.logger.CInfof(ctx, "goal node: %v\n", rrt.maps.optNode.Q()) + mp.logger.CDebugf(ctx, "goal node: %v\n", rrt.maps.optNode.Q()) for n := range rrt.maps.startMap { - mp.logger.CInfof(ctx, "start node: %v\n", n.Q()) + mp.logger.CDebugf(ctx, "start node: %v\n", n.Q()) break } - mp.logger.Info("DOF", mp.lfs.dof) + mp.logger.Debug("DOF", mp.lfs.dof) interpConfig, err := referenceframe.InterpolateFS(mp.fs, seed, rrt.maps.optNode.Q(), 0.5) if err != nil { rrt.solutionChan <- &rrtSolution{err: err} diff --git a/motionplan/motionPlanner.go b/motionplan/motionPlanner.go index 10c5e05e1e0..7057041f9b1 100644 --- a/motionplan/motionPlanner.go +++ b/motionplan/motionPlanner.go @@ -21,6 +21,10 @@ import ( "go.viam.com/rdk/spatialmath" ) +// When we generate solutions, if a new solution is within this level of similarity to an existing one, discard it as a duplicate. +// This prevents seeding the solution tree with 50 copies of essentially the same configuration. +const defaultSimScore = 0.05 + // motionPlanner provides an interface to path planning methods, providing ways to request a path to be planned, and // management of the constraints used to plan paths. type motionPlanner interface { @@ -470,7 +474,7 @@ IK: FS: mp.fs, } simscore := mp.planOpts.configurationDistanceFunc(similarity) - if simscore < 0.1 { + if simscore < defaultSimScore { continue IK } } diff --git a/motionplan/plan.go b/motionplan/plan.go index 19eee448904..cacbb82fa17 100644 --- a/motionplan/plan.go +++ b/motionplan/plan.go @@ -376,7 +376,7 @@ func (p *PlanState) Configuration() map[string][]referenceframe.Input { // ComputePoses returns the poses of a PlanState if they are populated, or computes them using the given FrameSystem if not. func (p *PlanState) ComputePoses(fs referenceframe.FrameSystem) (PathState, error) { - if p.poses != nil { + if len(p.poses) > 0 { return p.poses, nil } diff --git a/motionplan/planManager.go b/motionplan/planManager.go index ad079ebcc59..7ef23a85c6f 100644 --- a/motionplan/planManager.go +++ b/motionplan/planManager.go @@ -97,7 +97,6 @@ func (pm *planManager) planMultiWaypoint(ctx context.Context, request *PlanReque waypoints := []atomicWaypoint{} for i := range request.Goals { - request.Logger.Info("i", i) select { case <-ctx.Done(): return nil, ctx.Err() diff --git a/motionplan/plannerOptions.go b/motionplan/plannerOptions.go index 342b3f892fe..29272463b27 100644 --- a/motionplan/plannerOptions.go +++ b/motionplan/plannerOptions.go @@ -27,7 +27,7 @@ const ( defaultPseudolinearTolerance = 0.8 // Number of IK solutions that should be generated before stopping. - defaultSolutionsToSeed = 50 + defaultSolutionsToSeed = 100 // Check constraints are still met every this many mm/degrees of movement. defaultResolution = 2.0 @@ -64,7 +64,7 @@ const ( defaultRobotCollisionConstraintDesc = "Collision between a robot component that is moving and one that is stationary" // When breaking down a path into smaller waypoints, add a waypoint every this many mm of movement. - defaultPathStateSize = 10 + defaultPathStateSize = 15 // This is commented out due to Go compiler bug. See comment in newBasicPlannerOptions for explanation. // var defaultPlanner = newCBiRRTMotionPlanner. diff --git a/services/motion/builtin/builtin.go b/services/motion/builtin/builtin.go index ee3738c0026..09b75cc8b0a 100644 --- a/services/motion/builtin/builtin.go +++ b/services/motion/builtin/builtin.go @@ -417,6 +417,9 @@ func (ms *builtIn) plan(ctx context.Context, req motion.MoveReq) (motionplan.Pla if len(waypoints) == 0 { return nil, errors.New("could not find any waypoints to plan for in MoveRequest. Fill in Destination or goal_state") } + // The contents of waypoints can be gigantic, and if so, making copies of `extra` becomes the majority of motion planning runtime. + // As the meaning from `waypoints` has already been extracted above into its proper data structure, there is no longer a need to + // keep it in `extra`. req.Extra["waypoints"] = nil // re-evaluate goal poses to be in the frame of World diff --git a/spatialmath/box.go b/spatialmath/box.go index ef5b8a99d57..d58030583b7 100644 --- a/spatialmath/box.go +++ b/spatialmath/box.go @@ -240,7 +240,7 @@ func (b *box) vertices() []r3.Vector { return verts } -// vertices returns the vertices defining the box. +// toMesh returns a 12-triangle mesh representation of the box, 2 right triangles for each face. func (b *box) toMesh() *Mesh { if b.mesh == nil { m := &Mesh{pose: b.pose} From 6e58d22b59db5d8d8fb811dd7b21cd05523e2795 Mon Sep 17 00:00:00 2001 From: Peter LoVerso Date: Mon, 27 Jan 2025 10:25:41 -0800 Subject: [PATCH 6/8] Fix nil map assignment --- services/motion/builtin/builtin.go | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/services/motion/builtin/builtin.go b/services/motion/builtin/builtin.go index 2e42df186ee..a9bd6d552c2 100644 --- a/services/motion/builtin/builtin.go +++ b/services/motion/builtin/builtin.go @@ -420,7 +420,9 @@ func (ms *builtIn) plan(ctx context.Context, req motion.MoveReq) (motionplan.Pla // The contents of waypoints can be gigantic, and if so, making copies of `extra` becomes the majority of motion planning runtime. // As the meaning from `waypoints` has already been extracted above into its proper data structure, there is no longer a need to // keep it in `extra`. - req.Extra["waypoints"] = nil + if req.Extra != nil { + req.Extra["waypoints"] = nil + } // re-evaluate goal poses to be in the frame of World // TODO (RSDK-8847) : this is a workaround to help account for us not yet being able to properly synchronize simultaneous motion across From a21ef8ff2d6487a21f8d7ede9fe5194765a3f21d Mon Sep 17 00:00:00 2001 From: Peter LoVerso Date: Mon, 27 Jan 2025 14:16:10 -0800 Subject: [PATCH 7/8] Update triangle utils to no longer be methods on triangles --- spatialmath/geometry_utils.go | 2 +- spatialmath/mesh_test.go | 6 +++--- spatialmath/triangle.go | 24 ++++++++++++------------ spatialmath/triangle_test.go | 4 ++-- 4 files changed, 18 insertions(+), 18 deletions(-) diff --git a/spatialmath/geometry_utils.go b/spatialmath/geometry_utils.go index 66f40315b1e..d40152db4e2 100644 --- a/spatialmath/geometry_utils.go +++ b/spatialmath/geometry_utils.go @@ -109,7 +109,7 @@ func closestPointsSegmentTriangle(ap1, ap2 r3.Vector, t *Triangle) (bestSegPt, b // If the line overlaps the triangle and is parallel to the triangle plane, // the chosen triangle point is arbitrary. segPt, _ := closestPointsSegmentPlane(ap1, ap2, t.p0, t.normal) - triPt, inside := t.ClosestInsidePoint(segPt) + triPt, inside := ClosestTriangleInsidePoint(t, segPt) if inside { // If inside is false, then these will not be the best points, because they are based on the segment-plane intersection return segPt, triPt diff --git a/spatialmath/mesh_test.go b/spatialmath/mesh_test.go index bfc3570c294..2a301c2902f 100644 --- a/spatialmath/mesh_test.go +++ b/spatialmath/mesh_test.go @@ -14,9 +14,9 @@ func TestClosestPoint(t *testing.T) { tri := NewTriangle(p0, p1, p2) qp1 := r3.Vector{-1, 0, 0} - cp1 := tri.ClosestPointToCoplanarPoint(qp1) - cp2 := tri.ClosestPointToPoint(qp1) - cp3, inside := tri.ClosestInsidePoint(qp1) + cp1 := ClosestPointTriangleCoplanarPoint(tri, qp1) + cp2 := ClosestPointTrianglePoint(tri, qp1) + cp3, inside := ClosestTriangleInsidePoint(tri, qp1) test.That(t, inside, test.ShouldBeFalse) test.That(t, cp3.ApproxEqual(qp1), test.ShouldBeTrue) test.That(t, cp1.ApproxEqual(cp2), test.ShouldBeTrue) diff --git a/spatialmath/triangle.go b/spatialmath/triangle.go index 1a8f9d4192e..f500e243e69 100644 --- a/spatialmath/triangle.go +++ b/spatialmath/triangle.go @@ -25,9 +25,9 @@ func NewTriangle(p0, p1, p2 r3.Vector) *Triangle { } } -// ClosestPointToCoplanarPoint takes a point, and returns the closest point on the triangle to the given point +// ClosestPointTriangleCoplanarPoint takes a point, and returns the closest point on the triangle to the given point // The given point *MUST* be coplanar with the triangle. If it is known ahead of time that the point is coplanar, this is faster. -func (t *Triangle) ClosestPointToCoplanarPoint(pt r3.Vector) r3.Vector { +func ClosestPointTriangleCoplanarPoint(t *Triangle, pt r3.Vector) r3.Vector { // Determine whether point is inside all triangle edges: c0 := pt.Sub(t.p0).Cross(t.p1.Sub(t.p0)) c1 := pt.Sub(t.p1).Cross(t.p2.Sub(t.p1)) @@ -57,10 +57,10 @@ func (t *Triangle) ClosestPointToCoplanarPoint(pt r3.Vector) r3.Vector { return refPt } -// ClosestPointToPoint takes a point, and returns the closest point on the triangle to the given point. -// This is slower than ClosestPointToCoplanarPoint. -func (t *Triangle) ClosestPointToPoint(point r3.Vector) r3.Vector { - closestPtInside, inside := t.ClosestInsidePoint(point) +// ClosestPointTrianglePoint takes a point, and returns the closest point on the triangle to the given point. +// This is slower than ClosestPointTriangleCoplanarPoint. +func ClosestPointTrianglePoint(t *Triangle, point r3.Vector) r3.Vector { + closestPtInside, inside := ClosestTriangleInsidePoint(t, point) if inside { return closestPtInside } @@ -83,11 +83,11 @@ func (t *Triangle) ClosestPointToPoint(point r3.Vector) r3.Vector { return closestPt } -// ClosestInsidePoint returns the closest point on a triangle IF AND ONLY IF the query point's projection overlaps the triangle. +// ClosestTriangleInsidePoint returns the closest point on a triangle IF AND ONLY IF the query point's projection overlaps the triangle. // Otherwise it will return the query point. // To visualize this- if one draws a tetrahedron using the triangle and the query point, all angles from the triangle to the query point // must be <= 90 degrees. -func (t *Triangle) ClosestInsidePoint(point r3.Vector) (r3.Vector, bool) { +func ClosestTriangleInsidePoint(t *Triangle, point r3.Vector) (r3.Vector, bool) { eps := 1e-6 // Parametrize the triangle s.t. a point inside the triangle is @@ -119,9 +119,9 @@ func (t *Triangle) Normal() r3.Vector { return t.normal } -// IntersectsPlane determines if the triangle intersects with a plane defined by a point and normal vector. +// TriangleIntersectsPlane determines if the triangle intersects with a plane defined by a point and normal vector. // Returns true if the triangle intersects with or lies on the plane. -func (t *Triangle) IntersectsPlane(planePt, planeNormal r3.Vector) bool { +func TriangleIntersectsPlane(t *Triangle, planePt, planeNormal r3.Vector) bool { // Calculate signed distances from each triangle vertex to the plane d0 := planeNormal.Dot(t.p0.Sub(planePt)) d1 := planeNormal.Dot(t.p1.Sub(planePt)) @@ -143,9 +143,9 @@ func (t *Triangle) IntersectsPlane(planePt, planeNormal r3.Vector) bool { // Returns the two points defining the intersection line segment and whether an intersection exists. // If the triangle only touches the plane at a point, both returned points will be the same. // If the triangle lies in the plane, it returns two points representing the longest edge of the triangle. -func (t *Triangle) TrianglePlaneIntersectingSegment(planePt, planeNormal r3.Vector) (r3.Vector, r3.Vector, bool) { +func TrianglePlaneIntersectingSegment(t *Triangle, planePt, planeNormal r3.Vector) (r3.Vector, r3.Vector, bool) { // First check if there's an intersection - if !t.IntersectsPlane(planePt, planeNormal) { + if !TriangleIntersectsPlane(t, planePt, planeNormal) { return r3.Vector{}, r3.Vector{}, false } diff --git a/spatialmath/triangle_test.go b/spatialmath/triangle_test.go index e483b71ad4a..8e6d8c6b7d9 100644 --- a/spatialmath/triangle_test.go +++ b/spatialmath/triangle_test.go @@ -74,7 +74,7 @@ func TestTriangleIntersectsPlane(t *testing.T) { for _, tt := range tests { t.Run(tt.name, func(t *testing.T) { - got := tt.triangle.IntersectsPlane(tt.planePt, tt.planeNormal) + got := TriangleIntersectsPlane(tt.triangle, tt.planePt, tt.planeNormal) test.That(t, tt.want, test.ShouldEqual, got) }) } @@ -146,7 +146,7 @@ func TestTrianglePlaneIntersectingSegment(t *testing.T) { for _, tt := range tests { t.Run(tt.name, func(t *testing.T) { - gotP1, gotP2, gotExists := tt.triangle.TrianglePlaneIntersectingSegment(tt.planePt, tt.planeNormal) + gotP1, gotP2, gotExists := TrianglePlaneIntersectingSegment(tt.triangle, tt.planePt, tt.planeNormal) test.That(t, tt.wantExists, test.ShouldEqual, gotExists) if tt.wantExists { test.That(t, tt.wantP1.ApproxEqual(gotP1), test.ShouldBeTrue) From 0ac5ac6115c275b3827b5f592ab4cd5a958fdbd3 Mon Sep 17 00:00:00 2001 From: Ray Bjorkman Date: Mon, 27 Jan 2025 18:47:33 -0500 Subject: [PATCH 8/8] remove some triangle functions --- spatialmath/mesh_test.go | 23 ----- spatialmath/triangle.go | 153 ++-------------------------------- spatialmath/triangle_test.go | 157 ----------------------------------- 3 files changed, 6 insertions(+), 327 deletions(-) delete mode 100644 spatialmath/mesh_test.go delete mode 100644 spatialmath/triangle_test.go diff --git a/spatialmath/mesh_test.go b/spatialmath/mesh_test.go deleted file mode 100644 index 2a301c2902f..00000000000 --- a/spatialmath/mesh_test.go +++ /dev/null @@ -1,23 +0,0 @@ -package spatialmath - -import ( - "testing" - - "github.com/golang/geo/r3" - "go.viam.com/test" -) - -func TestClosestPoint(t *testing.T) { - p0 := r3.Vector{0, 0, 0} - p1 := r3.Vector{1, 0, 0} - p2 := r3.Vector{0, 0, 2} - tri := NewTriangle(p0, p1, p2) - - qp1 := r3.Vector{-1, 0, 0} - cp1 := ClosestPointTriangleCoplanarPoint(tri, qp1) - cp2 := ClosestPointTrianglePoint(tri, qp1) - cp3, inside := ClosestTriangleInsidePoint(tri, qp1) - test.That(t, inside, test.ShouldBeFalse) - test.That(t, cp3.ApproxEqual(qp1), test.ShouldBeTrue) - test.That(t, cp1.ApproxEqual(cp2), test.ShouldBeTrue) -} diff --git a/spatialmath/triangle.go b/spatialmath/triangle.go index f500e243e69..40f6b11c103 100644 --- a/spatialmath/triangle.go +++ b/spatialmath/triangle.go @@ -1,8 +1,6 @@ package spatialmath import ( - "math" - "github.com/golang/geo/r3" ) @@ -25,62 +23,14 @@ func NewTriangle(p0, p1, p2 r3.Vector) *Triangle { } } -// ClosestPointTriangleCoplanarPoint takes a point, and returns the closest point on the triangle to the given point -// The given point *MUST* be coplanar with the triangle. If it is known ahead of time that the point is coplanar, this is faster. -func ClosestPointTriangleCoplanarPoint(t *Triangle, pt r3.Vector) r3.Vector { - // Determine whether point is inside all triangle edges: - c0 := pt.Sub(t.p0).Cross(t.p1.Sub(t.p0)) - c1 := pt.Sub(t.p1).Cross(t.p2.Sub(t.p1)) - c2 := pt.Sub(t.p2).Cross(t.p0.Sub(t.p2)) - inside := c0.Dot(t.normal) <= 0 && c1.Dot(t.normal) <= 0 && c2.Dot(t.normal) <= 0 - - if inside { - return pt - } - - // Edge 1: - refPt := ClosestPointSegmentPoint(t.p0, t.p1, pt) - bestDist := pt.Sub(refPt).Norm2() - - // Edge 2: - point2 := ClosestPointSegmentPoint(t.p1, t.p2, pt) - if distsq := pt.Sub(point2).Norm2(); distsq < bestDist { - refPt = point2 - bestDist = distsq - } - - // Edge 3: - point3 := ClosestPointSegmentPoint(t.p2, t.p0, pt) - if distsq := pt.Sub(point3).Norm2(); distsq < bestDist { - return point3 - } - return refPt +// Points returns the three points associated with the triangle. +func (t *Triangle) Points() []r3.Vector { + return []r3.Vector{t.p0, t.p1, t.p2} } -// ClosestPointTrianglePoint takes a point, and returns the closest point on the triangle to the given point. -// This is slower than ClosestPointTriangleCoplanarPoint. -func ClosestPointTrianglePoint(t *Triangle, point r3.Vector) r3.Vector { - closestPtInside, inside := ClosestTriangleInsidePoint(t, point) - if inside { - return closestPtInside - } - - // If the closest point is outside the triangle, it must be on an edge, so we - // check each triangle edge for a closest point to the point pt. - closestPt := ClosestPointSegmentPoint(t.p0, t.p1, point) - bestDist := point.Sub(closestPt).Norm2() - - newPt := ClosestPointSegmentPoint(t.p1, t.p2, point) - if newDist := point.Sub(newPt).Norm2(); newDist < bestDist { - closestPt = newPt - bestDist = newDist - } - - newPt = ClosestPointSegmentPoint(t.p2, t.p0, point) - if newDist := point.Sub(newPt).Norm2(); newDist < bestDist { - return newPt - } - return closestPt +// Normal returns the triangle's normal vector. +func (t *Triangle) Normal() r3.Vector { + return t.normal } // ClosestTriangleInsidePoint returns the closest point on a triangle IF AND ONLY IF the query point's projection overlaps the triangle. @@ -108,94 +58,3 @@ func ClosestTriangleInsidePoint(t *Triangle, point r3.Vector) (r3.Vector, bool) inside := (0 <= u+eps) && (u <= 1+eps) && (0 <= v+eps) && (v <= 1+eps) && (u+v <= 1+eps) return t.p0.Add(e0.Mul(u)).Add(e1.Mul(v)), inside } - -// Points returns the three points associated with the triangle. -func (t *Triangle) Points() []r3.Vector { - return []r3.Vector{t.p0, t.p1, t.p2} -} - -// Normal returns the triangle's normal vector. -func (t *Triangle) Normal() r3.Vector { - return t.normal -} - -// TriangleIntersectsPlane determines if the triangle intersects with a plane defined by a point and normal vector. -// Returns true if the triangle intersects with or lies on the plane. -func TriangleIntersectsPlane(t *Triangle, planePt, planeNormal r3.Vector) bool { - // Calculate signed distances from each triangle vertex to the plane - d0 := planeNormal.Dot(t.p0.Sub(planePt)) - d1 := planeNormal.Dot(t.p1.Sub(planePt)) - d2 := planeNormal.Dot(t.p2.Sub(planePt)) - - // If all points are on the same side of the plane (all distances positive or all negative), - // there is no intersection - if (d0 > floatEpsilon && d1 > floatEpsilon && d2 > floatEpsilon) || - (d0 < -floatEpsilon && d1 < -floatEpsilon && d2 < -floatEpsilon) { - return false - } - - // If any point is very close to the plane or points lie on different sides, - // there is an intersection - return true -} - -// TrianglePlaneIntersectingSegment determines the line segment where a triangle intersects with a plane. -// Returns the two points defining the intersection line segment and whether an intersection exists. -// If the triangle only touches the plane at a point, both returned points will be the same. -// If the triangle lies in the plane, it returns two points representing the longest edge of the triangle. -func TrianglePlaneIntersectingSegment(t *Triangle, planePt, planeNormal r3.Vector) (r3.Vector, r3.Vector, bool) { - // First check if there's an intersection - if !TriangleIntersectsPlane(t, planePt, planeNormal) { - return r3.Vector{}, r3.Vector{}, false - } - - // Calculate signed distances from each vertex to the plane - d0 := planeNormal.Dot(t.p0.Sub(planePt)) - d1 := planeNormal.Dot(t.p1.Sub(planePt)) - d2 := planeNormal.Dot(t.p2.Sub(planePt)) - - // If triangle lies in plane (all distances are approximately zero) - if math.Abs(d0) < floatEpsilon && math.Abs(d1) < floatEpsilon && math.Abs(d2) < floatEpsilon { - // Return the longest edge of the triangle - e1 := t.p1.Sub(t.p0).Norm2() - e2 := t.p2.Sub(t.p1).Norm2() - e3 := t.p0.Sub(t.p2).Norm2() - if e1 >= e2 && e1 >= e3 { - return t.p0, t.p1, true - } else if e2 >= e1 && e2 >= e3 { - return t.p1, t.p2, true - } - return t.p2, t.p0, true - } - - // Find the two edges that intersect with the plane - var intersections []r3.Vector - edges := [][2]r3.Vector{ - {t.p0, t.p1}, - {t.p1, t.p2}, - {t.p2, t.p0}, - } - dists := []float64{d0, d1, d2} - - for i := 0; i < 3; i++ { - j := (i + 1) % 3 - // If edge crosses plane (distances have different signs) - if (dists[i] * dists[j]) < 0 { - // Calculate intersection point using linear interpolation - t := dists[i] / (dists[i] - dists[j]) - edge := edges[i] - intersection := edge[0].Add(edge[1].Sub(edge[0]).Mul(t)) - intersections = append(intersections, intersection) - } else if math.Abs(dists[i]) < floatEpsilon { - // Vertex lies on plane - intersections = append(intersections, edges[i][0]) - } - } - - // We should have exactly two intersection points - if len(intersections) < 2 { - // Handle degenerate case where triangle only touches plane at a point - return intersections[0], intersections[0], true - } - return intersections[0], intersections[1], true -} diff --git a/spatialmath/triangle_test.go b/spatialmath/triangle_test.go deleted file mode 100644 index 8e6d8c6b7d9..00000000000 --- a/spatialmath/triangle_test.go +++ /dev/null @@ -1,157 +0,0 @@ -package spatialmath - -import ( - "testing" - - "github.com/golang/geo/r3" - "go.viam.com/test" -) - -func TestTriangleIntersectsPlane(t *testing.T) { - tests := []struct { - name string - triangle *Triangle - planePt r3.Vector - planeNormal r3.Vector - want bool - }{ - { - name: "triangle intersects plane", - triangle: NewTriangle( - r3.Vector{X: -1, Y: 0, Z: -1}, - r3.Vector{X: 1, Y: 0, Z: -1}, - r3.Vector{X: 0, Y: 0, Z: 1}, - ), - planePt: r3.Vector{X: 0, Y: 0, Z: 0}, - planeNormal: r3.Vector{X: 0, Y: 0, Z: 1}, - want: true, - }, - { - name: "triangle above plane", - triangle: NewTriangle( - r3.Vector{X: 0, Y: 0, Z: 1}, - r3.Vector{X: 1, Y: 0, Z: 1}, - r3.Vector{X: 0, Y: 1, Z: 1}, - ), - planePt: r3.Vector{X: 0, Y: 0, Z: 0}, - planeNormal: r3.Vector{X: 0, Y: 0, Z: 1}, - want: false, - }, - { - name: "triangle below plane", - triangle: NewTriangle( - r3.Vector{X: 0, Y: 0, Z: -1}, - r3.Vector{X: 1, Y: 0, Z: -1}, - r3.Vector{X: 0, Y: 1, Z: -1}, - ), - planePt: r3.Vector{X: 0, Y: 0, Z: 0}, - planeNormal: r3.Vector{X: 0, Y: 0, Z: 1}, - want: false, - }, - { - name: "triangle lies in plane", - triangle: NewTriangle( - r3.Vector{X: 0, Y: 0, Z: 0}, - r3.Vector{X: 1, Y: 0, Z: 0}, - r3.Vector{X: 0, Y: 1, Z: 0}, - ), - planePt: r3.Vector{X: 0, Y: 0, Z: 0}, - planeNormal: r3.Vector{X: 0, Y: 0, Z: 1}, - want: true, - }, - { - name: "triangle touches plane at vertex", - triangle: NewTriangle( - r3.Vector{X: 0, Y: 0, Z: 0}, - r3.Vector{X: 1, Y: 0, Z: 1}, - r3.Vector{X: 0, Y: 1, Z: 1}, - ), - planePt: r3.Vector{X: 0, Y: 0, Z: 0}, - planeNormal: r3.Vector{X: 0, Y: 0, Z: 1}, - want: true, - }, - } - - for _, tt := range tests { - t.Run(tt.name, func(t *testing.T) { - got := TriangleIntersectsPlane(tt.triangle, tt.planePt, tt.planeNormal) - test.That(t, tt.want, test.ShouldEqual, got) - }) - } -} - -func TestTrianglePlaneIntersectingSegment(t *testing.T) { - tests := []struct { - name string - triangle *Triangle - planePt r3.Vector - planeNormal r3.Vector - wantP1 r3.Vector - wantP2 r3.Vector - wantExists bool - }{ - { - name: "triangle intersects plane - simple case", - triangle: NewTriangle( - r3.Vector{X: -1, Y: 0, Z: -1}, - r3.Vector{X: 1, Y: 0, Z: -1}, - r3.Vector{X: 0, Y: 0, Z: 1}, - ), - planePt: r3.Vector{X: 0, Y: 0, Z: 0}, - planeNormal: r3.Vector{X: 0, Y: 0, Z: 1}, - wantP1: r3.Vector{X: 0.5, Y: 0, Z: 0}, - wantP2: r3.Vector{X: -0.5, Y: 0, Z: 0}, - wantExists: true, - }, - { - name: "triangle lies in plane", - triangle: NewTriangle( - r3.Vector{X: 0, Y: 0, Z: 0}, - r3.Vector{X: 1, Y: 0, Z: 0}, - r3.Vector{X: 0, Y: 1, Z: 0}, - ), - planePt: r3.Vector{X: 0, Y: 0, Z: 0}, - planeNormal: r3.Vector{X: 0, Y: 0, Z: 1}, - wantP1: r3.Vector{X: 1, Y: 0, Z: 0}, - wantP2: r3.Vector{X: 0, Y: 1, Z: 0}, - wantExists: true, - }, - { - name: "no intersection", - triangle: NewTriangle( - r3.Vector{X: 0, Y: 0, Z: 1}, - r3.Vector{X: 1, Y: 0, Z: 1}, - r3.Vector{X: 0, Y: 1, Z: 1}, - ), - planePt: r3.Vector{X: 0, Y: 0, Z: 0}, - planeNormal: r3.Vector{X: 0, Y: 0, Z: 1}, - wantP1: r3.Vector{}, - wantP2: r3.Vector{}, - wantExists: false, - }, - { - name: "triangle touches plane at vertex", - triangle: NewTriangle( - r3.Vector{X: 0, Y: 0, Z: 0}, - r3.Vector{X: 1, Y: 0, Z: 1}, - r3.Vector{X: 0, Y: 1, Z: 1}, - ), - planePt: r3.Vector{X: 0, Y: 0, Z: 0}, - planeNormal: r3.Vector{X: 0, Y: 0, Z: 1}, - wantP1: r3.Vector{X: 0, Y: 0, Z: 0}, - wantP2: r3.Vector{X: 0, Y: 0, Z: 0}, - wantExists: true, - }, - } - - for _, tt := range tests { - t.Run(tt.name, func(t *testing.T) { - gotP1, gotP2, gotExists := TrianglePlaneIntersectingSegment(tt.triangle, tt.planePt, tt.planeNormal) - test.That(t, tt.wantExists, test.ShouldEqual, gotExists) - if tt.wantExists { - test.That(t, tt.wantP1.ApproxEqual(gotP1), test.ShouldBeTrue) - test.That(t, tt.wantP2.ApproxEqual(gotP2), test.ShouldBeTrue) - } - }) - } -}