# Path: imusim/trajectories/base.py
# class PositionTrajectory(AbstractTrajectory):
#     """Represents a continous trajectory of positions"""
# 
#     @abstractmethod
#     def position(self, t):
#         """
#         Return position in the global frame at time t.
# 
#         @param t: the time at which to evaluate the position
#         @return: the position vector at time t
#         """
#         pass
# 
#     @abstractmethod
#     def velocity(self, t):
#         """
#         Return velocity in the global frame at time t.
# 
#         @param t: the time at which to evaluate the velocity
#         @return: the velocity vector at time t
#         """
#         pass
# 
#     @abstractmethod
#     def acceleration(self, t):
#         """
#         Return acceleration in the global frame at time t.
# 
#         @param t: the time at which to evaluate the acceleration
#         @return: the acceleration vector at time t
#         """
#         pass
# 
# class RotationTrajectory(AbstractTrajectory):
#     """
#     Represents a continuous trajectory of rotations
#     """
# 
#     @abstractmethod
#     def rotation(self,t):
#         """
#         Return rotation relative to the global frame at time t.
# 
#         @param t: the time at which to evaluate the rotation
#         @return: the rotation L{Quaternion} at time t
#         """
#         pass
# 
#     @abstractmethod
#     def rotationalVelocity(self, t):
#         """
#         Return rotational velocity relative to the global frame at time t.
# 
#         @param t: the time at which to evaluate the rotational velocity
#         @return: the rotational velocity vector at time t
#         """
#         pass
# 
#     @abstractmethod
#     def rotationalAcceleration(self, t):
#         """
#         Return rotational acceleration relative to the global frame at time t.
# 
#         @param t: the time at which to evaluate the rotational acceleration
#         @return: the rotational accelration vector at time t
#         """
#         pass
# 
# Path: imusim/utilities/caching.py
# class CacheLastValue(object):
#     """
#     Provides a decorator for methods of the form f(t)
#     to cache the last value
# 
#     @param tolerance: tolerance to use when checking that two times are equal.
#     """
# 
#     def __init__(self, tolerance=1e-6):
#         self._tolerance = tolerance
#         self._cache = defaultdict(CacheEntry)
# 
#     def __call__(self, method):
#         tolerance = self._tolerance
#         closure_abs = abs
#         cache = self._cache
# 
#         @wraps(method)
#         def checkcache(obj, t):
#             cacheEntry = cache[obj]
#             try:
#                 if closure_abs(t - cacheEntry.time) < tolerance:
#                     return cacheEntry.result
#                 cacheEntry.time = t
#                 cacheEntry.result = method(obj, t)
#                 return cacheEntry.result
#             except:
#                 return method(obj, t)
#         return checkcache


