|
1 | 1 | package spatialmath |
2 | 2 |
|
3 | | -import ( |
4 | | - "github.com/golang/geo/r3" |
5 | | -) |
6 | | - |
7 | 3 | // This file incorporates work covered by the Brax project -- https://github.com/google/brax/blob/main/LICENSE. |
8 | 4 | // Copyright 2021 The Brax Authors, which is licensed under the Apache License Version 2.0 (the “License”). |
9 | 5 | // You may obtain a copy of the license at http://www.apache.org/licenses/LICENSE-2.0. |
10 | 6 |
|
11 | | -// mesh is a collision geometry that represents a set of triangles that represent a mesh. |
12 | | -type mesh struct { |
| 7 | +// Mesh is a set of triangles at some pose. Triangle points are in the frame of the mesh. |
| 8 | +type Mesh struct { |
13 | 9 | pose Pose |
14 | | - triangles []*triangle |
| 10 | + triangles []*Triangle |
15 | 11 | } |
16 | 12 |
|
17 | | -type triangle struct { |
18 | | - p0 r3.Vector |
19 | | - p1 r3.Vector |
20 | | - p2 r3.Vector |
21 | | - |
22 | | - normal r3.Vector |
23 | | -} |
24 | | - |
25 | | -func newTriangle(p0, p1, p2 r3.Vector) *triangle { |
26 | | - return &triangle{ |
27 | | - p0: p0, |
28 | | - p1: p1, |
29 | | - p2: p2, |
30 | | - normal: PlaneNormal(p0, p1, p2), |
| 13 | +// NewMesh creates a mesh from the given triangles and pose. |
| 14 | +func NewMesh(pose Pose, triangles []*Triangle) *Mesh { |
| 15 | + return &Mesh{ |
| 16 | + pose: pose, |
| 17 | + triangles: triangles, |
31 | 18 | } |
32 | 19 | } |
33 | 20 |
|
34 | | -// closestPointToCoplanarPoint takes a point, and returns the closest point on the triangle to the given point |
35 | | -// The given point *MUST* be coplanar with the triangle. If it is known ahead of time that the point is coplanar, this is faster. |
36 | | -func (t *triangle) closestPointToCoplanarPoint(pt r3.Vector) r3.Vector { |
37 | | - // Determine whether point is inside all triangle edges: |
38 | | - c0 := pt.Sub(t.p0).Cross(t.p1.Sub(t.p0)) |
39 | | - c1 := pt.Sub(t.p1).Cross(t.p2.Sub(t.p1)) |
40 | | - c2 := pt.Sub(t.p2).Cross(t.p0.Sub(t.p2)) |
41 | | - inside := c0.Dot(t.normal) <= 0 && c1.Dot(t.normal) <= 0 && c2.Dot(t.normal) <= 0 |
42 | | - |
43 | | - if inside { |
44 | | - return pt |
45 | | - } |
46 | | - |
47 | | - // Edge 1: |
48 | | - refPt := ClosestPointSegmentPoint(t.p0, t.p1, pt) |
49 | | - bestDist := pt.Sub(refPt).Norm2() |
50 | | - |
51 | | - // Edge 2: |
52 | | - point2 := ClosestPointSegmentPoint(t.p1, t.p2, pt) |
53 | | - if distsq := pt.Sub(point2).Norm2(); distsq < bestDist { |
54 | | - refPt = point2 |
55 | | - bestDist = distsq |
56 | | - } |
57 | | - |
58 | | - // Edge 3: |
59 | | - point3 := ClosestPointSegmentPoint(t.p2, t.p0, pt) |
60 | | - if distsq := pt.Sub(point3).Norm2(); distsq < bestDist { |
61 | | - return point3 |
62 | | - } |
63 | | - return refPt |
| 21 | +// Pose returns the pose of the mesh. |
| 22 | +func (m *Mesh) Pose() Pose { |
| 23 | + return m.pose |
64 | 24 | } |
65 | 25 |
|
66 | | -// closestPointToPoint takes a point, and returns the closest point on the triangle to the given point, as well as whether the point |
67 | | -// is on the edge of the triangle. |
68 | | -// This is slower than closestPointToCoplanarPoint. |
69 | | -func (t *triangle) closestPointToPoint(point r3.Vector) r3.Vector { |
70 | | - closestPtInside, inside := t.closestInsidePoint(point) |
71 | | - if inside { |
72 | | - return closestPtInside |
73 | | - } |
74 | | - |
75 | | - // If the closest point is outside the triangle, it must be on an edge, so we |
76 | | - // check each triangle edge for a closest point to the point pt. |
77 | | - closestPt := ClosestPointSegmentPoint(t.p0, t.p1, point) |
78 | | - bestDist := point.Sub(closestPt).Norm2() |
79 | | - |
80 | | - newPt := ClosestPointSegmentPoint(t.p1, t.p2, point) |
81 | | - if newDist := point.Sub(newPt).Norm2(); newDist < bestDist { |
82 | | - closestPt = newPt |
83 | | - bestDist = newDist |
84 | | - } |
85 | | - |
86 | | - newPt = ClosestPointSegmentPoint(t.p2, t.p0, point) |
87 | | - if newDist := point.Sub(newPt).Norm2(); newDist < bestDist { |
88 | | - return newPt |
89 | | - } |
90 | | - return closestPt |
| 26 | +// Triangles returns the triangles associated with the mesh. |
| 27 | +func (m *Mesh) Triangles() []*Triangle { |
| 28 | + return m.triangles |
91 | 29 | } |
92 | 30 |
|
93 | | -// closestInsidePoint returns the closest point on a triangle IF AND ONLY IF the query point's projection overlaps the triangle. |
94 | | -// Otherwise it will return the query point. |
95 | | -// To visualize this- if one draws a tetrahedron using the triangle and the query point, all angles from the triangle to the query point |
96 | | -// must be <= 90 degrees. |
97 | | -func (t *triangle) closestInsidePoint(point r3.Vector) (r3.Vector, bool) { |
98 | | - // Parametrize the triangle s.t. a point inside the triangle is |
99 | | - // Q = p0 + u * e0 + v * e1, when 0 <= u <= 1, 0 <= v <= 1, and |
100 | | - // 0 <= u + v <= 1. Let e0 = (p1 - p0) and e1 = (p2 - p0). |
101 | | - // We analytically minimize the distance between the point pt and Q. |
102 | | - e0 := t.p1.Sub(t.p0) |
103 | | - e1 := t.p2.Sub(t.p0) |
104 | | - a := e0.Norm2() |
105 | | - b := e0.Dot(e1) |
106 | | - c := e1.Norm2() |
107 | | - d := point.Sub(t.p0) |
108 | | - // The determinant is 0 only if the angle between e1 and e0 is 0 |
109 | | - // (i.e. the triangle has overlapping lines). |
110 | | - det := (a*c - b*b) |
111 | | - u := (c*e0.Dot(d) - b*e1.Dot(d)) / det |
112 | | - v := (-b*e0.Dot(d) + a*e1.Dot(d)) / det |
113 | | - inside := (0 <= u) && (u <= 1) && (0 <= v) && (v <= 1) && (u+v <= 1) |
114 | | - return t.p0.Add(e0.Mul(u)).Add(e1.Mul(v)), inside |
| 31 | +// Transform transforms the mesh. As triangles are in the mesh's frame, they are unchanged. |
| 32 | +func (m *Mesh) Transform(pose Pose) *Mesh { |
| 33 | + // Triangle points are in frame of mesh, like the corners of a box, so no need to transform them |
| 34 | + return &Mesh{ |
| 35 | + pose: Compose(pose, m.pose), |
| 36 | + triangles: m.triangles, |
| 37 | + } |
115 | 38 | } |
0 commit comments