def test_from_rotation_matrix():
np.testing.assert_array_equal(Quaternion.zero().to_rotation_matrix()[0],
np.identity(3))
np.testing.assert_array_equal(
Quaternion.from_rotation_matrix(np.identity(3)).data[0],
Quaternion.zero().data[0])
def test_axis_rates():
q = Quaternion.from_euler(Point(0.0, 0.0, np.pi / 2))
qdot = Quaternion.from_euler(Point(np.radians(5), 0.0, np.pi / 2))
rates = Quaternion.axis_rates(q, qdot)
np.testing.assert_almost_equal(np.degrees(rates.data), Point(0, 5, 0).data)
def test_to_euler():
qarr = Quaternion(np.random.random((2, 4))).norm()
eulers = qarr.to_euler()
checks = np.array(
[R.from_euler("xyz", eul.data[0]).as_quat() for eul in eulers])
np.testing.assert_array_almost_equal(checks, qarr.xyzw)
def test_norm():
qarr = Quaternion(np.random.random((2, 4)))
def npcheck(q1):
res = np.quaternion(*q1.data[0]).normalized()
return np.array([res.w, res.x, res.y, res.z])
earr = [npcheck(q) for q in qarr]
np.testing.assert_array_almost_equal(qarr.norm().data, earr)
def test_transform_point2():
tqs = Quaternion(np.random.random((10, 4))).norm()
np.testing.assert_array_almost_equal(
tqs.transform_point(Point(1, 1, 1)).data,
quaternion.rotate_vectors(np.array(
[np.quaternion(*q.data[0]) for q in tqs]),
Point(1, 1, 1).tile(1).data,
axis=1).reshape(10, 3))
def test_conjugate():
qarr = Quaternion(np.random.random((6, 4))).norm()
def npcheck(q1):
res = np.quaternion(*q1.data[0]).conjugate()
return np.array([res.w, res.x, res.y, res.z])
earr = np.array([npcheck(q) for q in qarr])
np.testing.assert_array_almost_equal(qarr.conjugate().data, earr)
def test_mul():
q1 = Quaternion(np.random.random((2, 4))).norm()
q2 = Quaternion(np.random.random((2, 4))).norm()
def npcheck(q1, q2):
res = np.quaternion(*q1.data[0]) * np.quaternion(*q2.data[0])
return np.array([res.w, res.x, res.y, res.z])
check = q1 * q2
check_npy = np.array([npcheck(_1, _2) for _1, _2 in zip(q1, q2)])
np.testing.assert_array_almost_equal(
check.data,
check_npy,
err_msg="failed to do Quaternions * Quaternions")
def test_body_diff():
qs = Quaternion.zero(100)
qs = qs.body_rotate(Point.X(1, 100) * np.linspace(0, np.pi, 100))
dt = np.ones(100)
dq = qs.body_diff(dt)
np.testing.assert_array_almost_equal(dq.data,
Point.X(np.pi / 100, 100).data)
def rotate(self, rotation: Quaternion) -> Mesh:
seen = []
for vertex in self.vertices:
if vertex in seen:
continue
v = rotation.rotate(vertex)
vertex.move_to(v)
seen.append(vertex)
return self
def rotate(self,
axis: Vector,
angle: float,
global_rotation: bool = False):
rot = Quaternion.axis_angle(axis=axis, angle=angle)
if global_rotation:
self.rotation = rot * self.rotation # rotate over current camera rotation
else:
self.rotation = self.rotation * rot # rotate under current camera rotation
self.update_data()
def __init__(self,
position: Vector = Vector(),
focal_length: float = 500,
viewport_offset: Vector = Vector()):
self.position = position.copy(label="camera")
self.rotation = Quaternion.identity()
self.bearing = Vector(z=1)
self.bearing.projection = Vector(z=1)
self.view_port = viewport_offset.copy(label="view_port")
self.view_port.z = focal_length
self.C = Vector(x=cos(0), y=cos(0), z=cos(0))
self.S = Vector(x=sin(0), y=sin(0), z=sin(0))
def points(self, euler=None, cg_pos=None):
""" Compute the points defining the glider
Returns points in NED coordinates
Arguments:
euler: optionally a numpy array of euler angles defining the
aircraft orientation. If not specified will use the euler
property of the class
cg_pos: optionally a numpy array defining the aircraft position. If
not specified will use the cg_pos property
Returns:
X: 3xn numpy array of points defining the aircraft. in NED
"""
if euler is not None:
self.euler = euler
if cg_pos is not None:
self.cg_pos = cg_pos
q = Quaternion()
q.from_euler(self.euler)
X_ac = np.array([q.rot(pt) for pt in self.local_points])
X_ac = enu_to_ned(X_ac)
return X_ac + self.cg_pos
def test_inverse():
q = Quaternion(1, 0, 0, 0)
assert q.norm() == Quaternion(1, 0, 0, 0)
def npcheck(q1):
res = np.quaternion(*q1.data[0]).inverse()
return np.array([res.w, res.x, res.y, res.z])
qarr = Quaternion(np.random.random((2, 4))).norm()
earr = [npcheck(q) for q in qarr]
np.testing.assert_array_almost_equal(qarr.inverse().data, earr)
def setup_hexacopter(lat, lon, alt) : m = Multicopter(lat, lon, alt) m.add_propeller(True, Quaternion.from_vector(2.0, 1.0, 0.0)) m.add_propeller(False, Quaternion.from_vector(0.0, 2.0, 0.0), Quaternion.from_axial_rotation(1.0, 0.0, 0.0, -15.0)) m.add_propeller(True, Quaternion.from_vector(-2.0, 1.0, 0.0)) m.add_propeller(False, Quaternion.from_vector(-2.0, -1.0, 0.0)) m.add_propeller(True, Quaternion.from_vector(0.0, -2.0, 0.0), Quaternion.from_axial_rotation(1.0, 0.0, 0.0, 15.0)) m.add_propeller(False, Quaternion.from_vector(2.0, -1.0, 0.0)) m.contact_lst.append(Quaternion.from_vector(1.0, 1.0, -1.0)) m.contact_lst.append(Quaternion.from_vector(1.0, -1.0, -1.0)) m.contact_lst.append(Quaternion.from_vector(-1.0, -1.0, -1.0)) m.contact_lst.append(Quaternion.from_vector(-1.0, 1.0, -1.0))
def test_init():
data = np.random.random((500, 4))
qs = Quaternion(data)
np.testing.assert_array_equal(data, qs.data)
def test_rotate():
q = Quaternion.from_euler(P0())
qdot = q.rotate(Point(0, 0, np.radians(5)))
assert qdot.transform_point(PX()).y == approx(np.sin(np.radians(5)))
def test_body_rotate():
q = Quaternion.from_euler(Point(0, 0, np.pi / 2))
qdot = q.body_rotate(Point(np.radians(5), 0, 0))
assert qdot.transform_point(Point(0, 1, 0)).z == np.sin(np.radians(5))
def test_body_rotate_zero():
qinit = Quaternion.from_euler(Point(0, 0, 0))
qdot = qinit.body_rotate(Point(0, 0, 0))
np.testing.assert_array_equal(list(qinit), list(qdot))
def test_to_from_axis_angle():
points = Point(np.random.random((100, 3)))
tqs = Quaternion.from_axis_angle(points)
tps = tqs.to_axis_angle()
np.testing.assert_array_almost_equal(points.data, tps.data)
def test_to_axis_angle():
q1 = Quaternion.from_euler(PZ(np.pi / 4))
assert q1.to_axis_angle() == PZ(np.pi / 4)
def test_from_euler():
parr = np.random.random((20, 3))
np.testing.assert_array_almost_equal(
Quaternion.from_euler(Point(parr)).xyzw,
R.from_euler('xyz', parr).as_quat())
from scene.entity import Entity
from scene.scene import Scene
from shape.cube import Cube
from shape.plane import Plane
geometry_options.line_thickness = 1
windowCenter = Vector(x=options.width / 2, y=options.height / 2, z=0)
origin = Vector(x=options.originOffset,
y=options.originOffset,
z=options.originOffset)
m = Mesh().import_from("ressources/Cylinder.obj")
m.scale(20)
m.rotate(rotation=Quaternion.axis_angle(Vector(y=1), 180))
cubeSize = 20
plane = Plane(length=4, grid_size=20.0)
plane.translate(Vector(x=-cubeSize, y=-cubeSize, z=cubeSize))
cube = Cube(cube_size=cubeSize)
meshes = [
cube,
m,
plane,
plane.copy().rotate(rotation=Quaternion.axis_angle(Vector(
x=1), angle=-90)),
plane.copy().rotate(rotation=Quaternion.axis_angle(Vector(y=1), angle=90)),
# cube,
# Cube(cube_size=cubeSize / 2, origin=Vector(y=(cubeSize + cubeSize / 2))),
# Cube(cube_size=cubeSize / 2, origin=Vector(y=-(cubeSize + cubeSize / 2))),
# Cube(cube_size=cubeSize / 2, origin=Vector(x=(cubeSize + cubeSize / 2))),
