-
Notifications
You must be signed in to change notification settings - Fork 110
/
point.go
143 lines (123 loc) · 4.28 KB
/
point.go
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
package spatialmath
import (
"encoding/json"
"math"
"github.com/golang/geo/r3"
commonpb "go.viam.com/api/common/v1"
)
// PointCreator implements the GeometryCreator interface for point structs.
type pointCreator struct {
offset Pose
label string
}
// point is a collision geometry that represents a single point in 3D space that occupies no geometry.
type point struct {
pose Pose
label string
}
// NewPointCreator instantiates a PointCreator class, which allows instantiating point geometries given only a pose which is applied
// at the specified offset from the pose. These pointers have dimensions given by the provided halfSize vector.
func NewPointCreator(offset Pose, label string) GeometryCreator {
return &pointCreator{offset, label}
}
// NewGeometry instantiates a new point from a PointCreator class.
func (pc *pointCreator) NewGeometry(pose Pose) Geometry {
return &point{Compose(pc.offset, pose), pc.label}
}
func (pc *pointCreator) Offset() Pose {
return pc.offset
}
func (pc *pointCreator) MarshalJSON() ([]byte, error) {
config, err := NewGeometryConfig(pc)
if err != nil {
return nil, err
}
return json.Marshal(config)
}
// NewPoint instantiates a new point Geometry.
func NewPoint(pt r3.Vector, label string) Geometry {
return &point{NewPoseFromPoint(pt), label}
}
// Label returns the labels of this point.
func (pt *point) Label() string {
if pt != nil {
return pt.label
}
return ""
}
// Pose returns the pose of the point.
func (pt *point) Pose() Pose {
return pt.pose
}
// Vertices returns the vertices defining the point.
func (pt *point) Vertices() []r3.Vector {
return []r3.Vector{pt.pose.Point()}
}
// AlmostEqual compares the point with another geometry and checks if they are equivalent.
func (pt *point) AlmostEqual(g Geometry) bool {
other, ok := g.(*point)
if !ok {
return false
}
return PoseAlmostEqual(pt.pose, other.pose)
}
// Transform premultiplies the point pose with a transform, allowing the point to be moved in space.
func (pt *point) Transform(toPremultiply Pose) Geometry {
return &point{Compose(toPremultiply, pt.pose), pt.label}
}
// ToProto converts the point to a Geometry proto message.
func (pt *point) ToProtobuf() *commonpb.Geometry {
return &commonpb.Geometry{
Center: PoseToProtobuf(pt.pose),
GeometryType: &commonpb.Geometry_Sphere{
Sphere: &commonpb.Sphere{
RadiusMm: 0,
},
},
}
}
// CollidesWith checks if the given point collides with the given geometry and returns true if it does.
func (pt *point) CollidesWith(g Geometry) (bool, error) {
if other, ok := g.(*box); ok {
return pointVsBoxCollision(other, pt.pose.Point()), nil
}
if other, ok := g.(*sphere); ok {
return sphereVsPointDistance(other, pt.pose.Point()) <= 0, nil
}
if other, ok := g.(*point); ok {
return pt.AlmostEqual(other), nil
}
return true, newCollisionTypeUnsupportedError(pt, g)
}
// CollidesWith checks if the given point collides with the given geometry and returns true if it does.
func (pt *point) DistanceFrom(g Geometry) (float64, error) {
if other, ok := g.(*box); ok {
return pointVsBoxDistance(other, pt.pose.Point()), nil
}
if other, ok := g.(*sphere); ok {
return sphereVsPointDistance(other, pt.pose.Point()), nil
}
if other, ok := g.(*point); ok {
return pt.pose.Point().Sub(other.pose.Point()).Norm(), nil
}
return math.Inf(-1), newCollisionTypeUnsupportedError(pt, g)
}
// EncompassedBy returns a bool describing if the given point is completely encompassed by the given geometry.
func (pt *point) EncompassedBy(g Geometry) (bool, error) {
return pt.CollidesWith(g)
}
// pointVsBoxCollision takes a box and a point as arguments and returns a bool describing if they are in collision. \
// true == collision / false == no collision.
func pointVsBoxCollision(b *box, pt r3.Vector) bool {
return b.closestPoint(pt).Sub(pt).Norm() <= 0
}
// pointVsBoxDistance takes a box and a point as arguments and returns a floating point number. If this number is nonpositive it represents
// the penetration depth of the point within the box. If the returned float is positive it represents the separation distance between the
// point and the box, which are not in collision.
func pointVsBoxDistance(b *box, pt r3.Vector) float64 {
distance := b.closestPoint(pt).Sub(pt).Norm()
if distance > 0 {
return distance
}
return -b.penetrationDepth(pt)
}