simplified and tested
This commit is contained in:
parent
dc508cdfce
commit
2dc46b783a
|
@ -112,11 +112,8 @@ class Rotation:
|
||||||
B = 2.0 * np.dot(self.quaternion[1:],other)
|
B = 2.0 * np.dot(self.quaternion[1:],other)
|
||||||
C = 2.0 * _P*self.quaternion[0]
|
C = 2.0 * _P*self.quaternion[0]
|
||||||
|
|
||||||
return np.array([
|
return A*other + B*self.quaternion[1:] + C * np.cross(self.quaternion[1:],other)
|
||||||
A*other[0] + B*self.quaternion[1] + C*(self.quaternion[2]*other[2] - self.quaternion[3]*other[1]),
|
|
||||||
A*other[1] + B*self.quaternion[2] + C*(self.quaternion[3]*other[0] - self.quaternion[1]*other[2]),
|
|
||||||
A*other[2] + B*self.quaternion[3] + C*(self.quaternion[1]*other[1] - self.quaternion[2]*other[0]),
|
|
||||||
])
|
|
||||||
elif other.shape == (3,3,): # rotate a single (3x3)-matrix
|
elif other.shape == (3,3,): # rotate a single (3x3)-matrix
|
||||||
return np.dot(self.asMatrix(),np.dot(other,self.asMatrix().T))
|
return np.dot(self.asMatrix(),np.dot(other,self.asMatrix().T))
|
||||||
elif other.shape == (3,3,3,3,):
|
elif other.shape == (3,3,3,3,):
|
||||||
|
|
|
@ -300,3 +300,10 @@ class TestRotation:
|
||||||
f = Rotation._get_pyramid_order(a,'forward')
|
f = Rotation._get_pyramid_order(a,'forward')
|
||||||
b = Rotation._get_pyramid_order(a,'backward')
|
b = Rotation._get_pyramid_order(a,'backward')
|
||||||
assert np.all(np.take_along_axis(np.take_along_axis(a,f,-1),b,-1) == a)
|
assert np.all(np.take_along_axis(np.take_along_axis(a,f,-1),b,-1) == a)
|
||||||
|
|
||||||
|
|
||||||
|
@pytest.mark.parametrize('x',[np.random.rand(3),
|
||||||
|
np.random.rand(3,3)])
|
||||||
|
def test_rotation_identity(self,x):
|
||||||
|
R = Rotation()
|
||||||
|
assert np.allclose(x,R*x)
|
||||||
|
|
Loading…
Reference in New Issue