# Path: SafeRLBench/envs/_quadrocopter/transformations.py
# def vector_norm(data, axis=None, out=None):
#     """Return length, i.e. eucledian norm, of ndarray along axis.
# 
#     >>> v = numpy.random.random(3)
#     >>> n = vector_norm(v)
#     >>> numpy.allclose(n, numpy.linalg.norm(v))
#     True
#     >>> v = numpy.random.rand(6, 5, 3)
#     >>> n = vector_norm(v, axis=-1)
#     >>> numpy.allclose(n, numpy.sqrt(numpy.sum(v*v, axis=2)))
#     True
#     >>> n = vector_norm(v, axis=1)
#     >>> numpy.allclose(n, numpy.sqrt(numpy.sum(v*v, axis=1)))
#     True
#     >>> v = numpy.random.rand(5, 4, 3)
#     >>> n = numpy.empty((5, 3), dtype=numpy.float64)
#     >>> vector_norm(v, axis=1, out=n)
#     >>> numpy.allclose(n, numpy.sqrt(numpy.sum(v*v, axis=1)))
#     True
#     >>> vector_norm([])
#     0.0
#     >>> vector_norm([1.0])
#     1.0
# 
#     """
#     data = numpy.array(data, dtype=numpy.float64, copy=True)
#     if out is None:
#         if data.ndim == 1:
#             return math.sqrt(numpy.dot(data, data))
#         data *= data
#         out = numpy.atleast_1d(numpy.sum(data, axis=axis))
#         numpy.sqrt(out, out)
#         return out
#     else:
#         data *= data
#         numpy.sum(data, axis=axis, out=out)
#         numpy.sqrt(out, out)
# 
# def quaternion_multiply(quaternion1, quaternion0):
#     """Return multiplication of two quaternions.
# 
#     >>> q = quaternion_multiply([1, -2, 3, 4], [-5, 6, 7, 8])
#     >>> numpy.allclose(q, [-44, -14, 48, 28])
#     True
# 
#     """
#     x0, y0, z0, w0 = quaternion0
#     x1, y1, z1, w1 = quaternion1
#     return numpy.array((
#          x1*w0 + y1*z0 - z1*y0 + w1*x0,
#         -x1*z0 + y1*w0 + z1*x0 + w1*y0,
#          x1*y0 - y1*x0 + z1*w0 + w1*z0,
#         -x1*x0 - y1*y0 - z1*z0 + w1*w0), dtype=numpy.float64)
# 
# def quaternion_conjugate(quaternion):
#     """Return conjugate of quaternion.
# 
#     >>> q0 = random_quaternion()
#     >>> q1 = quaternion_conjugate(q0)
#     >>> q1[3] == q0[3] and all(q1[:3] == -q0[:3])
#     True
# 
#     """
#     return numpy.array((-quaternion[0], -quaternion[1],
#                         -quaternion[2], quaternion[3]), dtype=numpy.float64)
# 
# def quaternion_matrix(quaternion):
#     """Return homogeneous rotation matrix from quaternion.
# 
#     >>> R = quaternion_matrix([0.06146124, 0, 0, 0.99810947])
#     >>> numpy.allclose(R, rotation_matrix(0.123, (1, 0, 0)))
#     True
# 
#     """
#     q = numpy.array(quaternion[:4], dtype=numpy.float64, copy=True)
#     nq = numpy.dot(q, q)
#     if nq < _EPS:
#         return numpy.identity(4)
#     q *= math.sqrt(2.0 / nq)
#     q = numpy.outer(q, q)
#     return numpy.array((
#         (1.0-q[1, 1]-q[2, 2],     q[0, 1]-q[2, 3],     q[0, 2]+q[1, 3], 0.0),
#         (    q[0, 1]+q[2, 3], 1.0-q[0, 0]-q[2, 2],     q[1, 2]-q[0, 3], 0.0),
#         (    q[0, 2]-q[1, 3],     q[1, 2]+q[0, 3], 1.0-q[0, 0]-q[1, 1], 0.0),
#         (                0.0,                 0.0,                 0.0, 1.0)
#         ), dtype=numpy.float64)
# 
# def quaternion_about_axis(angle, axis):
#     """Return quaternion for rotation about axis.
# 
#     >>> q = quaternion_about_axis(0.123, (1, 0, 0))
#     >>> numpy.allclose(q, [0.06146124, 0, 0, 0.99810947])
#     True
# 
#     """
#     quaternion = numpy.zeros((4, ), dtype=numpy.float64)
#     quaternion[:3] = axis[:3]
#     qlen = vector_norm(quaternion)
#     if qlen > _EPS:
#         quaternion *= math.sin(angle/2.0) / qlen
#     quaternion[3] = math.cos(angle/2.0)
#     return quaternion


import math
import numpy as np
from .transformations import (vector_norm,
                              quaternion_multiply,
                              quaternion_conjugate,
                              quaternion_matrix,
                              quaternion_about_axis)
        return angle / dt * r[:3] / vector_norm(r[:3])


def apply_omega_to_quat(q, omega, dt):
    """
    Convert a quaternion q and apply the angular velocity omega to it over dt.

    Parameters:
    -----------
    q: quaternion
    omega: ndarray
        angular velocity
    dt: float
        time difference

    Returns:
    --------
    quaternion
        The quaternion of the orientation after rotation with omega for dt
        seconds.
    """
    # rotation angle around each axis
    w = omega * dt

    # only rotate if the angle we rotate through is actually significant
    if vector_norm(w) < np.finfo(float).eps * 4.0:
        return q

    # quaternion corresponding to this rotation
    # w = 0 is not a problem because numpy is awesome

