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
|
try:
from . import generic as g
except BaseException:
import generic as g
tol_norm = 1e-10
class AlignTests(g.unittest.TestCase):
def test_align(self):
"""
Test aligning two 3D vectors
"""
# function we're testing
align = g.trimesh.geometry.align_vectors
is_rigid = g.trimesh.transformations.is_rigid
# start with some edge cases and make sure the transform works
target = g.np.array([0, 0, -1], dtype=g.np.float64)
vectors = g.np.vstack(
(
g.trimesh.unitize(g.random((1000, 3)) - 0.5),
g.random((1000, 3)) - 0.5,
[-target, target],
g.trimesh.util.generate_basis(target),
[
[7.12106798e-07, -7.43194705e-08, 1.00000000e00],
[0, 0, -1],
[1e-4, 1e-4, -1],
],
)
)
# collect errors
norms = []
unitized = g.trimesh.unitize(vectors)
for unit_dest, dest in zip(unitized[-10:], vectors[-10:]):
for unit, vector in zip(unitized, vectors):
T, _a = align(vector, dest, return_angle=True)
assert is_rigid(T)
assert g.np.isclose(g.np.linalg.det(T), 1.0)
# rotate vector with transform
check = g.np.dot(T[:3, :3], unit)
# compare to target vector
norm = g.np.linalg.norm(check - unit_dest)
norms.append(norm)
assert norm < tol_norm
norms = g.np.array(norms)
# these vectors should be perpendicular and zero
angles = [
align(i, target, return_angle=True)[1]
for i in g.trimesh.util.generate_basis(target)
]
assert g.np.allclose(angles, [g.np.pi / 2, g.np.pi / 2, 0.0])
def test_range(self):
# function we're testing
align = g.trimesh.geometry.align_vectors
is_rigid = g.trimesh.transformations.is_rigid
# generate angles from 0 to 180 degrees
angles = g.np.linspace(0.0, g.np.pi / 1e7, 10000)
# generate on- plane vectors
vectors = g.np.column_stack(
(g.np.cos(angles), g.np.sin(angles), g.np.zeros(len(angles)))
)
# rotate them arbitrarily off the plane just for funsies
vectors = g.trimesh.transform_points(vectors, g.transforms[20])
for angle, vector in zip(angles, vectors):
g.trimesh.util.generate_basis(vector)
# check alignment to first vector
# which was created with zero angle
T, a = align(vector, vectors[0], return_angle=True)
assert is_rigid(T)
# check to make sure returned angle corresponds with truth
assert g.np.isclose(a, angle, atol=1e-6)
# check to make sure returned transform is correct
check = g.np.dot(T[:3, :3], vector)
norm = g.np.linalg.norm(check - vectors[0])
assert norm < tol_norm
def test_rigid(self):
# check issues with near-reversed vectors not returning rigid
align = g.trimesh.geometry.align_vectors
T = align([0, 0, -1], [-1e-17, 1e-17, 1])
assert g.np.isclose(g.np.linalg.det(T), 1.0)
T = align([0, 0, -1], [-1e-4, 1e-4, 1])
assert g.np.isclose(g.np.linalg.det(T), 1.0)
vector_1 = g.np.array([7.12106798e-07, -7.43194705e-08, 1.00000000e00])
vector_2 = g.np.array([0, 0, -1])
T, _angle = align(vector_1, vector_2, return_angle=True)
assert g.np.isclose(g.np.linalg.det(T), 1.0)
if __name__ == "__main__":
g.trimesh.util.attach_to_log()
g.unittest.main()
|
