-
Notifications
You must be signed in to change notification settings - Fork 110
/
point.go
138 lines (120 loc) · 4.14 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
package spatialmath
import (
"encoding/json"
"fmt"
"math"
"github.com/golang/geo/r3"
commonpb "go.viam.com/api/common/v1"
)
// point is a collision geometry that represents a single point in 3D space that occupies no geometry.
type point struct {
position r3.Vector
label string
}
// NewPoint instantiates a new point Geometry.
func NewPoint(pt r3.Vector, label string) Geometry {
return &point{pt, label}
}
// String returns a human readable string that represents the pointCreator.
func (pt *point) String() string {
p := pt.position
return fmt.Sprintf("Type: Point, Location X:%.0f, Y:%.0f, Z:%.0f", p.X, p.Y, p.Z)
}
func (pt point) MarshalJSON() ([]byte, error) {
config, err := NewGeometryConfig(&pt)
if err != nil {
return nil, err
}
return json.Marshal(config)
}
// Label returns the labels of this point.
func (pt *point) Label() string {
return pt.label
}
// SetLabel sets the label of this point.
func (pt *point) SetLabel(label string) {
pt.label = label
}
// Pose returns the pose of the point.
func (pt *point) Pose() Pose {
return NewPoseFromPoint(pt.position)
}
// 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 pt.position.ApproxEqual(other.position)
}
// 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, NewPoseFromPoint(pt.position)).Point(), pt.label}
}
// ToProto converts the point to a Geometry proto message.
func (pt *point) ToProtobuf() *commonpb.Geometry {
return &commonpb.Geometry{
Center: PoseToProtobuf(NewPoseFromPoint(pt.position)),
GeometryType: &commonpb.Geometry_Sphere{
Sphere: &commonpb.Sphere{
RadiusMm: 0,
},
},
Label: pt.label,
}
}
// 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(pt.position, other), nil
}
if other, ok := g.(*sphere); ok {
return sphereVsPointDistance(other, pt.position) <= 0, nil
}
if other, ok := g.(*capsule); ok {
return capsuleVsPointDistance(other, pt.position) <= 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(pt.position, other), nil
}
if other, ok := g.(*sphere); ok {
return sphereVsPointDistance(other, pt.position), nil
}
if other, ok := g.(*capsule); ok {
return capsuleVsPointDistance(other, pt.position), nil
}
if other, ok := g.(*point); ok {
return pt.position.Sub(other.position).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(pt r3.Vector, b *box) 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(pt r3.Vector, b *box) float64 {
distance := b.closestPoint(pt).Sub(pt).Norm()
if distance > 0 {
return distance
}
return -b.pointPenetrationDepth(pt)
}
// ToPointCloud converts a point geometry into a []r3.Vector.
func (pt *point) ToPoints(resolution float64) []r3.Vector {
return []r3.Vector{pt.position}
}