# Path: SafeRLBench/envs/_quadrocopter/transformations.py
# def vector_norm(data, axis=None, out=None):
#     """Return length, i.e. eucledian norm, of ndarray along axis.
# 
#     >>> v = numpy.random.random(3)
#     >>> n = vector_norm(v)
#     >>> numpy.allclose(n, numpy.linalg.norm(v))
#     True
#     >>> v = numpy.random.rand(6, 5, 3)
#     >>> n = vector_norm(v, axis=-1)
#     >>> numpy.allclose(n, numpy.sqrt(numpy.sum(v*v, axis=2)))
#     True
#     >>> n = vector_norm(v, axis=1)
#     >>> numpy.allclose(n, numpy.sqrt(numpy.sum(v*v, axis=1)))
#     True
#     >>> v = numpy.random.rand(5, 4, 3)
#     >>> n = numpy.empty((5, 3), dtype=numpy.float64)
#     >>> vector_norm(v, axis=1, out=n)
#     >>> numpy.allclose(n, numpy.sqrt(numpy.sum(v*v, axis=1)))
#     True
#     >>> vector_norm([])
#     0.0
#     >>> vector_norm([1.0])
#     1.0
# 
#     """
#     data = numpy.array(data, dtype=numpy.float64, copy=True)
#     if out is None:
#         if data.ndim == 1:
#             return math.sqrt(numpy.dot(data, data))
#         data *= data
#         out = numpy.atleast_1d(numpy.sum(data, axis=axis))
#         numpy.sqrt(out, out)
#         return out
#     else:
#         data *= data
#         numpy.sum(data, axis=axis, out=out)
#         numpy.sqrt(out, out)
# 
# def quaternion_multiply(quaternion1, quaternion0):
#     """Return multiplication of two quaternions.
# 
#     >>> q = quaternion_multiply([1, -2, 3, 4], [-5, 6, 7, 8])
#     >>> numpy.allclose(q, [-44, -14, 48, 28])
#     True
# 
#     """
#     x0, y0, z0, w0 = quaternion0
#     x1, y1, z1, w1 = quaternion1
#     return numpy.array((
#          x1*w0 + y1*z0 - z1*y0 + w1*x0,
#         -x1*z0 + y1*w0 + z1*x0 + w1*y0,
#          x1*y0 - y1*x0 + z1*w0 + w1*z0,
#         -x1*x0 - y1*y0 - z1*z0 + w1*w0), dtype=numpy.float64)
# 
# def quaternion_conjugate(quaternion):
#     """Return conjugate of quaternion.
# 
#     >>> q0 = random_quaternion()
#     >>> q1 = quaternion_conjugate(q0)
#     >>> q1[3] == q0[3] and all(q1[:3] == -q0[:3])
#     True
# 
#     """
#     return numpy.array((-quaternion[0], -quaternion[1],
#                         -quaternion[2], quaternion[3]), dtype=numpy.float64)
# 
# def quaternion_matrix(quaternion):
#     """Return homogeneous rotation matrix from quaternion.
# 
#     >>> R = quaternion_matrix([0.06146124, 0, 0, 0.99810947])
#     >>> numpy.allclose(R, rotation_matrix(0.123, (1, 0, 0)))
#     True
# 
#     """
#     q = numpy.array(quaternion[:4], dtype=numpy.float64, copy=True)
#     nq = numpy.dot(q, q)
#     if nq < _EPS:
#         return numpy.identity(4)
#     q *= math.sqrt(2.0 / nq)
#     q = numpy.outer(q, q)
#     return numpy.array((
#         (1.0-q[1, 1]-q[2, 2],     q[0, 1]-q[2, 3],     q[0, 2]+q[1, 3], 0.0),
#         (    q[0, 1]+q[2, 3], 1.0-q[0, 0]-q[2, 2],     q[1, 2]-q[0, 3], 0.0),
#         (    q[0, 2]-q[1, 3],     q[1, 2]+q[0, 3], 1.0-q[0, 0]-q[1, 1], 0.0),
#         (                0.0,                 0.0,                 0.0, 1.0)
#         ), dtype=numpy.float64)
# 
# def quaternion_about_axis(angle, axis):
#     """Return quaternion for rotation about axis.
# 
#     >>> q = quaternion_about_axis(0.123, (1, 0, 0))
#     >>> numpy.allclose(q, [0.06146124, 0, 0, 0.99810947])
#     True
# 
#     """
#     quaternion = numpy.zeros((4, ), dtype=numpy.float64)
#     quaternion[:3] = axis[:3]
#     qlen = vector_norm(quaternion)
#     if qlen > _EPS:
#         quaternion *= math.sin(angle/2.0) / qlen
#     quaternion[3] = math.cos(angle/2.0)
#     return quaternion


# Path: SafeRLBench/envs/_quadrocopter/quaternions.py
import math
import numpy as np
from .transformations import (vector_norm,
                              quaternion_multiply,
                              quaternion_conjugate,
                              quaternion_matrix,
                              quaternion_about_axis)

        return angle / dt * r[:3] / vector_norm(r[:3])


def apply_omega_to_quat(q, omega, dt):
    """
    Convert a quaternion q and apply the angular velocity omega to it over dt.

    Parameters:
    -----------
    q: quaternion
    omega: ndarray
        angular velocity
    dt: float
        time difference

    Returns:
    --------
    quaternion
        The quaternion of the orientation after rotation with omega for dt
        seconds.
    """
    # rotation angle around each axis
    w = omega * dt

    # only rotate if the angle we rotate through is actually significant
    if vector_norm(w) < np.finfo(float).eps * 4.0:
        return q

    # quaternion corresponding to this rotation
    # w = 0 is not a problem because numpy is awesome

    r = quaternion_about_axis(vector_norm(w), w)