def _check_orthogonality(equations):
"""
Helper method for _connect_to_cartesian. It checks if
set of transformation equations create orthogonal curvilinear
coordinate system
Parameters
==========
equations : Lambda
Lambda of transformation equations
"""
x1, x2, x3 = symbols("x1, x2, x3", cls=Dummy)
equations = equations(x1, x2, x3)
v1 = Matrix([diff(equations[0], x1),
diff(equations[1], x1), diff(equations[2], x1)])
v2 = Matrix([diff(equations[0], x2),
diff(equations[1], x2), diff(equations[2], x2)])
v3 = Matrix([diff(equations[0], x3),
diff(equations[1], x3), diff(equations[2], x3)])
if any(simplify(i[0] + i[1] + i[2]) == 0 for i in (v1, v2, v3)):
return False
else:
if simplify(v1.dot(v2)) == 0 and simplify(v2.dot(v3)) == 0 \
and simplify(v3.dot(v1)) == 0:
return True
else:
return False
def test_matrix_to_vector():
m = Matrix([[1], [2], [3]])
assert matrix_to_vector(m, C) == C.i + 2 * C.j + 3 * C.k
m = Matrix([[0], [0], [0]])
assert matrix_to_vector(m, N) == matrix_to_vector(m, C) == \
Vector.zero
m = Matrix([[q1], [q2], [q3]])
assert matrix_to_vector(m, N) == q1 * N.i + q2 * N.j + q3 * N.k
def _rot(axis, angle):
"""DCM for simple axis 1, 2 or 3 rotations. """
if axis == 1:
return Matrix(rot_axis1(angle).T)
elif axis == 2:
return Matrix(rot_axis2(angle).T)
elif axis == 3:
return Matrix(rot_axis3(angle).T)
def test_dyadic():
d1 = A.x | A.x
d2 = A.y | A.y
d3 = A.x | A.y
assert d1 * 0 == 0
assert d1 != 0
assert d1 * 2 == 2 * A.x | A.x
assert d1 / 2. == 0.5 * d1
assert d1 & (0 * d1) == 0
assert d1 & d2 == 0
assert d1 & A.x == A.x
assert d1 ^ A.x == 0
assert d1 ^ A.y == A.x | A.z
assert d1 ^ A.z == - A.x | A.y
assert d2 ^ A.x == - A.y | A.z
assert A.x ^ d1 == 0
assert A.y ^ d1 == - A.z | A.x
assert A.z ^ d1 == A.y | A.x
assert A.x & d1 == A.x
assert A.y & d1 == 0
assert A.y & d2 == A.y
assert d1 & d3 == A.x | A.y
assert d3 & d1 == 0
assert d1.dt(A) == 0
q = dynamicsymbols('q')
qd = dynamicsymbols('q', 1)
B = A.orientnew('B', 'Axis', [q, A.z])
assert d1.express(B) == d1.express(B, B)
assert d1.express(B) == ((cos(q)**2) * (B.x | B.x) + (-sin(q) * cos(q)) *
(B.x | B.y) + (-sin(q) * cos(q)) * (B.y | B.x) + (sin(q)**2) *
(B.y | B.y))
assert d1.express(B, A) == (cos(q)) * (B.x | A.x) + (-sin(q)) * (B.y | A.x)
assert d1.express(A, B) == (cos(q)) * (A.x | B.x) + (-sin(q)) * (A.x | B.y)
assert d1.dt(B) == (-qd) * (A.y | A.x) + (-qd) * (A.x | A.y)
assert d1.to_matrix(A) == Matrix([[1, 0, 0], [0, 0, 0], [0, 0, 0]])
assert d1.to_matrix(A, B) == Matrix([[cos(q), -sin(q), 0],
[0, 0, 0],
[0, 0, 0]])
assert d3.to_matrix(A) == Matrix([[0, 1, 0], [0, 0, 0], [0, 0, 0]])
a, b, c, d, e, f = symbols('a, b, c, d, e, f')
v1 = a * A.x + b * A.y + c * A.z
v2 = d * A.x + e * A.y + f * A.z
d4 = v1.outer(v2)
assert d4.to_matrix(A) == Matrix([[a * d, a * e, a * f],
[b * d, b * e, b * f],
[c * d, c * e, c * f]])
d5 = v1.outer(v1)
C = A.orientnew('C', 'Axis', [q, A.x])
for expected, actual in zip(C.dcm(A) * d5.to_matrix(A) * C.dcm(A).T,
d5.to_matrix(C)):
assert (expected - actual).simplify() == 0
raises(TypeError, lambda: d1.applyfunc(0))
def test_dcm():
q1, q2, q3, q4 = dynamicsymbols('q1 q2 q3 q4')
N = ReferenceFrame('N')
A = N.orientnew('A', 'Axis', [q1, N.z])
B = A.orientnew('B', 'Axis', [q2, A.x])
C = B.orientnew('C', 'Axis', [q3, B.y])
D = N.orientnew('D', 'Axis', [q4, N.y])
E = N.orientnew('E', 'Space', [q1, q2, q3], '123')
assert N.dcm(C) == Matrix(
[[
-sin(q1) * sin(q2) * sin(q3) + cos(q1) * cos(q3),
-sin(q1) * cos(q2),
sin(q1) * sin(q2) * cos(q3) + sin(q3) * cos(q1)
],
[
sin(q1) * cos(q3) + sin(q2) * sin(q3) * cos(q1),
cos(q1) * cos(q2),
sin(q1) * sin(q3) - sin(q2) * cos(q1) * cos(q3)
], [-sin(q3) * cos(q2),
sin(q2), cos(q2) * cos(q3)]])
# This is a little touchy. Is it ok to use simplify in assert?
test_mat = D.dcm(C) - Matrix(
[[
cos(q1) * cos(q3) * cos(q4) - sin(q3) *
(-sin(q4) * cos(q2) + sin(q1) * sin(q2) * cos(q4)),
-sin(q2) * sin(q4) - sin(q1) * cos(q2) * cos(q4),
sin(q3) * cos(q1) * cos(q4) + cos(q3) *
(-sin(q4) * cos(q2) + sin(q1) * sin(q2) * cos(q4))
],
[
sin(q1) * cos(q3) + sin(q2) * sin(q3) * cos(q1),
cos(q1) * cos(q2),
sin(q1) * sin(q3) - sin(q2) * cos(q1) * cos(q3)
],
[
sin(q4) * cos(q1) * cos(q3) - sin(q3) *
(cos(q2) * cos(q4) + sin(q1) * sin(q2) * sin(q4)),
sin(q2) * cos(q4) - sin(q1) * sin(q4) * cos(q2),
sin(q3) * sin(q4) * cos(q1) + cos(q3) *
(cos(q2) * cos(q4) + sin(q1) * sin(q2) * sin(q4))
]])
assert test_mat.expand() == zeros(3, 3)
assert E.dcm(N) == Matrix(
[[cos(q2) * cos(q3), sin(q3) * cos(q2), -sin(q2)],
[
sin(q1) * sin(q2) * cos(q3) - sin(q3) * cos(q1),
sin(q1) * sin(q2) * sin(q3) + cos(q1) * cos(q3),
sin(q1) * cos(q2)
],
[
sin(q1) * sin(q3) + sin(q2) * cos(q1) * cos(q3),
-sin(q1) * cos(q3) + sin(q2) * sin(q3) * cos(q1),
cos(q1) * cos(q2)
]])
def test_Vector():
assert A.x != A.y
assert A.y != A.z
assert A.z != A.x
assert A.x + 0 == A.x
v1 = x*A.x + y*A.y + z*A.z
v2 = x**2*A.x + y**2*A.y + z**2*A.z
v3 = v1 + v2
v4 = v1 - v2
assert isinstance(v1, Vector)
assert dot(v1, A.x) == x
assert dot(v1, A.y) == y
assert dot(v1, A.z) == z
assert isinstance(v2, Vector)
assert dot(v2, A.x) == x**2
assert dot(v2, A.y) == y**2
assert dot(v2, A.z) == z**2
assert isinstance(v3, Vector)
# We probably shouldn't be using simplify in dot...
assert dot(v3, A.x) == x**2 + x
assert dot(v3, A.y) == y**2 + y
assert dot(v3, A.z) == z**2 + z
assert isinstance(v4, Vector)
# We probably shouldn't be using simplify in dot...
assert dot(v4, A.x) == x - x**2
assert dot(v4, A.y) == y - y**2
assert dot(v4, A.z) == z - z**2
assert v1.to_matrix(A) == Matrix([[x], [y], [z]])
q = symbols('q')
B = A.orientnew('B', 'Axis', (q, A.x))
assert v1.to_matrix(B) == Matrix([[x],
[ y * cos(q) + z * sin(q)],
[-y * sin(q) + z * cos(q)]])
#Test the separate method
B = ReferenceFrame('B')
v5 = x*A.x + y*A.y + z*B.z
assert Vector(0).separate() == {}
assert v1.separate() == {A: v1}
assert v5.separate() == {A: x*A.x + y*A.y, B: z*B.z}
#Test the free_symbols property
v6 = x*A.x + y*A.y + z*A.z
assert v6.free_symbols(A) == {x,y,z}
raises(TypeError, lambda: v3.applyfunc(v1))
def test_w_diff_dcm1():
# Ref:
# Dynamics Theory and Applications, Kane 1985
# Sec. 2.1 ANGULAR VELOCITY
A = ReferenceFrame('A')
B = ReferenceFrame('B')
c11, c12, c13 = dynamicsymbols('C11 C12 C13')
c21, c22, c23 = dynamicsymbols('C21 C22 C23')
c31, c32, c33 = dynamicsymbols('C31 C32 C33')
c11d, c12d, c13d = dynamicsymbols('C11 C12 C13', level=1)
c21d, c22d, c23d = dynamicsymbols('C21 C22 C23', level=1)
c31d, c32d, c33d = dynamicsymbols('C31 C32 C33', level=1)
DCM = Matrix([[c11, c12, c13], [c21, c22, c23], [c31, c32, c33]])
B.orient(A, 'DCM', DCM)
b1a = (B.x).express(A)
b2a = (B.y).express(A)
b3a = (B.z).express(A)
# Equation (2.1.1)
B.set_ang_vel(
A,
B.x * (dot((b3a).dt(A), B.y)) + B.y * (dot(
(b1a).dt(A), B.z)) + B.z * (dot((b2a).dt(A), B.x)))
# Equation (2.1.21)
expr = ((c12 * c13d + c22 * c23d + c32 * c33d) * B.x +
(c13 * c11d + c23 * c21d + c33 * c31d) * B.y +
(c11 * c12d + c21 * c22d + c31 * c32d) * B.z)
assert B.ang_vel_in(A) - expr == 0
def test_w_diff_dcm2():
q1, q2, q3 = dynamicsymbols('q1:4')
N = ReferenceFrame('N')
A = N.orientnew('A', 'axis', [q1, N.x])
B = A.orientnew('B', 'axis', [q2, A.y])
C = B.orientnew('C', 'axis', [q3, B.z])
DCM = C.dcm(N).T
D = N.orientnew('D', 'DCM', DCM)
# Frames D and C are the same ReferenceFrame,
# since they have equal DCM respect to frame N.
# Therefore, D and C should have same angle velocity in N.
assert D.dcm(N) == C.dcm(N) == Matrix(
[[
cos(q2) * cos(q3),
sin(q1) * sin(q2) * cos(q3) + sin(q3) * cos(q1),
sin(q1) * sin(q3) - sin(q2) * cos(q1) * cos(q3)
],
[
-sin(q3) * cos(q2),
-sin(q1) * sin(q2) * sin(q3) + cos(q1) * cos(q3),
sin(q1) * cos(q3) + sin(q2) * sin(q3) * cos(q1)
], [sin(q2), -sin(q1) * cos(q2),
cos(q1) * cos(q2)]])
assert (D.ang_vel_in(N) - C.ang_vel_in(N)).express(N).simplify() == 0
def test_issue_11498():
A = ReferenceFrame('A')
B = ReferenceFrame('B')
# Identity transformation
A.orient(B, 'DCM', eye(3))
assert A.dcm(B) == Matrix([[1, 0, 0], [0, 1, 0], [0, 0, 1]])
assert B.dcm(A) == Matrix([[1, 0, 0], [0, 1, 0], [0, 0, 1]])
# x -> y
# y -> -z
# z -> -x
A.orient(B, 'DCM', Matrix([[0, 1, 0], [0, 0, -1], [-1, 0, 0]]))
assert B.dcm(A) == Matrix([[0, 1, 0], [0, 0, -1], [-1, 0, 0]])
assert A.dcm(B) == Matrix([[0, 0, -1], [1, 0, 0], [0, -1, 0]])
assert B.dcm(A).T == A.dcm(B)
def test_dcm_diff_16824():
# NOTE : This is a regression test for the bug introduced in PR 14758,
# identified in 16824, and solved by PR 16828.
# This is the solution to Problem 2.2 on page 264 in Kane & Lenvinson's
# 1985 book.
q1, q2, q3 = dynamicsymbols('q1:4')
s1 = sin(q1)
c1 = cos(q1)
s2 = sin(q2)
c2 = cos(q2)
s3 = sin(q3)
c3 = cos(q3)
dcm = Matrix([[c2 * c3, s1 * s2 * c3 - s3 * c1, c1 * s2 * c3 + s3 * s1],
[c2 * s3, s1 * s2 * s3 + c3 * c1, c1 * s2 * s3 - c3 * s1],
[-s2, s1 * c2, c1 * c2]])
A = ReferenceFrame('A')
B = ReferenceFrame('B')
B.orient(A, 'DCM', dcm)
AwB = B.ang_vel_in(A)
alpha2 = s3 * c2 * q1.diff() + c3 * q2.diff()
beta2 = s1 * c2 * q3.diff() + c1 * q2.diff()
assert simplify(AwB.dot(A.y) - alpha2) == 0
assert simplify(AwB.dot(B.y) - beta2) == 0
def __new__(cls, q0, q1, q2, q3):
q0 = sympify(q0)
q1 = sympify(q1)
q2 = sympify(q2)
q3 = sympify(q3)
parent_orient = (Matrix([[
q0**2 + q1**2 - q2**2 - q3**2, 2 * (q1 * q2 - q0 * q3),
2 * (q0 * q2 + q1 * q3)
],
[
2 * (q1 * q2 + q0 * q3),
q0**2 - q1**2 + q2**2 - q3**2,
2 * (q2 * q3 - q0 * q1)
],
[
2 * (q1 * q3 - q0 * q2),
2 * (q0 * q1 + q2 * q3),
q0**2 - q1**2 - q2**2 + q3**2
]]))
parent_orient = parent_orient.T
obj = super().__new__(cls, q0, q1, q2, q3)
obj._q0 = q0
obj._q1 = q1
obj._q2 = q2
obj._q3 = q3
obj._parent_orient = parent_orient
return obj
def to_matrix(self, system):
"""
Returns the matrix form of this vector with respect to the
specified coordinate system.
Parameters
==========
system : CoordSys3D
The system wrt which the matrix form is to be computed
Examples
========
>>> from sympy.vector import CoordSys3D
>>> C = CoordSys3D('C')
>>> from sympy.abc import a, b, c
>>> v = a*C.i + b*C.j + c*C.k
>>> v.to_matrix(C)
Matrix([
[a],
[b],
[c]])
"""
return Matrix([self.dot(unit_vec) for unit_vec in
system.base_vectors()])
def test_orienters():
A = CoordSys3D('A')
axis_orienter = AxisOrienter(a, A.k)
body_orienter = BodyOrienter(a, b, c, '123')
space_orienter = SpaceOrienter(a, b, c, '123')
q_orienter = QuaternionOrienter(q1, q2, q3, q4)
assert axis_orienter.rotation_matrix(A) == Matrix([[cos(a),
sin(a), 0],
[-sin(a),
cos(a), 0], [0, 0, 1]])
assert body_orienter.rotation_matrix() == Matrix(
[[
cos(b) * cos(c),
sin(a) * sin(b) * cos(c) + sin(c) * cos(a),
sin(a) * sin(c) - sin(b) * cos(a) * cos(c)
],
[
-sin(c) * cos(b), -sin(a) * sin(b) * sin(c) + cos(a) * cos(c),
sin(a) * cos(c) + sin(b) * sin(c) * cos(a)
], [sin(b), -sin(a) * cos(b),
cos(a) * cos(b)]])
assert space_orienter.rotation_matrix() == Matrix(
[[cos(b) * cos(c), sin(c) * cos(b), -sin(b)],
[
sin(a) * sin(b) * cos(c) - sin(c) * cos(a),
sin(a) * sin(b) * sin(c) + cos(a) * cos(c),
sin(a) * cos(b)
],
[
sin(a) * sin(c) + sin(b) * cos(a) * cos(c),
-sin(a) * cos(c) + sin(b) * sin(c) * cos(a),
cos(a) * cos(b)
]])
assert q_orienter.rotation_matrix() == Matrix(
[[
q1**2 + q2**2 - q3**2 - q4**2, 2 * q1 * q4 + 2 * q2 * q3,
-2 * q1 * q3 + 2 * q2 * q4
],
[
-2 * q1 * q4 + 2 * q2 * q3, q1**2 - q2**2 + q3**2 - q4**2,
2 * q1 * q2 + 2 * q3 * q4
],
[
2 * q1 * q3 + 2 * q2 * q4, -2 * q1 * q2 + 2 * q3 * q4,
q1**2 - q2**2 - q3**2 + q4**2
]])
def test_orient_body():
A = ReferenceFrame('A')
B = ReferenceFrame('B')
B.orient_body_fixed(A, (1, 1, 0), 'XYX')
assert B.dcm(A) == Matrix([[cos(1), sin(1)**2, -sin(1) * cos(1)],
[0, cos(1), sin(1)],
[sin(1), -sin(1) * cos(1),
cos(1)**2]])
def test_vector():
assert isinstance(i, BaseVector)
assert i != j
assert j != k
assert k != i
assert i - i == Vector.zero
assert i + Vector.zero == i
assert i - Vector.zero == i
assert Vector.zero != 0
assert -Vector.zero == Vector.zero
v1 = a * i + b * j + c * k
v2 = a**2 * i + b**2 * j + c**2 * k
v3 = v1 + v2
v4 = 2 * v1
v5 = a * i
assert isinstance(v1, VectorAdd)
assert v1 - v1 == Vector.zero
assert v1 + Vector.zero == v1
assert v1.dot(i) == a
assert v1.dot(j) == b
assert v1.dot(k) == c
assert i.dot(v2) == a**2
assert j.dot(v2) == b**2
assert k.dot(v2) == c**2
assert v3.dot(i) == a**2 + a
assert v3.dot(j) == b**2 + b
assert v3.dot(k) == c**2 + c
assert v1 + v2 == v2 + v1
assert v1 - v2 == -1 * (v2 - v1)
assert a * v1 == v1 * a
assert isinstance(v5, VectorMul)
assert v5.base_vector == i
assert v5.measure_number == a
assert isinstance(v4, Vector)
assert isinstance(v4, VectorAdd)
assert isinstance(v4, Vector)
assert isinstance(Vector.zero, VectorZero)
assert isinstance(Vector.zero, Vector)
assert isinstance(v1 * 0, VectorZero)
assert v1.to_matrix(C) == Matrix([[a], [b], [c]])
assert i.components == {i: 1}
assert v5.components == {i: a}
assert v1.components == {i: a, j: b, k: c}
assert VectorAdd(v1, Vector.zero) == v1
assert VectorMul(a, v1) == v1 * a
assert VectorMul(1, i) == i
assert VectorAdd(v1, Vector.zero) == v1
assert VectorMul(0, Vector.zero) == Vector.zero
raises(TypeError, lambda: v1.outer(1))
raises(TypeError, lambda: v1.dot(1))
def test_frame_dict():
A = ReferenceFrame('A')
B = ReferenceFrame('B')
C = ReferenceFrame('C')
a, b, c = symbols('a b c')
B.orient_axis(A, A.x, a)
assert A._dcm_dict == {
B: Matrix([[1, 0, 0], [0, cos(a), -sin(a)], [0, sin(a),
cos(a)]])
}
assert B._dcm_dict == {
A: Matrix([[1, 0, 0], [0, cos(a), sin(a)], [0, -sin(a),
cos(a)]])
}
assert C._dcm_dict == {}
B.orient_axis(C, C.x, b)
# Previous relation is not wiped
assert A._dcm_dict == {
B: Matrix([[1, 0, 0], [0, cos(a), -sin(a)], [0, sin(a),
cos(a)]])
}
assert B._dcm_dict == {A: Matrix([[1, 0, 0],[0, cos(a), sin(a)],[0, -sin(a), cos(a)]]), \
C: Matrix([[1, 0, 0],[0, cos(b), sin(b)],[0, -sin(b), cos(b)]])}
assert C._dcm_dict == {
B: Matrix([[1, 0, 0], [0, cos(b), -sin(b)], [0, sin(b),
cos(b)]])
}
A.orient_axis(B, B.x, c)
# Previous relation is updated
assert B._dcm_dict == {C: Matrix([[1, 0, 0],[0, cos(b), sin(b)],[0, -sin(b), cos(b)]]),\
A: Matrix([[1, 0, 0],[0, cos(c), -sin(c)],[0, sin(c), cos(c)]])}
assert A._dcm_dict == {
B: Matrix([[1, 0, 0], [0, cos(c), sin(c)], [0, -sin(c),
cos(c)]])
}
assert C._dcm_dict == {
B: Matrix([[1, 0, 0], [0, cos(b), -sin(b)], [0, sin(b),
cos(b)]])
}
def test_dcm_cache_dict():
A = ReferenceFrame('A')
B = ReferenceFrame('B')
C = ReferenceFrame('C')
D = ReferenceFrame('D')
a, b, c = symbols('a b c')
B.orient_axis(A, A.x, a)
C.orient_axis(B, B.x, b)
D.orient_axis(C, C.x, c)
assert D._dcm_dict == {
C: Matrix([[1, 0, 0], [0, cos(c), sin(c)], [0, -sin(c),
cos(c)]])
}
assert C._dcm_dict == {B: Matrix([[1, 0, 0],[0, cos(b), sin(b)],[0, -sin(b), cos(b)]]), \
D: Matrix([[1, 0, 0],[0, cos(c), -sin(c)],[0, sin(c), cos(c)]])}
assert B._dcm_dict == {A: Matrix([[1, 0, 0],[0, cos(a), sin(a)],[0, -sin(a), cos(a)]]), \
C: Matrix([[1, 0, 0],[0, cos(b), -sin(b)],[0, sin(b), cos(b)]])}
assert A._dcm_dict == {
B: Matrix([[1, 0, 0], [0, cos(a), -sin(a)], [0, sin(a),
cos(a)]])
}
assert D._dcm_dict == D._dcm_cache
D.dcm(
A
) # Check calculated dcm relation is stored in _dcm_cache and not in _dcm_dict
assert list(A._dcm_cache.keys()) == [A, B, D]
assert list(D._dcm_cache.keys()) == [C, A]
assert list(A._dcm_dict.keys()) == [B]
assert list(D._dcm_dict.keys()) == [C]
assert A._dcm_dict != A._dcm_cache
A.orient_axis(
B, B.x, b) # _dcm_cache of A is wiped out and new relation is stored.
assert A._dcm_dict == {
B: Matrix([[1, 0, 0], [0, cos(b), sin(b)], [0, -sin(b),
cos(b)]])
}
assert A._dcm_dict == A._dcm_cache
assert B._dcm_dict == {C: Matrix([[1, 0, 0],[0, cos(b), -sin(b)],[0, sin(b), cos(b)]]), \
A: Matrix([[1, 0, 0],[0, cos(b), -sin(b)],[0, sin(b), cos(b)]])}
def _rotation_trans_equations(cls, matrix, equations):
"""
Returns the transformation equations obtained from rotation matrix.
Parameters
==========
matrix : Matrix
Rotation matrix
equations : tuple
Transformation equations
"""
return tuple(matrix * Matrix(equations))
def to_matrix(self, system, second_system=None):
"""
Returns the matrix form of the dyadic with respect to one or two
coordinate systems.
Parameters
==========
system : CoordSys3D
The coordinate system that the rows and columns of the matrix
correspond to. If a second system is provided, this
only corresponds to the rows of the matrix.
second_system : CoordSys3D, optional, default=None
The coordinate system that the columns of the matrix correspond
to.
Examples
========
>>> from sympy.vector import CoordSys3D
>>> N = CoordSys3D('N')
>>> v = N.i + 2*N.j
>>> d = v.outer(N.i)
>>> d.to_matrix(N)
Matrix([
[1, 0, 0],
[2, 0, 0],
[0, 0, 0]])
>>> from sympy import Symbol
>>> q = Symbol('q')
>>> P = N.orient_new_axis('P', q, N.k)
>>> d.to_matrix(N, P)
Matrix([
[ cos(q), -sin(q), 0],
[2*cos(q), -2*sin(q), 0],
[ 0, 0, 0]])
"""
if second_system is None:
second_system = system
return Matrix([
i.dot(self).dot(j) for i in system for j in second_system
]).reshape(3, 3)
def rotation_matrix(self, system):
"""
The rotation matrix corresponding to this orienter
instance.
Parameters
==========
system : CoordSys3D
The coordinate system wrt which the rotation matrix
is to be computed
"""
axis = sympy.vector.express(self.axis, system).normalize()
axis = axis.to_matrix(system)
theta = self.angle
parent_orient = (
(eye(3) - axis * axis.T) * cos(theta) +
Matrix([[0, -axis[2], axis[1]], [axis[2], 0, -axis[0]],
[-axis[1], axis[0], 0]]) * sin(theta) + axis * axis.T)
parent_orient = parent_orient.T
return parent_orient
def scalar_map(self, other):
"""
Returns a dictionary which expresses the coordinate variables
(base scalars) of this frame in terms of the variables of
otherframe.
Parameters
==========
otherframe : CoordSys3D
The other system to map the variables to.
Examples
========
>>> from sympy.vector import CoordSys3D
>>> from sympy import Symbol
>>> A = CoordSys3D('A')
>>> q = Symbol('q')
>>> B = A.orient_new_axis('B', q, A.k)
>>> A.scalar_map(B)
{A.x: B.x*cos(q) - B.y*sin(q), A.y: B.x*sin(q) + B.y*cos(q), A.z: B.z}
"""
relocated_scalars = []
origin_coords = tuple(self.position_wrt(other).to_matrix(other))
for i, x in enumerate(other.base_scalars()):
relocated_scalars.append(x - origin_coords[i])
vars_matrix = (self.rotation_matrix(other) *
Matrix(relocated_scalars))
mapping = {}
for i, x in enumerate(self.base_scalars()):
mapping[x] = trigsimp(vars_matrix[i])
return mapping
def __new__(cls, name, transformation=None, parent=None, location=None,
rotation_matrix=None, vector_names=None, variable_names=None):
"""
The orientation/location parameters are necessary if this system
is being defined at a certain orientation or location wrt another.
Parameters
==========
name : str
The name of the new CoordSys3D instance.
transformation : Lambda, Tuple, str
Transformation defined by transformation equations or chosen
from predefined ones.
location : Vector
The position vector of the new system's origin wrt the parent
instance.
rotation_matrix : SymPy ImmutableMatrix
The rotation matrix of the new coordinate system with respect
to the parent. In other words, the output of
new_system.rotation_matrix(parent).
parent : CoordSys3D
The coordinate system wrt which the orientation/location
(or both) is being defined.
vector_names, variable_names : iterable(optional)
Iterables of 3 strings each, with custom names for base
vectors and base scalars of the new system respectively.
Used for simple str printing.
"""
name = str(name)
Vector = sympy.vector.Vector
Point = sympy.vector.Point
if not isinstance(name, str):
raise TypeError("name should be a string")
if transformation is not None:
if (location is not None) or (rotation_matrix is not None):
raise ValueError("specify either `transformation` or "
"`location`/`rotation_matrix`")
if isinstance(transformation, (Tuple, tuple, list)):
if isinstance(transformation[0], MatrixBase):
rotation_matrix = transformation[0]
location = transformation[1]
else:
transformation = Lambda(transformation[0],
transformation[1])
elif isinstance(transformation, Callable):
x1, x2, x3 = symbols('x1 x2 x3', cls=Dummy)
transformation = Lambda((x1, x2, x3),
transformation(x1, x2, x3))
elif isinstance(transformation, str):
transformation = Str(transformation)
elif isinstance(transformation, (Str, Lambda)):
pass
else:
raise TypeError("transformation: "
"wrong type {}".format(type(transformation)))
# If orientation information has been provided, store
# the rotation matrix accordingly
if rotation_matrix is None:
rotation_matrix = ImmutableDenseMatrix(eye(3))
else:
if not isinstance(rotation_matrix, MatrixBase):
raise TypeError("rotation_matrix should be an Immutable" +
"Matrix instance")
rotation_matrix = rotation_matrix.as_immutable()
# If location information is not given, adjust the default
# location as Vector.zero
if parent is not None:
if not isinstance(parent, CoordSys3D):
raise TypeError("parent should be a " +
"CoordSys3D/None")
if location is None:
location = Vector.zero
else:
if not isinstance(location, Vector):
raise TypeError("location should be a Vector")
# Check that location does not contain base
# scalars
for x in location.free_symbols:
if isinstance(x, BaseScalar):
raise ValueError("location should not contain" +
" BaseScalars")
origin = parent.origin.locate_new(name + '.origin',
location)
else:
location = Vector.zero
origin = Point(name + '.origin')
if transformation is None:
transformation = Tuple(rotation_matrix, location)
if isinstance(transformation, Tuple):
lambda_transformation = CoordSys3D._compose_rotation_and_translation(
transformation[0],
transformation[1],
parent
)
r, l = transformation
l = l._projections
lambda_lame = CoordSys3D._get_lame_coeff('cartesian')
lambda_inverse = lambda x, y, z: r.inv()*Matrix(
[x-l[0], y-l[1], z-l[2]])
elif isinstance(transformation, Str):
trname = transformation.name
lambda_transformation = CoordSys3D._get_transformation_lambdas(trname)
if parent is not None:
if parent.lame_coefficients() != (S.One, S.One, S.One):
raise ValueError('Parent for pre-defined coordinate '
'system should be Cartesian.')
lambda_lame = CoordSys3D._get_lame_coeff(trname)
lambda_inverse = CoordSys3D._set_inv_trans_equations(trname)
elif isinstance(transformation, Lambda):
if not CoordSys3D._check_orthogonality(transformation):
raise ValueError("The transformation equation does not "
"create orthogonal coordinate system")
lambda_transformation = transformation
lambda_lame = CoordSys3D._calculate_lame_coeff(lambda_transformation)
lambda_inverse = None
else:
lambda_transformation = lambda x, y, z: transformation(x, y, z)
lambda_lame = CoordSys3D._get_lame_coeff(transformation)
lambda_inverse = None
if variable_names is None:
if isinstance(transformation, Lambda):
variable_names = ["x1", "x2", "x3"]
elif isinstance(transformation, Str):
if transformation.name == 'spherical':
variable_names = ["r", "theta", "phi"]
elif transformation.name == 'cylindrical':
variable_names = ["r", "theta", "z"]
else:
variable_names = ["x", "y", "z"]
else:
variable_names = ["x", "y", "z"]
if vector_names is None:
vector_names = ["i", "j", "k"]
# All systems that are defined as 'roots' are unequal, unless
# they have the same name.
# Systems defined at same orientation/position wrt the same
# 'parent' are equal, irrespective of the name.
# This is true even if the same orientation is provided via
# different methods like Axis/Body/Space/Quaternion.
# However, coincident systems may be seen as unequal if
# positioned/oriented wrt different parents, even though
# they may actually be 'coincident' wrt the root system.
if parent is not None:
obj = super().__new__(
cls, Str(name), transformation, parent)
else:
obj = super().__new__(
cls, Str(name), transformation)
obj._name = name
# Initialize the base vectors
_check_strings('vector_names', vector_names)
vector_names = list(vector_names)
latex_vects = [(r'\mathbf{\hat{%s}_{%s}}' % (x, name)) for
x in vector_names]
pretty_vects = ['%s_%s' % (x, name) for x in vector_names]
obj._vector_names = vector_names
v1 = BaseVector(0, obj, pretty_vects[0], latex_vects[0])
v2 = BaseVector(1, obj, pretty_vects[1], latex_vects[1])
v3 = BaseVector(2, obj, pretty_vects[2], latex_vects[2])
obj._base_vectors = (v1, v2, v3)
# Initialize the base scalars
_check_strings('variable_names', vector_names)
variable_names = list(variable_names)
latex_scalars = [(r"\mathbf{{%s}_{%s}}" % (x, name)) for
x in variable_names]
pretty_scalars = ['%s_%s' % (x, name) for x in variable_names]
obj._variable_names = variable_names
obj._vector_names = vector_names
x1 = BaseScalar(0, obj, pretty_scalars[0], latex_scalars[0])
x2 = BaseScalar(1, obj, pretty_scalars[1], latex_scalars[1])
x3 = BaseScalar(2, obj, pretty_scalars[2], latex_scalars[2])
obj._base_scalars = (x1, x2, x3)
obj._transformation = transformation
obj._transformation_lambda = lambda_transformation
obj._lame_coefficients = lambda_lame(x1, x2, x3)
obj._transformation_from_parent_lambda = lambda_inverse
setattr(obj, variable_names[0], x1)
setattr(obj, variable_names[1], x2)
setattr(obj, variable_names[2], x3)
setattr(obj, vector_names[0], v1)
setattr(obj, vector_names[1], v2)
setattr(obj, vector_names[2], v3)
# Assign params
obj._parent = parent
if obj._parent is not None:
obj._root = obj._parent._root
else:
obj._root = obj
obj._parent_rotation_matrix = rotation_matrix
obj._origin = origin
# Return the instance
return obj
def test_orient_quaternion():
A = ReferenceFrame('A')
B = ReferenceFrame('B')
B.orient_quaternion(A, (0, 0, 0, 0))
assert B.dcm(A) == Matrix([[0, 0, 0], [0, 0, 0], [0, 0, 0]])
def test_orient_space():
A = ReferenceFrame('A')
B = ReferenceFrame('B')
B.orient_space_fixed(A, (0, 0, 0), '123')
assert B.dcm(A) == Matrix([[1, 0, 0], [0, 1, 0], [0, 0, 1]])
def orient_new(self, name, orienters, location=None,
vector_names=None, variable_names=None):
"""
Creates a new CoordSys3D oriented in the user-specified way
with respect to this system.
Please refer to the documentation of the orienter classes
for more information about the orientation procedure.
Parameters
==========
name : str
The name of the new CoordSys3D instance.
orienters : iterable/Orienter
An Orienter or an iterable of Orienters for orienting the
new coordinate system.
If an Orienter is provided, it is applied to get the new
system.
If an iterable is provided, the orienters will be applied
in the order in which they appear in the iterable.
location : Vector(optional)
The location of the new coordinate system's origin wrt this
system's origin. If not specified, the origins are taken to
be coincident.
vector_names, variable_names : iterable(optional)
Iterables of 3 strings each, with custom names for base
vectors and base scalars of the new system respectively.
Used for simple str printing.
Examples
========
>>> from sympy.vector import CoordSys3D
>>> from sympy import symbols
>>> q0, q1, q2, q3 = symbols('q0 q1 q2 q3')
>>> N = CoordSys3D('N')
Using an AxisOrienter
>>> from sympy.vector import AxisOrienter
>>> axis_orienter = AxisOrienter(q1, N.i + 2 * N.j)
>>> A = N.orient_new('A', (axis_orienter, ))
Using a BodyOrienter
>>> from sympy.vector import BodyOrienter
>>> body_orienter = BodyOrienter(q1, q2, q3, '123')
>>> B = N.orient_new('B', (body_orienter, ))
Using a SpaceOrienter
>>> from sympy.vector import SpaceOrienter
>>> space_orienter = SpaceOrienter(q1, q2, q3, '312')
>>> C = N.orient_new('C', (space_orienter, ))
Using a QuaternionOrienter
>>> from sympy.vector import QuaternionOrienter
>>> q_orienter = QuaternionOrienter(q0, q1, q2, q3)
>>> D = N.orient_new('D', (q_orienter, ))
"""
if variable_names is None:
variable_names = self._variable_names
if vector_names is None:
vector_names = self._vector_names
if isinstance(orienters, Orienter):
if isinstance(orienters, AxisOrienter):
final_matrix = orienters.rotation_matrix(self)
else:
final_matrix = orienters.rotation_matrix()
# TODO: trigsimp is needed here so that the matrix becomes
# canonical (scalar_map also calls trigsimp; without this, you can
# end up with the same CoordinateSystem that compares differently
# due to a differently formatted matrix). However, this is
# probably not so good for performance.
final_matrix = trigsimp(final_matrix)
else:
final_matrix = Matrix(eye(3))
for orienter in orienters:
if isinstance(orienter, AxisOrienter):
final_matrix *= orienter.rotation_matrix(self)
else:
final_matrix *= orienter.rotation_matrix()
return CoordSys3D(name, rotation_matrix=final_matrix,
vector_names=vector_names,
variable_names=variable_names,
location=location,
parent=self)
def test_orient_explicit():
A = ReferenceFrame('A')
B = ReferenceFrame('B')
A.orient_explicit(B, eye(3))
assert A.dcm(B) == Matrix([[1, 0, 0], [0, 1, 0], [0, 0, 1]])
def test_rotation_matrix():
N = CoordSys3D('N')
A = N.orient_new_axis('A', q1, N.k)
B = A.orient_new_axis('B', q2, A.i)
C = B.orient_new_axis('C', q3, B.j)
D = N.orient_new_axis('D', q4, N.j)
E = N.orient_new_space('E', q1, q2, q3, '123')
F = N.orient_new_quaternion('F', q1, q2, q3, q4)
G = N.orient_new_body('G', q1, q2, q3, '123')
assert N.rotation_matrix(C) == Matrix([
[- sin(q1) * sin(q2) * sin(q3) + cos(q1) * cos(q3), - sin(q1) *
cos(q2), sin(q1) * sin(q2) * cos(q3) + sin(q3) * cos(q1)], \
[sin(q1) * cos(q3) + sin(q2) * sin(q3) * cos(q1), \
cos(q1) * cos(q2), sin(q1) * sin(q3) - sin(q2) * cos(q1) * \
cos(q3)], [- sin(q3) * cos(q2), sin(q2), cos(q2) * cos(q3)]])
test_mat = D.rotation_matrix(C) - Matrix(
[[cos(q1) * cos(q3) * cos(q4) - sin(q3) * (- sin(q4) * cos(q2) +
sin(q1) * sin(q2) * cos(q4)), - sin(q2) * sin(q4) - sin(q1) *
cos(q2) * cos(q4), sin(q3) * cos(q1) * cos(q4) + cos(q3) * \
(- sin(q4) * cos(q2) + sin(q1) * sin(q2) * cos(q4))], \
[sin(q1) * cos(q3) + sin(q2) * sin(q3) * cos(q1), cos(q1) * \
cos(q2), sin(q1) * sin(q3) - sin(q2) * cos(q1) * cos(q3)], \
[sin(q4) * cos(q1) * cos(q3) - sin(q3) * (cos(q2) * cos(q4) + \
sin(q1) * sin(q2) * \
sin(q4)), sin(q2) *
cos(q4) - sin(q1) * sin(q4) * cos(q2), sin(q3) * \
sin(q4) * cos(q1) + cos(q3) * (cos(q2) * cos(q4) + \
sin(q1) * sin(q2) * sin(q4))]])
assert test_mat.expand() == zeros(3, 3)
assert E.rotation_matrix(N) == Matrix(
[[cos(q2)*cos(q3), sin(q3)*cos(q2), -sin(q2)],
[sin(q1)*sin(q2)*cos(q3) - sin(q3)*cos(q1), \
sin(q1)*sin(q2)*sin(q3) + cos(q1)*cos(q3), sin(q1)*cos(q2)], \
[sin(q1)*sin(q3) + sin(q2)*cos(q1)*cos(q3), - \
sin(q1)*cos(q3) + sin(q2)*sin(q3)*cos(q1), cos(q1)*cos(q2)]])
assert F.rotation_matrix(N) == Matrix([[
q1**2 + q2**2 - q3**2 - q4**2, 2 * q1 * q4 + 2 * q2 * q3,
-2 * q1 * q3 + 2 * q2 * q4
],
[
-2 * q1 * q4 + 2 * q2 * q3,
q1**2 - q2**2 + q3**2 - q4**2,
2 * q1 * q2 + 2 * q3 * q4
],
[
2 * q1 * q3 + 2 * q2 * q4,
-2 * q1 * q2 + 2 * q3 * q4,
q1**2 - q2**2 - q3**2 + q4**2
]])
assert G.rotation_matrix(N) == Matrix(
[[
cos(q2) * cos(q3),
sin(q1) * sin(q2) * cos(q3) + sin(q3) * cos(q1),
sin(q1) * sin(q3) - sin(q2) * cos(q1) * cos(q3)
],
[
-sin(q3) * cos(q2),
-sin(q1) * sin(q2) * sin(q3) + cos(q1) * cos(q3),
sin(q1) * cos(q3) + sin(q2) * sin(q3) * cos(q1)
], [sin(q2), -sin(q1) * cos(q2),
cos(q1) * cos(q2)]])
def test_vector_with_orientation():
"""
Tests the effects of orientation of coordinate systems on
basic vector operations.
"""
N = CoordSys3D('N')
A = N.orient_new_axis('A', q1, N.k)
B = A.orient_new_axis('B', q2, A.i)
C = B.orient_new_axis('C', q3, B.j)
# Test to_matrix
v1 = a * N.i + b * N.j + c * N.k
assert v1.to_matrix(A) == Matrix([[a * cos(q1) + b * sin(q1)],
[-a * sin(q1) + b * cos(q1)], [c]])
# Test dot
assert N.i.dot(A.i) == cos(q1)
assert N.i.dot(A.j) == -sin(q1)
assert N.i.dot(A.k) == 0
assert N.j.dot(A.i) == sin(q1)
assert N.j.dot(A.j) == cos(q1)
assert N.j.dot(A.k) == 0
assert N.k.dot(A.i) == 0
assert N.k.dot(A.j) == 0
assert N.k.dot(A.k) == 1
assert N.i.dot(A.i + A.j) == -sin(q1) + cos(q1) == \
(A.i + A.j).dot(N.i)
assert A.i.dot(C.i) == cos(q3)
assert A.i.dot(C.j) == 0
assert A.i.dot(C.k) == sin(q3)
assert A.j.dot(C.i) == sin(q2) * sin(q3)
assert A.j.dot(C.j) == cos(q2)
assert A.j.dot(C.k) == -sin(q2) * cos(q3)
assert A.k.dot(C.i) == -cos(q2) * sin(q3)
assert A.k.dot(C.j) == sin(q2)
assert A.k.dot(C.k) == cos(q2) * cos(q3)
# Test cross
assert N.i.cross(A.i) == sin(q1) * A.k
assert N.i.cross(A.j) == cos(q1) * A.k
assert N.i.cross(A.k) == -sin(q1) * A.i - cos(q1) * A.j
assert N.j.cross(A.i) == -cos(q1) * A.k
assert N.j.cross(A.j) == sin(q1) * A.k
assert N.j.cross(A.k) == cos(q1) * A.i - sin(q1) * A.j
assert N.k.cross(A.i) == A.j
assert N.k.cross(A.j) == -A.i
assert N.k.cross(A.k) == Vector.zero
assert N.i.cross(A.i) == sin(q1) * A.k
assert N.i.cross(A.j) == cos(q1) * A.k
assert N.i.cross(A.i + A.j) == sin(q1) * A.k + cos(q1) * A.k
assert (A.i + A.j).cross(N.i) == (-sin(q1) - cos(q1)) * N.k
assert A.i.cross(C.i) == sin(q3) * C.j
assert A.i.cross(C.j) == -sin(q3) * C.i + cos(q3) * C.k
assert A.i.cross(C.k) == -cos(q3) * C.j
assert C.i.cross(A.i) == (-sin(q3)*cos(q2))*A.j + \
(-sin(q2)*sin(q3))*A.k
assert C.j.cross(A.i) == (sin(q2)) * A.j + (-cos(q2)) * A.k
assert express(C.k.cross(A.i), C).trigsimp() == cos(q3) * C.j
def test_dyadic():
a, b = symbols('a, b')
assert Dyadic.zero != 0
assert isinstance(Dyadic.zero, DyadicZero)
assert BaseDyadic(A.i, A.j) != BaseDyadic(A.j, A.i)
assert (BaseDyadic(Vector.zero, A.i) == BaseDyadic(A.i, Vector.zero) ==
Dyadic.zero)
d1 = A.i | A.i
d2 = A.j | A.j
d3 = A.i | A.j
assert isinstance(d1, BaseDyadic)
d_mul = a * d1
assert isinstance(d_mul, DyadicMul)
assert d_mul.base_dyadic == d1
assert d_mul.measure_number == a
assert isinstance(a * d1 + b * d3, DyadicAdd)
assert d1 == A.i.outer(A.i)
assert d3 == A.i.outer(A.j)
v1 = a * A.i - A.k
v2 = A.i + b * A.j
assert v1 | v2 == v1.outer(v2) == a * (A.i|A.i) + (a*b) * (A.i|A.j) +\
- (A.k|A.i) - b * (A.k|A.j)
assert d1 * 0 == Dyadic.zero
assert d1 != Dyadic.zero
assert d1 * 2 == 2 * (A.i | A.i)
assert d1 / 2. == 0.5 * d1
assert d1.dot(0 * d1) == Vector.zero
assert d1 & d2 == Dyadic.zero
assert d1.dot(A.i) == A.i == d1 & A.i
assert d1.cross(Vector.zero) == Dyadic.zero
assert d1.cross(A.i) == Dyadic.zero
assert d1 ^ A.j == d1.cross(A.j)
assert d1.cross(A.k) == -A.i | A.j
assert d2.cross(A.i) == -A.j | A.k == d2 ^ A.i
assert A.i ^ d1 == Dyadic.zero
assert A.j.cross(d1) == -A.k | A.i == A.j ^ d1
assert Vector.zero.cross(d1) == Dyadic.zero
assert A.k ^ d1 == A.j | A.i
assert A.i.dot(d1) == A.i & d1 == A.i
assert A.j.dot(d1) == Vector.zero
assert Vector.zero.dot(d1) == Vector.zero
assert A.j & d2 == A.j
assert d1.dot(d3) == d1 & d3 == A.i | A.j == d3
assert d3 & d1 == Dyadic.zero
q = symbols('q')
B = A.orient_new_axis('B', q, A.k)
assert express(d1, B) == express(d1, B, B)
expr1 = ((cos(q)**2) * (B.i | B.i) + (-sin(q) * cos(q)) * (B.i | B.j) +
(-sin(q) * cos(q)) * (B.j | B.i) + (sin(q)**2) * (B.j | B.j))
assert (express(d1, B) - expr1).simplify() == Dyadic.zero
expr2 = (cos(q)) * (B.i | A.i) + (-sin(q)) * (B.j | A.i)
assert (express(d1, B, A) - expr2).simplify() == Dyadic.zero
expr3 = (cos(q)) * (A.i | B.i) + (-sin(q)) * (A.i | B.j)
assert (express(d1, A, B) - expr3).simplify() == Dyadic.zero
assert d1.to_matrix(A) == Matrix([[1, 0, 0], [0, 0, 0], [0, 0, 0]])
assert d1.to_matrix(A, B) == Matrix([[cos(q), -sin(q), 0], [0, 0, 0],
[0, 0, 0]])
assert d3.to_matrix(A) == Matrix([[0, 1, 0], [0, 0, 0], [0, 0, 0]])
a, b, c, d, e, f = symbols('a, b, c, d, e, f')
v1 = a * A.i + b * A.j + c * A.k
v2 = d * A.i + e * A.j + f * A.k
d4 = v1.outer(v2)
assert d4.to_matrix(A) == Matrix([[a * d, a * e, a * f],
[b * d, b * e, b * f],
[c * d, c * e, c * f]])
d5 = v1.outer(v1)
C = A.orient_new_axis('C', q, A.i)
for expected, actual in zip(C.rotation_matrix(A) * d5.to_matrix(A) * \
C.rotation_matrix(A).T, d5.to_matrix(C)):
assert (expected - actual).simplify() == 0
def test_Vector_diffs():
q1, q2, q3, q4 = dynamicsymbols('q1 q2 q3 q4')
q1d, q2d, q3d, q4d = dynamicsymbols('q1 q2 q3 q4', 1)
q1dd, q2dd, q3dd, q4dd = dynamicsymbols('q1 q2 q3 q4', 2)
N = ReferenceFrame('N')
A = N.orientnew('A', 'Axis', [q3, N.z])
B = A.orientnew('B', 'Axis', [q2, A.x])
v1 = q2 * A.x + q3 * N.y
v2 = q3 * B.x + v1
v3 = v1.dt(B)
v4 = v2.dt(B)
v5 = q1*A.x + q2*A.y + q3*A.z
assert v1.dt(N) == q2d * A.x + q2 * q3d * A.y + q3d * N.y
assert v1.dt(A) == q2d * A.x + q3 * q3d * N.x + q3d * N.y
assert v1.dt(B) == (q2d * A.x + q3 * q3d * N.x + q3d *
N.y - q3 * cos(q3) * q2d * N.z)
assert v2.dt(N) == (q2d * A.x + (q2 + q3) * q3d * A.y + q3d * B.x + q3d *
N.y)
assert v2.dt(A) == q2d * A.x + q3d * B.x + q3 * q3d * N.x + q3d * N.y
assert v2.dt(B) == (q2d * A.x + q3d * B.x + q3 * q3d * N.x + q3d * N.y -
q3 * cos(q3) * q2d * N.z)
assert v3.dt(N) == (q2dd * A.x + q2d * q3d * A.y + (q3d**2 + q3 * q3dd) *
N.x + q3dd * N.y + (q3 * sin(q3) * q2d * q3d -
cos(q3) * q2d * q3d - q3 * cos(q3) * q2dd) * N.z)
assert v3.dt(A) == (q2dd * A.x + (2 * q3d**2 + q3 * q3dd) * N.x + (q3dd -
q3 * q3d**2) * N.y + (q3 * sin(q3) * q2d * q3d -
cos(q3) * q2d * q3d - q3 * cos(q3) * q2dd) * N.z)
assert v3.dt(B) == (q2dd * A.x - q3 * cos(q3) * q2d**2 * A.y + (2 *
q3d**2 + q3 * q3dd) * N.x + (q3dd - q3 * q3d**2) *
N.y + (2 * q3 * sin(q3) * q2d * q3d - 2 * cos(q3) *
q2d * q3d - q3 * cos(q3) * q2dd) * N.z)
assert v4.dt(N) == (q2dd * A.x + q3d * (q2d + q3d) * A.y + q3dd * B.x +
(q3d**2 + q3 * q3dd) * N.x + q3dd * N.y + (q3 *
sin(q3) * q2d * q3d - cos(q3) * q2d * q3d - q3 *
cos(q3) * q2dd) * N.z)
assert v4.dt(A) == (q2dd * A.x + q3dd * B.x + (2 * q3d**2 + q3 * q3dd) *
N.x + (q3dd - q3 * q3d**2) * N.y + (q3 * sin(q3) *
q2d * q3d - cos(q3) * q2d * q3d - q3 * cos(q3) *
q2dd) * N.z)
assert v4.dt(B) == (q2dd * A.x - q3 * cos(q3) * q2d**2 * A.y + q3dd * B.x +
(2 * q3d**2 + q3 * q3dd) * N.x + (q3dd - q3 * q3d**2) *
N.y + (2 * q3 * sin(q3) * q2d * q3d - 2 * cos(q3) *
q2d * q3d - q3 * cos(q3) * q2dd) * N.z)
assert v5.dt(B) == q1d*A.x + (q3*q2d + q2d)*A.y + (-q2*q2d + q3d)*A.z
assert v5.dt(A) == q1d*A.x + q2d*A.y + q3d*A.z
assert v5.dt(N) == (-q2*q3d + q1d)*A.x + (q1*q3d + q2d)*A.y + q3d*A.z
assert v3.diff(q1d, N) == 0
assert v3.diff(q2d, N) == A.x - q3 * cos(q3) * N.z
assert v3.diff(q3d, N) == q3 * N.x + N.y
assert v3.diff(q1d, A) == 0
assert v3.diff(q2d, A) == A.x - q3 * cos(q3) * N.z
assert v3.diff(q3d, A) == q3 * N.x + N.y
assert v3.diff(q1d, B) == 0
assert v3.diff(q2d, B) == A.x - q3 * cos(q3) * N.z
assert v3.diff(q3d, B) == q3 * N.x + N.y
assert v4.diff(q1d, N) == 0
assert v4.diff(q2d, N) == A.x - q3 * cos(q3) * N.z
assert v4.diff(q3d, N) == B.x + q3 * N.x + N.y
assert v4.diff(q1d, A) == 0
assert v4.diff(q2d, A) == A.x - q3 * cos(q3) * N.z
assert v4.diff(q3d, A) == B.x + q3 * N.x + N.y
assert v4.diff(q1d, B) == 0
assert v4.diff(q2d, B) == A.x - q3 * cos(q3) * N.z
assert v4.diff(q3d, B) == B.x + q3 * N.x + N.y
# diff() should only express vector components in the derivative frame if
# the orientation of the component's frame depends on the variable
v6 = q2**2*N.y + q2**2*A.y + q2**2*B.y
# already expressed in N
n_measy = 2*q2
# A_C_N does not depend on q2, so don't express in N
a_measy = 2*q2
# B_C_N depends on q2, so express in N
b_measx = (q2**2*B.y).dot(N.x).diff(q2)
b_measy = (q2**2*B.y).dot(N.y).diff(q2)
b_measz = (q2**2*B.y).dot(N.z).diff(q2)
n_comp, a_comp = v6.diff(q2, N).args
assert len(v6.diff(q2, N).args) == 2 # only N and A parts
assert n_comp[1] == N
assert a_comp[1] == A
assert n_comp[0] == Matrix([b_measx, b_measy + n_measy, b_measz])
assert a_comp[0] == Matrix([0, a_measy, 0])