from sympy import init_printing
init_printing(use_latex=True)

from sympy import Symbol, symbols
from sympy.abc import a, b, c

from sympy.physics.mechanics import ReferenceFrame

A = ReferenceFrame('A', latexs=['\mathbf{i}', '\mathbf{j}', '\mathbf{k}'])
A.x + A.y + A.z

type(A.x)

2 * A.x

# También usando símbolos
a * (A.x + 2 * A.y)

4 + A.z  # Type Error

from sympy.physics.mechanics import dot, cross

dot(A.x, A.y)

dot(A.x, A.x)

A.x & A.y

A.x & A.x

cross(A.x, A.x)

cross(A.x, A.y)

A.x ^ A.x

A.x ^ A.y

v = -A.x + a * A.y
v

v.magnitude()

v.normalize()

v.diff(a, A)  # Se especifica el sistema de referencia

A1 = ReferenceFrame('A_1', latexs=['\mathbf{i_1}', '\mathbf{j_1}', '\mathbf{k_1}'])

A.x + A1.x

(A.x + A1.x).args

phi = Symbol('phi')

A1.orient(A, 'Axis', [phi, A.z])  # Rotación phi alrededor del eje A.z
A1.dcm(A)  # "Direct Cosine Matrix"

from sympy.physics.mechanics import dynamicsymbols

Omega = dynamicsymbols('Omega')

XYZ = ReferenceFrame("u", latexs=['\mathbf{i}', '\mathbf{j}', '\mathbf{k}'])
X1Y1Z = XYZ.orientnew("v", 'Axis', [Omega, XYZ.z], latexs=['\mathbf{i}_1', '\mathbf{j}_1', '\mathbf{k}_1'])

X1Y1Z.dcm(XYZ)

R = Symbol('R')
r = R * X1Y1Z.x
r

r.dt(X1Y1Z)

# Equivalente, mejor no usar
t = Symbol('t')
r.diff(t, X1Y1Z)

v = r.dt(XYZ)
v.simplify()
v

v.express(XYZ)

v.express(XYZ).args

r = Symbol('r')
e_b, e_a = symbols('e_b, e_a')

from sympy.physics.mechanics import ReferenceFrame, Point

# Definimos nuestra propia clase para que los versores sean IJK
class IJKReferenceFrame(ReferenceFrame):
    def __init__(self, name):
        super().__init__(name, latexs=['\mathbf{{{}_{{{}}}}}'.format(idx, name) for idx in ("i", "j", "k")])

A = IJKReferenceFrame("A")
O = Point("O")
O.set_vel(A, 0)

A1 = IJKReferenceFrame("A1")
psi = dynamicsymbols('psi')
A1.orient(A, 'Axis', [psi, A.z])
A1.dcm(A)  # T_{A1A}

A1.ang_vel_in(A)

A2 = IJKReferenceFrame("A2")
beta = dynamicsymbols('beta')
A2.orient(A1, 'Axis', [beta, -A1.y])
A2.dcm(A1)  # T_{A2A1}

E_b = O.locatenew('E_b', e_b * A1.x)
E_b.pos_from(O)

A2.ang_vel_in(A1)

A2.dcm(A)  # T_{A2A}

A2.ang_vel_in(A)

A3 = IJKReferenceFrame("A3")
zeta = dynamicsymbols('zeta')
A3.orient(A2, 'Axis', [zeta, A2.z])
A3.dcm(A2)  # T_{A3A2}

E_a = E_b.locatenew('E_a', (e_a - e_b) * A2.x)
E_a.pos_from(E_b)

A3.ang_vel_in(A2)

P = E_a.locatenew('P', (r - e_a) * A2.x)
P.pos_from(O)

# En SymPy, v2pt_theory hace referencia al campo de velocidades
# de un sólido rígido. Los argumentos son:
# * `O`, punto del que conocemos la velocidad respecto a A
# * `A`, sistema de referencia donde queremos expresar la velocidad
# * `A1`, sistema de referencia donde están fijos ambos puntos
E_b.v2pt_theory(O, A, A1)