-
Notifications
You must be signed in to change notification settings - Fork 111
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
RSDK-9587 generate series of waypoints to cover surface given a pcd m…
…esh (#4719) Co-authored-by: Ray Bjorkman <[email protected]>
- Loading branch information
Showing
11 changed files
with
139 additions
and
159 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -1,115 +1,38 @@ | ||
| 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. | ||
| type mesh struct { | ||
| // 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 | ||
| 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), | ||
| // NewMesh creates a mesh from the given triangles and pose. | ||
| 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 | ||
| // Pose returns the pose of the mesh. | ||
| 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 | ||
| // Triangles returns the triangles associated with the mesh. | ||
| 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 | ||
| // 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{ | ||
| pose: Compose(pose, m.pose), | ||
| triangles: m.triangles, | ||
| } | ||
| } |
This file was deleted.
Oops, something went wrong.
Oops, something went wrong.