591 lines
20 KiB
Python
591 lines
20 KiB
Python
|
#!/usr/bin/env python3
|
||
|
|
"""
|
||
|
|
section Platform Motion (geo.motion)
|
||
|
|
|
||
|
|
After all that, you still don't have platform motion. Enter the motion
|
||
|
|
module. It requires numpy, since you will have array_like attributes.
|
||
|
|
The SpaceCurve class is basically Vector which takes that into consider-
|
||
|
|
ation. (Recall the fundamental theorem of Space Curves? That curvature
|
||
|
|
and torision define a unique space curve? Yes, that one-- well space
|
||
|
|
curves define all that: velocity, normal, acceleration, angular velocity,
|
||
|
|
yadda yadda. They key property is you can define a local tangent frame,
|
||
|
|
with:
|
||
|
|
|
||
|
|
x parallel to the curve's velocity
|
||
|
|
y = z X x
|
||
|
|
z is some "z" orthogonal to x. The default "z" is DOWN, but you can
|
||
|
|
make it UP, or something else.
|
||
|
|
|
||
|
|
Hence, given a 3-DoF motion history, you get the transformation from
|
||
|
|
level cartesian space to the tangent frame. Now if you throw in attitude,
|
||
|
|
represented by any kind of rotation, boom, you have an affine
|
||
|
|
transformation to body coordinates.
|
||
|
|
|
||
|
|
But wait: these things all have array_like attributes, that means in
|
||
|
|
one object, you have the trasnformtion from a local pegged coordinate
|
||
|
|
system the body frame AT EVERY POINT IN THE MOTION HISTORY.
|
||
|
|
|
||
|
|
Now stop and think about that for a minute. IF you were still suffering
|
||
|
|
primative obession, using arrays for vectors, and using standalone
|
||
|
|
functions for coordinate transformations--you be in a real pickle.
|
||
|
|
All these arrays, which are JUST NUMBERS and have NO intrinsici meaning-
|
||
|
|
no you the developer has to keep it straight. Then, you have to pass
|
||
|
|
them to functions, and then to other functions-- how you deal with the
|
||
|
|
fact that the functions are not constant--- I don't know- but you do.
|
||
|
|
|
||
|
|
None of that. You got a gps history and an attitude history:
|
||
|
|
|
||
|
|
f = SpaceCurve(*gps).tld2body(imu)
|
||
|
|
|
||
|
|
f(look), f(target), etc...
|
||
|
|
|
||
|
|
does the whole affine transformation at every point.
|
||
|
|
"""
|
||
|
|
## \namespace ::geo::motion::motion SpaceCurve's and their Tangent Frames
|
||
|
|
__date__ = "10/30/2012"
|
||
|
|
__version__ = "1.21"
|
||
|
|
print ("importing %s version::%s"%(__name__, __version__))
|
||
|
|
|
||
|
|
import itertools
|
||
|
|
import operator
|
||
|
|
from functools import wraps
|
||
|
|
## you need numpy for this module to be useful
|
||
|
|
import numpy
|
||
|
|
|
||
|
|
## Note: this is an __all__ controlled import, so it does not pollute the
|
||
|
|
## namespace
|
||
|
|
from geo import euclid
|
||
|
|
from geo.euclid import charts
|
||
|
|
from geo.motion import dxdt
|
||
|
|
|
||
|
|
|
||
|
|
## Reative Body Frame Directions
|
||
|
|
UP = euclid.Z
|
||
|
|
DOWN = -UP
|
||
|
|
FORWARD = euclid.X
|
||
|
|
BACKWARD = -FORWARD
|
||
|
|
|
||
|
|
|
||
|
|
## tangent-left-up to tangent-right-down transformation
|
||
|
|
tlu2trd = charts.Roll(180)
|
||
|
|
## tangent-right-downto tangent-left-up transformation
|
||
|
|
trd2tlu = ~tlu2trd
|
||
|
|
|
||
|
|
## This is a derivative decorator:\n
|
||
|
|
## \f$D_n(f) \rightarrow \frac{d^nf}{ds^n} \f$ \n
|
||
|
|
## If you can follow how this works, then you might be a python guru.
|
||
|
|
def derivative(n):
|
||
|
|
"""derivative(n) returns a function decorator--which takes an input function
|
||
|
|
and returns a function that compute the n-th derivative of the function"""
|
||
|
|
def dnf_dtn(func):
|
||
|
|
"""The is the decorator that decorates it's argument "func",
|
||
|
|
making it into the n-th derivative of func, e.g:
|
||
|
|
|
||
|
|
if func(x) --> x**4, and n=2, then
|
||
|
|
|
||
|
|
dnf_dtn --> 12*x**2
|
||
|
|
"""
|
||
|
|
# Usage: decorators with arguments compute a new decorator
|
||
|
|
# (here, dnf_dtn) which then decorates the method, turning it into
|
||
|
|
# dfunc_dt. dfunc_dt calls the prime method with the derivative operator
|
||
|
|
# raised to "n". Finally, functools.wraps decorates it so that the user
|
||
|
|
# has access original method's docstring (that's all @wraps does).
|
||
|
|
# That it's a deocrator that returns a decoraated decorator should not
|
||
|
|
# be seen as "obscure"-- it's just the right way to do it.
|
||
|
|
@wraps(func)
|
||
|
|
def dfunc_dt(self):
|
||
|
|
dt = self.Dx(self.t)
|
||
|
|
return self.broadcast(func).prime(dt=dt**n)
|
||
|
|
return dfunc_dt
|
||
|
|
return dnf_dtn
|
||
|
|
|
||
|
|
|
||
|
|
## \f$ \vec{f(\vec{v})} \rightarrow \vec{f}/||v|| \f$ \n
|
||
|
|
## This decorator normalizes the output with abs(self):
|
||
|
|
def normalized(func):
|
||
|
|
""" func() --> func()/||self|| decorator"""
|
||
|
|
@wraps(func)
|
||
|
|
def nfunc(self, *args, **kwargs):
|
||
|
|
return func(self, *args, **kwargs)/abs(self)
|
||
|
|
return nfunc
|
||
|
|
|
||
|
|
## \f$ \vec{f(\vec{v})} \rightarrow \vec{f}/||f|| \f$ \n This decorator
|
||
|
|
## normalizes the output with abs(output)
|
||
|
|
def hat(func):
|
||
|
|
"""func() --> func().hat() decorator"""
|
||
|
|
@wraps(func)
|
||
|
|
def hfunc(self, *args, **kwargs):
|
||
|
|
return func(self, *args, **kwargs).hat()
|
||
|
|
return hfunc
|
||
|
|
|
||
|
|
|
||
|
|
## \f$ f(\vec{v}) \rightarrow 1/f \f$ \n This decorator returns the
|
||
|
|
## reciprocol of the function
|
||
|
|
def radius_of(func):
|
||
|
|
"""func --> 1/func decorator"""
|
||
|
|
@wraps(func)
|
||
|
|
def rfunc(self, *args, **kwargs):
|
||
|
|
return func(self, *args, **kwargs)**(-1)
|
||
|
|
return rfunc
|
||
|
|
|
||
|
|
## \f$ \vec{f(\vec{v})} \rightarrow ||f|| \f$ \n This decorator returns
|
||
|
|
## the magnitude of the vector function
|
||
|
|
def magnitude(func):
|
||
|
|
"""func --> |func| decorator"""
|
||
|
|
@wraps(func)
|
||
|
|
def mfunc(self, *args, **kwargs):
|
||
|
|
return abs(func(self, *args, **kwargs))
|
||
|
|
return mfunc
|
||
|
|
|
||
|
|
|
||
|
|
## \f$ f(\vec{v}) \rightarrow O(*f) \f$ \n A decorator for operators of
|
||
|
|
## functions-- now this is just silly.
|
||
|
|
def starfunc(op):
|
||
|
|
"""starfunc(op) decorator takes a binary operator "op"
|
||
|
|
and wraps a method that MUST return 2 objects: left
|
||
|
|
and right, and returns:
|
||
|
|
|
||
|
|
op(left, right)
|
||
|
|
"""
|
||
|
|
def starop(func):
|
||
|
|
@wraps(func)
|
||
|
|
def starF(self, *args, **kwargs):
|
||
|
|
return op(*func(self, *args, **kwargs))
|
||
|
|
return starF
|
||
|
|
return starop
|
||
|
|
|
||
|
|
## Vector with <a href="http://mathworld.wolfram.com/SpaceCurve.html">
|
||
|
|
## Space Curve</a> capabilities
|
||
|
|
class SpaceCurve(euclid.Vector):
|
||
|
|
"""v = SpaceCurve(x, y, z)
|
||
|
|
|
||
|
|
A space curve is a Vector, with the expectation
|
||
|
|
that:
|
||
|
|
|
||
|
|
x
|
||
|
|
y
|
||
|
|
z
|
||
|
|
|
||
|
|
are 1 dimensional numpy.ndarray with at least 3 points, on which various
|
||
|
|
numeric derivative operations are performed
|
||
|
|
"""
|
||
|
|
|
||
|
|
## \f$ D_x \f$ Differential operator wrt \f$x\f$\n see http://en.wikipedia.org/wiki/Differential_operator \n see http://docs.scipy.org/doc/scipy/reference/misc.html for other derivative functions \n This is an Inner Class used for differentiation, and if you don't like, you can overide it and use your own differentiater-- just set SpaceCurve.Dx = my favorite differental operator, of course, it may need to support composition (but not if you only need a tangent plane coordinate system)
|
||
|
|
class Dx(object):
|
||
|
|
"""Derivative operator class, so for example:
|
||
|
|
|
||
|
|
D = Dx(x) -- D is d/dx
|
||
|
|
|
||
|
|
So, for exmaple:
|
||
|
|
|
||
|
|
D(special.jn(n,x)) = special.jvp(n,x,1) --D(y) = y'(x)
|
||
|
|
pow(D,2,(special.jn(n,x)) = special.jvp(n,x,2) --D^2(y) = y"(x)
|
||
|
|
|
||
|
|
(D**2)(y) --> lammda y: pow(D,2,y) also...
|
||
|
|
|
||
|
|
Of course, x must be defined sensibly
|
||
|
|
|
||
|
|
NOTE: this class uses dxdt.deriv , so if you don't
|
||
|
|
like it, change it.
|
||
|
|
|
||
|
|
"""
|
||
|
|
## x in \f$ \frac{d}{dx} \f$
|
||
|
|
def __init__(self, x):
|
||
|
|
self.x = x
|
||
|
|
return None
|
||
|
|
## Dx(x)(y) = \f$ \frac{dy}{dx} \f$, a wrapper deriv() .
|
||
|
|
def __call__(self, y):
|
||
|
|
return dxdt.deriv(self.x, y)
|
||
|
|
## \f$\frac{d^n y}{dx^n} = {\rm pow}(x, n, y) \f$ = pow(x, n, y)
|
||
|
|
def __pow__(self, n, y=None):
|
||
|
|
return (
|
||
|
|
lambda y: pow(self, n, y) if y is None else
|
||
|
|
eval('self('*n+'y'+')'*n) if n > 0 else (~self)**(-n)
|
||
|
|
)
|
||
|
|
## Add derivatives
|
||
|
|
def __add__(self, other):
|
||
|
|
return lambda y: self(y) + other(y)
|
||
|
|
## R to L composition
|
||
|
|
def __mul__(self, other):
|
||
|
|
return lambda y: self(other(y))
|
||
|
|
## Dilation
|
||
|
|
def __rmul__(self, other):
|
||
|
|
return lambda y: other*self(y)
|
||
|
|
pass
|
||
|
|
|
||
|
|
## A Vector (elementwise) with optional t parameter
|
||
|
|
def __init__(self, x, y, z, t=None):
|
||
|
|
super(SpaceCurve, self).__init__(x, y, z)
|
||
|
|
## t is not in use yet, and when it is, it preculde the t decorator.
|
||
|
|
self.t = np.arange(len(x)) if t is None else t
|
||
|
|
return None
|
||
|
|
|
||
|
|
## len() --> length of the attributes (and they have to be equal)
|
||
|
|
def __len__(self):
|
||
|
|
try:
|
||
|
|
lens = map(len, self.iter())
|
||
|
|
except TypeError:
|
||
|
|
raise TypeError("Spacecurve's attributes have no len()")
|
||
|
|
pass
|
||
|
|
if lens[0] == lens[1] == lens[2]:
|
||
|
|
return lens[0]
|
||
|
|
raise ValueError("SpaceCurve's attributes have differnent len()'s.")
|
||
|
|
|
||
|
|
## \f$ \vec{v}'_i = \frac{dv_i}{dt} \f$ \n Vector derivative with resept to argument or index, keywords specify \f$t\f$ and/or \f$\partial \f$, or \f$\frac{d}{dt} \f$.
|
||
|
|
def prime(self, t=None, dx=None, dt=None):
|
||
|
|
"""dT/dt = T.prime([t=None [, dx=None, [dt=None, [inplace=None]]])
|
||
|
|
|
||
|
|
t defaults to (1,2,..., len(T)) if is None
|
||
|
|
|
||
|
|
dx controls numeric differentiation with respect to t, and
|
||
|
|
defaults to Dx(t), evaulated as dx(t)-- so if you want to overide
|
||
|
|
with a better (e.g. filtered derrivative), it has to be curried--
|
||
|
|
that is:
|
||
|
|
|
||
|
|
dx(t, x) = dx(t)(x).
|
||
|
|
|
||
|
|
Now if you was dt, then
|
||
|
|
|
||
|
|
inplace is not supported.
|
||
|
|
"""
|
||
|
|
dt = self.Dx(self.t)
|
||
|
|
return self.broadcast(dt)
|
||
|
|
|
||
|
|
## This is the 0-th order derivative, and it starts the decorator off
|
||
|
|
def __call__(self, t=None, dx=None):
|
||
|
|
return self
|
||
|
|
|
||
|
|
## \f$ \vec{v} \equiv \frac{d\vec{r}}{dt} \f$ \n
|
||
|
|
## <a href="http://mathworld.wolfram.com/Velocity.html for details.">
|
||
|
|
## Velocity</a>, as a derivative() decorated SpaceCurve.__call__
|
||
|
|
@derivative(1)
|
||
|
|
def velocity(self):
|
||
|
|
"""1st derivative of self()"""
|
||
|
|
return self
|
||
|
|
|
||
|
|
|
||
|
|
## \f$ \vec{a} \equiv \frac{d\vec{v}}{dt} = \frac{d^2\vec{r}}{dt^2} \f$ \n
|
||
|
|
## <a href="http://mathworld.wolfram.com/Acceleration.html for details.">
|
||
|
|
## Velocity</a>, as a derivative() decorated SpaceCurve.__call__
|
||
|
|
@derivative(2)
|
||
|
|
def acceleration(self):
|
||
|
|
"""2nd derivative of self()"""
|
||
|
|
return self
|
||
|
|
|
||
|
|
## \f$ \vec{j} \equiv \frac{d\vec{a}}{dt} = \frac{d^3\vec{r}}{dt^3} \f$ \n
|
||
|
|
## <a href="see http://en.wikipedia.org/wiki/Jerk_(physics)">Jerk</a>, as
|
||
|
|
## a derivative() decorated SpaceCurve.__call__
|
||
|
|
@derivative(3)
|
||
|
|
def jerk(self):
|
||
|
|
"""3rd derivative of self()"""
|
||
|
|
return self
|
||
|
|
|
||
|
|
## \f$ \vec{s} \equiv \frac{d\vec{j}}{dt} = \frac{d^4\vec{r}}{dt^4} \f$\n
|
||
|
|
## <a href="see http://en.wikipedia.org/wiki/Jounce)">Jounce</a>, aka
|
||
|
|
## SpaceCurve.Snap, as a derivative() decorated SpaceCurve.__call__
|
||
|
|
@derivative(4)
|
||
|
|
def jounce(self):
|
||
|
|
"""4th derivative of self()"""
|
||
|
|
return self
|
||
|
|
|
||
|
|
## Snap is Jounce
|
||
|
|
snap = jounce
|
||
|
|
|
||
|
|
## \f$ \frac{{d^5}\vec{r}}{dt^5} \f$ \n
|
||
|
|
## <a hef="http://en.wikipedia.org/wiki/Jounce">Crackle</a>, asa a
|
||
|
|
## derivative() decorated SpaceCurve.__call__
|
||
|
|
@derivative(5)
|
||
|
|
def crackle(self):
|
||
|
|
"""5th derivative of self()"""
|
||
|
|
return self
|
||
|
|
|
||
|
|
## \f$ \frac{{d^6}\vec{r}}{dt^6} \f$ \n
|
||
|
|
## <a hef="http://en.wikipedia.org/wiki/Jounce">Pop</a>, asa a derivative()
|
||
|
|
## decorated SpaceCurve.__call__
|
||
|
|
@derivative(6)
|
||
|
|
def pop(self):
|
||
|
|
"""6th derivative of self()"""
|
||
|
|
return self
|
||
|
|
|
||
|
|
## \f$ \frac{{d^7}\vec{r}}{dt^7} \f$ \n
|
||
|
|
## <a hef="http://en.wikipedia.org/wiki/Jounce">Lock</a>, asa a
|
||
|
|
## derivative() decorated SpaceCurve.__call__
|
||
|
|
@derivative(7)
|
||
|
|
def lock(self):
|
||
|
|
"""7th derivative of self()"""
|
||
|
|
return self
|
||
|
|
|
||
|
|
## \f$ \frac{{d^8}\vec{r}}{dt^8} \f$ \n
|
||
|
|
## <a hef="http://en.wikipedia.org/wiki/Jounce">Drop</a>, asa a
|
||
|
|
## derivative() decorated SpaceCurve.__call__
|
||
|
|
@derivative(8)
|
||
|
|
def drop(self):
|
||
|
|
"""8th derivative of self()"""
|
||
|
|
return self
|
||
|
|
|
||
|
|
## \f$ \frac{{d^9}\vec{r}}{dt^9} \f$ \n
|
||
|
|
## <a hef="http://en.wikipedia.org/wiki/Jounce">Shot</a>, asa a
|
||
|
|
## derivative() decorated SpaceCurve.__call__
|
||
|
|
@derivative(9)
|
||
|
|
def shot(self):
|
||
|
|
"""9th derivative of self()"""
|
||
|
|
return self
|
||
|
|
|
||
|
|
## \f$ \frac{{d^{10}}\vec{r}}{dt^{10}} \f$ \n
|
||
|
|
## <a hef="http://en.wikipedia.org/wiki/Jounce">Put</a>, asa a
|
||
|
|
## derivative() decorated SpaceCurve.__call__
|
||
|
|
@derivative(10)
|
||
|
|
def put(self):
|
||
|
|
"""10th derivative of self()"""
|
||
|
|
return self
|
||
|
|
|
||
|
|
|
||
|
|
## \f$ |\vec{v}| \f$ \n
|
||
|
|
## <a href="http://mathworld.wolfram.com/Speed.html"> Speed</a>, as a
|
||
|
|
# magnitude() decorated velocity()
|
||
|
|
@magnitude
|
||
|
|
def speed(self):
|
||
|
|
return self.velocity(t, dx, dt)
|
||
|
|
|
||
|
|
|
||
|
|
## \f$ \hat{T} \equiv \hat{v} \f$ \n
|
||
|
|
## <a href="http://mathworld.wolfram.com/TangentVector.html">Tangent Vector
|
||
|
|
## </a>, as a hat() decorated velocity()
|
||
|
|
@hat
|
||
|
|
def tangent(self):
|
||
|
|
"""Tangent is the 'hat'-ed Velocity"""
|
||
|
|
return self.velocity(t, dx, dt)
|
||
|
|
|
||
|
|
|
||
|
|
## \f$ \hat{N} \equiv \hat{\dot{\hat{T}}} = \frac{d\hat{T}}{dt}/|\frac{d\hat{T}}{dt}| \f$
|
||
|
|
## <a href="http://mathworld.wolfram.com/NormalVector.html">Normal Vector
|
||
|
|
## </a>, as a starfunc() decorated binormal() and tangent().
|
||
|
|
@starfunc(operator.__xor__)
|
||
|
|
def normal(self):
|
||
|
|
"""Normal is the cross product of the binormal and the Tangent"""
|
||
|
|
return self.binormal(), self.tangent()
|
||
|
|
|
||
|
|
## \f$ \vec{\omega} = \vec{v}/r \f$ \n
|
||
|
|
## <a href="http://mathworld.wolfram.com/AngularVelocity.html">
|
||
|
|
## Angular Velocity</a>, as a normalized() decorated velocity()
|
||
|
|
@normalized
|
||
|
|
def angular_velocity(self):
|
||
|
|
"""angular_velocity os the normalized velocity"""
|
||
|
|
return self.velocity()
|
||
|
|
|
||
|
|
## \f$ \vec{\alpha} = \vec{\omega}' = \vec{a}/r \f$ \n
|
||
|
|
## <a href="http://mathworld.wolfram.com/AngularAcceleration.html">
|
||
|
|
## Angular Acceleration</a>, as a normalized() decorated acceleration().
|
||
|
|
@normalized
|
||
|
|
def angular_acceleration(self):
|
||
|
|
"""AngularAccelration os the normalized Acceleration"""
|
||
|
|
return self.acceleration()
|
||
|
|
|
||
|
|
|
||
|
|
## \f$ \vec{B} = \vec{v} \times \vec{a} \f$ \n as a starfunc() __xor__ decorated velocity() and v.v
|
||
|
|
@starfunc(operator.__xor__)
|
||
|
|
def velocity_X_acceleration(self):
|
||
|
|
"""Velocity crossed with Acceleration"""
|
||
|
|
v = self.Velocity(t, dx, dt)
|
||
|
|
return v, v.velocity()
|
||
|
|
|
||
|
|
## \f$ \hat{B} = \vec{B}/|\vec{B}| \f$ \n
|
||
|
|
## <a href="http://mathworld.wolfram.com/BinormalVector.html">Binormal
|
||
|
|
## vector</a> as a hat() decorated velocity_X_acceleration()
|
||
|
|
@hat
|
||
|
|
def binormal(self):
|
||
|
|
"""Binormal is the 'hat'-ed velocity_X_acceleration"""
|
||
|
|
return self.velocity_X_acceleration()
|
||
|
|
|
||
|
|
## \f$ \tau = [\vec{v},\vec{a}, \dot{a}]/\sigma^2 \f$ \n
|
||
|
|
## <a href="http://mathworld.wolfram.com/Torsion.html">Torsion</a> via
|
||
|
|
## euclid.scalar_triple_product()
|
||
|
|
@starfunc(operator.__div__)
|
||
|
|
def torsion(self, t=None, dx=None, dt=None):
|
||
|
|
"""torsion: -- it's along story"""
|
||
|
|
v = self.velocity()
|
||
|
|
a = self.acceleration()
|
||
|
|
j = self.Jerk()
|
||
|
|
return (
|
||
|
|
euclid.scalar_triple_product(v, a, j),
|
||
|
|
(self.velocity_X_acceleration())**2
|
||
|
|
)
|
||
|
|
|
||
|
|
## \f$ \kappa = \frac{|\vec{v} \times \vec{a}|}{|\vec{v}|^3} \f$ \n
|
||
|
|
## <a href="http://mathworld.wolfram.com/Curvature.html">The Curvature</a>
|
||
|
|
@starfunc(operator.__div__)
|
||
|
|
def curvature(self):
|
||
|
|
"""||V X A||/||V||**3"""
|
||
|
|
v = self.velocity()
|
||
|
|
a = v.velocity()
|
||
|
|
return abs(v^a), abs(v)**3
|
||
|
|
|
||
|
|
## \f$ \vec{C} = \tau \vec{T} + \kappa \vec{B} \f$ \n
|
||
|
|
## <a href=" http://mathworld.wolfram.com/Centrode.html">The Centrode</a>
|
||
|
|
## in terms of the torsion(), curvature(), tangent() vector and the
|
||
|
|
## binormal() vector;
|
||
|
|
def centrode(self):
|
||
|
|
return (
|
||
|
|
self.torsion()*self.tangent() +
|
||
|
|
self.curvature()*self.binormal()
|
||
|
|
)
|
||
|
|
|
||
|
|
## The Darboux vector is the centrode
|
||
|
|
Darboux = centrode
|
||
|
|
|
||
|
|
## \f$ \sigma = 1/\tau \f$ \n
|
||
|
|
## <a href="http://mathworld.wolfram.com/RadiusofTorsion.html">Radius of
|
||
|
|
## torsion</a> as a radius_of() decorated torsion().
|
||
|
|
@radius_of
|
||
|
|
def radius_of_torsion(self):
|
||
|
|
"""radius_of() torsion"""
|
||
|
|
return self.torsion()
|
||
|
|
|
||
|
|
## \f$ \rho^2 = 1/ |v \times a |^2 \f$ \n <a href="http://mathworld.wolfram.com/RadiusofCurvature.html">Radius of curvature</a> as a radius_of() decorated curvature()
|
||
|
|
@radius_of
|
||
|
|
def radius_of_curvature(self):
|
||
|
|
"""radius_of() curvature"""
|
||
|
|
return self.curvature()
|
||
|
|
|
||
|
|
## \f$ s = \int_{\gamma} ds = \int_{\gamma} |\dot{\vec{r}}| \f$\n
|
||
|
|
## <a href="http://mathworld.wolfram.com/ArcLength.html">Arc Length</a>
|
||
|
|
## (computed for fixed time steps).
|
||
|
|
def arc_length(self, axis=None):
|
||
|
|
"""TODO: use scipy to integrate to make a nested function...."""
|
||
|
|
return self.speed().cumsum(axis=axis)
|
||
|
|
|
||
|
|
## \f$ {\bf T} = \hat{x}\hat{T}+\hat{y}\hat{N} + \hat{z}\hat{B} \f$ \n
|
||
|
|
## <a href="http://mathworld.wolfram.com/Trihedron.html">The Trihedron
|
||
|
|
## Tensor</a>
|
||
|
|
def trihedron(self):
|
||
|
|
return euclid.ziprows(self.tangent(), self.normal(), self.binormal())
|
||
|
|
|
||
|
|
## TNB
|
||
|
|
TNB = trihedron
|
||
|
|
|
||
|
|
## \f$ \kappa \f$, read-only curvature()
|
||
|
|
@property
|
||
|
|
def kappa(self):
|
||
|
|
return self.curvature()
|
||
|
|
|
||
|
|
## \f$ \tau \f$, read-only torsion()
|
||
|
|
@property
|
||
|
|
def tau(self):
|
||
|
|
return self.torsion()
|
||
|
|
|
||
|
|
## \f$ \vec{T} \f$, read-only tangent()
|
||
|
|
@property
|
||
|
|
def T(self):
|
||
|
|
return self.tangent()
|
||
|
|
|
||
|
|
## \f$ \vec{B} \f$, read-only normal()
|
||
|
|
@property
|
||
|
|
def N(self):
|
||
|
|
return self.normal()
|
||
|
|
|
||
|
|
## \f$ \vec{B} \f$ , read-only binormal()
|
||
|
|
@property
|
||
|
|
def B(self):
|
||
|
|
return self.binormal()
|
||
|
|
|
||
|
|
@property
|
||
|
|
def Tprime(self):
|
||
|
|
return self.kappa*self.N
|
||
|
|
|
||
|
|
@property
|
||
|
|
def Nprime(self):
|
||
|
|
return -self.kappa*self.T + self.tau*self.B
|
||
|
|
|
||
|
|
@property
|
||
|
|
def Bprime(self):
|
||
|
|
return -self.tau*self.B
|
||
|
|
|
||
|
|
## \f$ {\bf \vec{T}'} = {\bf \vec{\omega} \times \vec{T}} \f$ \n
|
||
|
|
## \f$ {\bf \vec{N}'} = {\bf \vec{\omega} \times \vec{N}} \f$ \n
|
||
|
|
## \f$ {\bf \vec{B}'} = {\bf \vec{\omega} \times \vec{B}} \f$ \n
|
||
|
|
## <a href="http://en.wikipedia.org/wiki/Frenet-Serret_formulas">
|
||
|
|
## Frenet-Serret</a> formuale.
|
||
|
|
def TNBPrime(self):
|
||
|
|
return euclid.ziprows(*map(self.Darboux().cross, self.TNB()))
|
||
|
|
|
||
|
|
|
||
|
|
## Matplotlib plotter
|
||
|
|
def plot3d(self, f=0):
|
||
|
|
import pylab
|
||
|
|
import matplotlib.pyplot as plt
|
||
|
|
from mpl_toolkits.mplot3d import Axes3D
|
||
|
|
from mpl_toolkits.mplot3d import axes3d
|
||
|
|
fig = plt.figure(f)
|
||
|
|
ax = Axes3D(fig)
|
||
|
|
ax.plot(self.x, self.y, self.z)
|
||
|
|
pylab.xlabel("x")
|
||
|
|
pylab.ylabel("y")
|
||
|
|
pylab.show()
|
||
|
|
return ax
|
||
|
|
|
||
|
|
## Recast as a lowly Vector
|
||
|
|
def vector(self):
|
||
|
|
return euclid.Vector(self.x, self.y, self.z)
|
||
|
|
|
||
|
|
## return space curve
|
||
|
|
def space_curve(self):
|
||
|
|
return self
|
||
|
|
|
||
|
|
## Compute tangent plane triplet from SpaceCurve \f${\bf \vec{r}}\f$:\n
|
||
|
|
## \f$ \bf{ \hat{x} } \propto {\bf \hat{r'}} \f$ \n
|
||
|
|
## \f$ {\bf \hat{z}} \propto {\bf \hat{z}} - {\bf (\hat{z}\cdot\hat{x})\hat{x}} \f$ \n
|
||
|
|
## \f$ \bf{ \hat{y} }= {\bf \hat{z} \times \hat{x} } \f$ \n
|
||
|
|
## (where "prime" is differentiation via prime())\n
|
||
|
|
## keyword z defines body z coordinate.
|
||
|
|
def tangent_frame_triplet(self, z=DOWN):
|
||
|
|
"""i, j, k = r.tangent_frame_triplet([z=DOWN])
|
||
|
|
|
||
|
|
i, j, k for a right handed orthonormal triplet, with:
|
||
|
|
|
||
|
|
i parallel to r.velocity()
|
||
|
|
j is k^i (cross product)
|
||
|
|
k is keyword z's normalized vector rejection of i
|
||
|
|
"""
|
||
|
|
i = self.tangent().vector()
|
||
|
|
k = (z.VectorRejection(i)).hat()
|
||
|
|
j = (k^i)
|
||
|
|
|
||
|
|
return i, j, k
|
||
|
|
|
||
|
|
|
||
|
|
## Tangent, Level, Up' to Level \n: Rotation connecting
|
||
|
|
## tangent-to-ellipsoid to tangent-to-motion frame \n computed from
|
||
|
|
## itertools.product:\n take all dot product combinations (as numbers,
|
||
|
|
## not euclid.Scalar() ) and make a euclid.Matrix().
|
||
|
|
def tlu2level(self):
|
||
|
|
return euclid.Matrix(
|
||
|
|
*[
|
||
|
|
(e_body*e_level).w for e_body, e_level in itertools.product(
|
||
|
|
self.tangent_frame_triplet(z=UP),
|
||
|
|
euclid.BASIS
|
||
|
|
)
|
||
|
|
]
|
||
|
|
).versor()
|
||
|
|
|
||
|
|
## invert tlu2level()
|
||
|
|
def level2tlu(self):
|
||
|
|
return ~(self.tlu2level())
|
||
|
|
|
||
|
|
## compose level2tlu() and tlu2rd()
|
||
|
|
def level2trd(self):
|
||
|
|
return self.level2tlu.compose(self.tlu2trd())
|
||
|
|
|
||
|
|
## Compute level frame to body frame-- with keyword defined system
|
||
|
|
## (TLU or TRD)
|
||
|
|
def level2body(self, imu, method=level2tlu):
|
||
|
|
return (operator.methodcaller(method)(self)).compose(imu)
|
||
|
|
|
||
|
|
## To Be Debugged
|
||
|
|
def level2body_affine(self, imu, method=level2tlu):
|
||
|
|
from geo.euclid.affine import Affine
|
||
|
|
print ("order not debugged")
|
||
|
|
R = self.level2body(imu, method=method)
|
||
|
|
T = -(~R)(self)
|
||
|
|
return Affine(R, T)
|
||
|
|
pass
|