def test_update_spaces_1(self):
from guitarist.iksolver.iksolver import _update_spaces
spaces = np.zeros((1, 4, 4), dtype=np.double)
spaces[0] = [[1, 0, 0, 2], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]]
angles = [np.pi / 2]
_update_spaces(spaces, angles)
assert np.allclose(spaces[0], [[0, -1, 0, 2], [1, 0, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]], 1e-5, 1e-8)
def test_update_spaces_random(self):
spaces = np.zeros((3, 4, 4), dtype=np.double)
from guitarist.iksolver.iksolver import _update_spaces
for t in range(100):
spaces[0] = spaces[1] = spaces[2] = np.eye(4)
angles = np.random.rand(3)
_update_spaces(spaces, angles)
for i in range(len(spaces)):
space = spaces[i]
x = space[0:3, 0]
y = space[0:3, 1]
z = space[0:3, 2]
assert np.allclose(np.dot(x, y), 0, 1e-8, 1e-8)
assert np.allclose(np.dot(y, z), 0, 1e-8, 1e-8)
assert np.allclose(np.dot(z, x), 0, 1e-8, 1e-8)
assert np.allclose(np.dot(x, x), 1, 1e-8, 1e-8)
assert np.allclose(np.dot(y, y), 1, 1e-8, 1e-8)
assert np.allclose(np.dot(z, z), 1, 1e-8, 1e-8)
