n = Matrix([0,0,1]);n

"""
Explanation: Simple Rotations
Example 1: Rotation about 3rd body axis
$\mathbf{\hat{n}} = \mathbf{a}_3 = \mathbf{b}_3$
End of explanation
"""


aCb = cos(th)*eye(3) + (1 - cos(th))*(n*n.T) + sin(th)*skew(n); aCb

"""
Explanation: ${}^\mathcal{A}C^\mathcal{B} = \cos\theta I + (1 - \cos\theta) [\mathbf{\hat{n}}][\mathbf{\hat{n}}]^T + \sin\theta [\mathbf{\hat{n}}\times]$:
End of explanation
"""


aCb.T*n

"""
Explanation: As a sanity check, show that ${}^\mathcal{B}C^\mathcal{A} [\mathbf{\hat{n}}] = [\mathbf{\hat{n}}]$
End of explanation
"""


n2 = Matrix([1,2,3])
n2 = n2/n2.norm()
n2

aCb2 = cos(th)*(eye(3) - n2*n2.T)+ n2*n2.T + sin(th)*skew(n2); aCb2

simplify(aCb2.T*n2 - n2)

simplify(aCb2*n2 - n2)

"""
Explanation: Example 2: Rotating about $1 \mathbf{b}_1 + 2 \mathbf{b}_2 + 3 \mathbf{b}_3$
End of explanation
"""


n1,n2,n3 = symbols('n_1,n_2,n_3')

ng = Matrix([n1,n2,n3]);ng

aCbg = simplify(cos(th)*eye(3) + (1 - cos(th))*(ng*ng.T) + sin(th)*skew(ng)); aCbg

bCag = simplify(cos(th)*eye(3) + (1 - cos(th))*(ng*ng.T) - sin(th)*skew(ng)); bCag

"""
Explanation: Example 3: Rotating about aribtary axis $n_1 \mathbf{b}_1 + n_2 \mathbf{b}_2 + n_3 \mathbf{b}_3$
End of explanation
"""