import numpy as np
from imusim.maths import vectors
from imusim.trajectories.base import PositionTrajectory, RotationTrajectory
from imusim.maths.quaternions import Quaternion
from imusim.utilities.caching import CacheLastValue
from imusim.utilities.documentation import prepend_method_doc
        local co-ordinate frame
    @ivar rotationOffset: the rotation offset from the parent trajectory
    """

    def __init__(self, parent, positionOffset=None, rotationOffset=None):
        """
        Initialise trajectory.

        @param parent: the L{AbstractTrajectory} which this trajectory follows.
        @param positionOffset: the offset vector from the parent trajectory in the
            co-ordinate frame of the parent. (3x1 L{np.ndarray})
        @param rotationOffset: the rotational offset from the parent trajectory
            (L{Quaternion})
        """
        self.parent = parent
        if positionOffset is None:
            self.positionOffset = np.zeros((3,1))
        else:
            self.positionOffset = positionOffset
        if rotationOffset is None:
            self.rotationOffset = Quaternion()
        else:
            self.rotationOffset = rotationOffset

    @CacheLastValue()
    def position(self, t):
        p = self.parent.position(t)
        r = self.parent.rotation(t)
        return p + r.rotateVector(self.positionOffset)


# Path: imusim/trajectories/base.py
# class PositionTrajectory(AbstractTrajectory):
#     """Represents a continous trajectory of positions"""
# 
#     @abstractmethod
#     def position(self, t):
#         """
#         Return position in the global frame at time t.
# 
#         @param t: the time at which to evaluate the position
#         @return: the position vector at time t
#         """
#         pass
# 
#     @abstractmethod
#     def velocity(self, t):
#         """
#         Return velocity in the global frame at time t.
# 
#         @param t: the time at which to evaluate the velocity
#         @return: the velocity vector at time t
#         """
#         pass
# 
#     @abstractmethod
#     def acceleration(self, t):
#         """
#         Return acceleration in the global frame at time t.
# 
#         @param t: the time at which to evaluate the acceleration
#         @return: the acceleration vector at time t
#         """
#         pass
# 
# class RotationTrajectory(AbstractTrajectory):
#     """
#     Represents a continuous trajectory of rotations
#     """
# 
#     @abstractmethod
#     def rotation(self,t):
#         """
#         Return rotation relative to the global frame at time t.
# 
#         @param t: the time at which to evaluate the rotation
#         @return: the rotation L{Quaternion} at time t
#         """
#         pass
# 
#     @abstractmethod
#     def rotationalVelocity(self, t):
#         """
#         Return rotational velocity relative to the global frame at time t.
# 
#         @param t: the time at which to evaluate the rotational velocity
#         @return: the rotational velocity vector at time t
#         """
#         pass
# 
#     @abstractmethod
#     def rotationalAcceleration(self, t):
#         """
#         Return rotational acceleration relative to the global frame at time t.
# 
#         @param t: the time at which to evaluate the rotational acceleration
#         @return: the rotational accelration vector at time t
#         """
#         pass
# 
# Path: imusim/utilities/caching.py
# class CacheLastValue(object):
#     """
#     Provides a decorator for methods of the form f(t)
#     to cache the last value
# 
#     @param tolerance: tolerance to use when checking that two times are equal.
#     """
# 
#     def __init__(self, tolerance=1e-6):
#         self._tolerance = tolerance
#         self._cache = defaultdict(CacheEntry)
# 
#     def __call__(self, method):
#         tolerance = self._tolerance
#         closure_abs = abs
#         cache = self._cache
# 
#         @wraps(method)
#         def checkcache(obj, t):
#             cacheEntry = cache[obj]
#             try:
#                 if closure_abs(t - cacheEntry.time) < tolerance:
#                     return cacheEntry.result
#                 cacheEntry.time = t
#                 cacheEntry.result = method(obj, t)
#                 return cacheEntry.result
#             except:
#                 return method(obj, t)
#         return checkcache


# Path: imusim/trajectories/offset.py
import numpy as np
from imusim.maths import vectors
from imusim.trajectories.base import PositionTrajectory, RotationTrajectory
from imusim.maths.quaternions import Quaternion
from imusim.utilities.caching import CacheLastValue
from imusim.utilities.documentation import prepend_method_doc

        local co-ordinate frame
    @ivar rotationOffset: the rotation offset from the parent trajectory
    """

    def __init__(self, parent, positionOffset=None, rotationOffset=None):
        """
        Initialise trajectory.

        @param parent: the L{AbstractTrajectory} which this trajectory follows.
        @param positionOffset: the offset vector from the parent trajectory in the
            co-ordinate frame of the parent. (3x1 L{np.ndarray})
        @param rotationOffset: the rotational offset from the parent trajectory
            (L{Quaternion})
        """
        self.parent = parent
        if positionOffset is None:
            self.positionOffset = np.zeros((3,1))
        else:
            self.positionOffset = positionOffset
        if rotationOffset is None:
            self.rotationOffset = Quaternion()
        else:
            self.rotationOffset = rotationOffset

    @CacheLastValue()
    def position(self, t):
        p = self.parent.position(t)
        r = self.parent.rotation(t)
        return p + r.rotateVector(self.positionOffset)


    @CacheLastValue()