pysim: added euler312 conversions

This commit is contained in:
Andrew Tridgell 2015-01-29 08:43:56 +11:00
parent f00861d2f1
commit edad8da613
1 changed files with 73 additions and 1 deletions

View File

@ -161,7 +161,7 @@ class Matrix3:
def to_euler(self):
'''find Euler angles for the matrix'''
'''find Euler angles (321 convention) for the matrix'''
if self.c.x >= 1.0:
pitch = pi
elif self.c.x <= -1.0:
@ -172,6 +172,40 @@ class Matrix3:
yaw = atan2(self.b.x, self.a.x)
return (roll, pitch, yaw)
def to_euler312(self):
'''find Euler angles (312 convention) for the matrix.
See http://www.atacolorado.com/eulersequences.doc
'''
T21 = self.a.y
T22 = self.b.y
T23 = self.c.y
T13 = self.c.x
T33 = self.c.z
yaw = atan2(-T21, T22)
roll = asin(T23)
pitch = atan2(-T13, T33)
return (yaw, roll, pitch)
def from_euler312(self, yaw, roll, pitch):
'''fill the matrix from Euler angles in radians in 312 convention'''
c3 = cos(pitch)
s3 = sin(pitch)
s2 = sin(roll)
c2 = cos(roll)
s1 = sin(yaw)
c1 = cos(yaw)
self.a.x = c1 * c3 - s1 * s2 * s3
self.b.y = c1 * c2
self.c.z = c3 * c2
self.a.y = -c2*s1
self.a.z = s3*c1 + c3*s2*s1
self.b.x = c3*s1 + s3*s2*c1
self.b.z = s1*s3 - s2*c1*c3
self.c.x = -s3*c2
self.c.y = s2
def __add__(self, m):
return Matrix3(self.a + m.a, self.b + m.b, self.c + m.c)
@ -262,7 +296,45 @@ def test_euler():
if diff.length() > 1.0e-12:
print('EULER ERROR:', v1, v2, diff.length())
def test_euler312_single(y,r,p):
'''check that from_euler312() and to_euler312() are consistent for one set of values'''
from math import degrees, radians
m = Matrix3()
m.from_euler312(radians(y), radians(r), radians(p))
(y2, r2, p2) = m.to_euler312()
v1 = Vector3(y,r,p)
v2 = Vector3(degrees(y2),degrees(r2),degrees(p2))
diff = v1 - v2
if diff.length() > 1.0e-12:
print('EULER ERROR:', v1, v2, diff.length())
def test_one_axis(y,r,p):
'''check that from_euler312() and from_euler() are consistent for one set of values on one axis'''
from math import degrees, radians
m = Matrix3()
m.from_euler312(radians(y), radians(r), radians(p))
(r2, p2, y2) = m.to_euler()
v1 = Vector3(y,r,p)
v2 = Vector3(degrees(y2),degrees(r2),degrees(p2))
diff = v1 - v2
if diff.length() > 1.0e-12:
print('EULER ERROR:', v1, v2, diff.length())
def test_euler312():
'''check that from_euler312() and to_euler312() are consistent'''
m = Matrix3()
for x in range(-89, 89, 3):
test_one_axis(x, 0, 0)
test_one_axis(0, x, 0)
test_one_axis(0, 0, x)
for r in range(-89, 89, 3):
for p in range(-179, 179, 3):
for y in range(-179, 179, 3):
test_euler312_single(y,r,p)
if __name__ == "__main__":
import doctest
doctest.testmod()
test_euler()
test_euler312()