-
Notifications
You must be signed in to change notification settings - Fork 3
/
math_utils.go
98 lines (82 loc) · 1.76 KB
/
math_utils.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
package quickhull
import (
"math"
"github.com/golang/geo/r3"
)
const (
minUint = uint(0)
maxUint = ^minUint
//minInt = -maxInt - 1
maxInt = int(maxUint >> 1)
)
func triangleNormal(a r3.Vector, b r3.Vector, c r3.Vector) r3.Vector {
x := a.X - c.X
y := a.Y - c.Y
z := a.Z - c.Z
rhsX := b.X - c.X
rhsY := b.Y - c.Y
rhsZ := b.Z - c.Z
return r3.Vector{
X: y*rhsZ - z*rhsY,
Y: z*rhsX - x*rhsZ,
Z: x*rhsY - y*rhsX,
}
}
func signedDistanceToPlane(v r3.Vector, p plane) float64 {
return p.n.Dot(v) + p.d
}
func squaredDistanceBetweenPointAndRay(p r3.Vector, r ray) float64 {
s := p.Sub(r.s)
t := s.Dot(r.v)
return s.Norm2() - t*t*r.vInvLengthSquared
}
// Find indices of extreme values (max x, min x, max y, min y, max z, min z) for the given point cloud
func extremeValues(vertexData []r3.Vector) (extremeValueIndices [6]int) {
vd0 := vertexData[0]
extremeVals := [6]float64{vd0.X, vd0.X, vd0.Y, vd0.Y, vd0.Z, vd0.Z}
xv := func(i, i2 int, v float64, idx int) {
if v > extremeVals[i] {
extremeVals[i] = v
extremeValueIndices[i] = idx
} else if v < extremeVals[i2] {
extremeVals[i2] = v
extremeValueIndices[i2] = idx
}
}
for i, pos := range vertexData[1:] {
idx := i + 1
xv(0, 1, pos.X, idx)
xv(2, 3, pos.Y, idx)
xv(4, 5, pos.Z, idx)
}
return
}
type vectorField int
const (
vfX vectorField = iota
vfY
vfZ
)
// Compute scale of the vertex data.
func scale(vertexData []r3.Vector, extremeValueIndices [6]int) (s float64) {
scl := func(i int, valType vectorField) {
v := vertexData[extremeValueIndices[i]]
var a float64
switch valType {
case vfX:
a = v.X
case vfY:
a = v.Y
case vfZ:
a = v.Z
}
s = math.Max(s, math.Abs(a))
}
scl(0, vfX)
scl(1, vfX)
scl(2, vfY)
scl(3, vfY)
scl(4, vfZ)
scl(5, vfZ)
return
}