def perpendicular_plane(self, pt):
"""
Plane perpendicular to the given plane and passing through the point pt.
Parameters
==========
pt: Point3D
Returns
=======
Plane
Examples
========
>>> from sympy import Plane, Point3D
>>> a = Plane(Point3D(1,4,6), normal_vector=[2, 4, 6])
>>> a.perpendicular_plane(Point3D(2, 3, 5))
Plane(Point3D(2, 3, 5), [2, 8, -6])
"""
a = Matrix(self.normal_vector)
b, c = pt, self.p1
d = Matrix(b.direction_ratio(c))
e = list(d.cross(a))
return Plane(pt, normal_vector=e)
def perpendicular_plane(self, pt):
"""
Plane perpendicular to the given plane and passing through the point pt.
Parameters
==========
pt: Point3D
Returns
=======
Plane
Examples
========
>>> from sympy import Plane, Point3D
>>> a = Plane(Point3D(1,4,6), normal_vector=[2, 4, 6])
>>> a.perpendicular_plane(Point3D(2, 3, 5))
Plane(Point3D(2, 3, 5), [2, 8, -6])
"""
a = Matrix(self.normal_vector)
b, c = pt, self.p1
d = Matrix(b.direction_ratio(c))
e = list(d.cross(a))
return Plane(pt, normal_vector=e)
def is_perpendicular(self, l):
"""is the given geometric entity perpendicualar to the given plane?
Parameters
==========
LinearEntity3D or Plane
Returns
=======
Boolean
Examples
========
>>> from sympy import Plane, Point3D
>>> a = Plane(Point3D(1,4,6), normal_vector=(2, 4, 6))
>>> b = Plane(Point3D(2, 2, 2), normal_vector=(-1, 2, -1))
>>> a.is_perpendicular(b)
True
"""
from sympy.geometry.line3d import LinearEntity3D
if isinstance(l, LinearEntity3D):
a = Matrix(l.direction_ratio)
b = Matrix(self.normal_vector)
if a.cross(b).is_zero:
return True
else:
return False
elif isinstance(l, Plane):
a = Matrix(l.normal_vector)
b = Matrix(self.normal_vector)
if a.dot(b) == 0:
return True
else:
return False
else:
return False
def is_perpendicular(self, l):
"""is the given geometric entity perpendicualar to the given plane?
Parameters
==========
LinearEntity3D or Plane
Returns
=======
Boolean
Examples
========
>>> from sympy import Plane, Point3D
>>> a = Plane(Point3D(1,4,6), normal_vector=(2, 4, 6))
>>> b = Plane(Point3D(2, 2, 2), normal_vector=(-1, 2, -1))
>>> a.is_perpendicular(b)
True
"""
from sympy.geometry.line import LinearEntity3D
if isinstance(l, LinearEntity3D):
a = Matrix(l.direction_ratio)
b = Matrix(self.normal_vector)
if a.cross(b).is_zero_matrix:
return True
else:
return False
elif isinstance(l, Plane):
a = Matrix(l.normal_vector)
b = Matrix(self.normal_vector)
if a.dot(b) == 0:
return True
else:
return False
else:
return False
def is_parallel(self, l):
"""Is the given geometric entity parallel to the plane?
Parameters
==========
LinearEntity3D or Plane
Returns
=======
Boolean
Examples
========
>>> from sympy import Plane, Point3D
>>> a = Plane(Point3D(1,4,6), normal_vector=(2, 4, 6))
>>> b = Plane(Point3D(3,1,3), normal_vector=(4, 8, 12))
>>> a.is_parallel(b)
True
"""
from sympy.geometry.line3d import LinearEntity3D
if isinstance(l, LinearEntity3D):
a = l.direction_ratio
b = self.normal_vector
c = sum([i * j for i, j in zip(a, b)])
if c == 0:
return True
else:
return False
elif isinstance(l, Plane):
a = Matrix(l.normal_vector)
b = Matrix(self.normal_vector)
if a.cross(b).is_zero:
return True
else:
return False
def is_parallel(self, l):
"""Is the given geometric entity parallel to the plane?
Parameters
==========
LinearEntity3D or Plane
Returns
=======
Boolean
Examples
========
>>> from sympy import Plane, Point3D
>>> a = Plane(Point3D(1,4,6), normal_vector=(2, 4, 6))
>>> b = Plane(Point3D(3,1,3), normal_vector=(4, 8, 12))
>>> a.is_parallel(b)
True
"""
from sympy.geometry.line import LinearEntity3D
if isinstance(l, LinearEntity3D):
a = l.direction_ratio
b = self.normal_vector
c = sum([i * j for i, j in zip(a, b)])
if c == 0:
return True
else:
return False
elif isinstance(l, Plane):
a = Matrix(l.normal_vector)
b = Matrix(self.normal_vector)
if a.cross(b).is_zero_matrix:
return True
else:
return False
def are_coplanar(*lines):
""" Returns True if the given lines are coplanar otherwise False
Parameters
==========
lines: list
Returns
=======
Boolean
Examples
========
>>> from sympy import Plane, Point3D, Line3D
>>> a = Line3D(Point3D(5, 0, 0), Point3D(1, -1, 1))
>>> b = Line3D(Point3D(0, -2, 0), Point3D(3, 1, 1))
>>> c = Line3D(Point3D(0, -1, 0), Point3D(5, -1, 9))
>>> Plane.are_coplanar(a, b, c)
False
"""
if all(isinstance(i, Line3D) for i in lines):
if len(lines) < 2:
return False
a = Matrix(lines[0].direction_ratio)
b = Matrix(lines[1].direction_ratio)
c = list(a.cross(b))
d = Plane(lines[0].p1, normal_vector=c)
for i in lines[2:]:
if i not in d:
return False
return True
else:
ValueError('Enter Lines only')
def are_coplanar(*lines):
""" Returns True if the given lines are coplanar otherwise False
Parameters
==========
lines: list
Returns
=======
Boolean
Examples
========
>>> from sympy import Plane, Point3D, Line3D
>>> a = Line3D(Point3D(5, 0, 0), Point3D(1, -1, 1))
>>> b = Line3D(Point3D(0, -2, 0), Point3D(3, 1, 1))
>>> c = Line3D(Point3D(0, -1, 0), Point3D(5, -1, 9))
>>> Plane.are_coplanar(a, b, c)
False
"""
if all(isinstance(i, Line3D) for i in lines):
if len(lines) < 2:
return False
a = Matrix(lines[0].direction_ratio)
b = Matrix(lines[1].direction_ratio)
c = list(a.cross(b))
d = Plane(lines[0].p1, normal_vector=c)
for i in lines[2:]:
if i not in d:
return False
return True
else:
ValueError('Enter Lines only')
def intersection(self, o):
""" The intersection with other geometrical entity.
Parameters
==========
Point, Point3D, LinearEntity, LinearEntity3D, Plane
Returns
=======
List
Examples
========
>>> from sympy import Point, Point3D, Line, Line3D, Plane
>>> a = Plane(Point3D(1, 2, 3), normal_vector=(1, 1, 1))
>>> b = Point3D(1, 2, 3)
>>> a.intersection(b)
[Point3D(1, 2, 3)]
>>> c = Line3D(Point3D(1, 4, 7), Point3D(2, 2, 2))
>>> a.intersection(c)
[Point3D(2, 2, 2)]
>>> d = Plane(Point3D(6, 0, 0), normal_vector=(2, -5, 3))
>>> e = Plane(Point3D(2, 0, 0), normal_vector=(3, 4, -3))
>>> d.intersection(e)
[Line3D(Point3D(78/23, -24/23, 0), Point3D(147/23, 321/23, 23))]
"""
from sympy.geometry.line import LinearEntity, LinearEntity3D
if not isinstance(o, GeometryEntity):
o = Point(o, dim=3)
if isinstance(o, Point):
if o in self:
return [o]
else:
return []
if isinstance(o, (LinearEntity, LinearEntity3D)):
# recast to 3D
p1, p2 = o.p1, o.p2
if isinstance(o, Segment):
o = Segment3D(p1, p2)
elif isinstance(o, Ray):
o = Ray3D(p1, p2)
elif isinstance(o, Line):
o = Line3D(p1, p2)
else:
raise ValueError("unhandled linear entity: %s" % o.func)
if o in self:
return [o]
else:
t = Dummy() # unnamed else it may clash with a symbol in o
a = Point3D(o.arbitrary_point(t))
p1, n = self.p1, Point3D(self.normal_vector)
# TODO: Replace solve with solveset, when this line is tested
c = solve((a - p1).dot(n), t)
if not c:
return []
else:
c = [i for i in c if i.is_real is not False]
if len(c) > 1:
c = [i for i in c if i.is_real]
if len(c) != 1:
raise Undecidable("not sure which point is real")
p = a.subs(t, c[0])
if p not in o:
return [] # e.g. a segment might not intersect a plane
return [p]
if isinstance(o, Plane):
if self.equals(o):
return [self]
if self.is_parallel(o):
return []
else:
x, y, z = map(Dummy, "xyz")
a, b = Matrix([self.normal_vector]), Matrix([o.normal_vector])
c = list(a.cross(b))
d = self.equation(x, y, z)
e = o.equation(x, y, z)
result = list(linsolve([d, e], x, y, z))[0]
for i in (x, y, z):
result = result.subs(i, 0)
return [Line3D(Point3D(result), direction_ratio=c)]
def test_sparse_matrix():
def sparse_eye(n):
return SparseMatrix.eye(n)
def sparse_zeros(n):
return SparseMatrix.zeros(n)
# creation args
raises(TypeError, lambda: SparseMatrix(1, 2))
a = SparseMatrix(((1, 0), (0, 1)))
assert SparseMatrix(a) == a
from sympy.matrices import MutableSparseMatrix, MutableDenseMatrix
a = MutableSparseMatrix([])
b = MutableDenseMatrix([1, 2])
assert a.row_join(b) == b
assert a.col_join(b) == b
assert type(a.row_join(b)) == type(a)
assert type(a.col_join(b)) == type(a)
# make sure 0 x n matrices get stacked correctly
sparse_matrices = [SparseMatrix.zeros(0, n) for n in range(4)]
assert SparseMatrix.hstack(*sparse_matrices) == Matrix(0, 6, [])
sparse_matrices = [SparseMatrix.zeros(n, 0) for n in range(4)]
assert SparseMatrix.vstack(*sparse_matrices) == Matrix(6, 0, [])
# test element assignment
a = SparseMatrix(((1, 0), (0, 1)))
a[3] = 4
assert a[1, 1] == 4
a[3] = 1
a[0, 0] = 2
assert a == SparseMatrix(((2, 0), (0, 1)))
a[1, 0] = 5
assert a == SparseMatrix(((2, 0), (5, 1)))
a[1, 1] = 0
assert a == SparseMatrix(((2, 0), (5, 0)))
assert a._smat == {(0, 0): 2, (1, 0): 5}
# test_multiplication
a = SparseMatrix((
(1, 2),
(3, 1),
(0, 6),
))
b = SparseMatrix((
(1, 2),
(3, 0),
))
c = a * b
assert c[0, 0] == 7
assert c[0, 1] == 2
assert c[1, 0] == 6
assert c[1, 1] == 6
assert c[2, 0] == 18
assert c[2, 1] == 0
try:
eval('c = a @ b')
except SyntaxError:
pass
else:
assert c[0, 0] == 7
assert c[0, 1] == 2
assert c[1, 0] == 6
assert c[1, 1] == 6
assert c[2, 0] == 18
assert c[2, 1] == 0
x = Symbol("x")
c = b * Symbol("x")
assert isinstance(c, SparseMatrix)
assert c[0, 0] == x
assert c[0, 1] == 2 * x
assert c[1, 0] == 3 * x
assert c[1, 1] == 0
c = 5 * b
assert isinstance(c, SparseMatrix)
assert c[0, 0] == 5
assert c[0, 1] == 2 * 5
assert c[1, 0] == 3 * 5
assert c[1, 1] == 0
#test_power
A = SparseMatrix([[2, 3], [4, 5]])
assert (A**5)[:] == [6140, 8097, 10796, 14237]
A = SparseMatrix([[2, 1, 3], [4, 2, 4], [6, 12, 1]])
assert (A**3)[:] == [290, 262, 251, 448, 440, 368, 702, 954, 433]
# test_creation
x = Symbol("x")
a = SparseMatrix([[x, 0], [0, 0]])
m = a
assert m.cols == m.rows
assert m.cols == 2
assert m[:] == [x, 0, 0, 0]
b = SparseMatrix(2, 2, [x, 0, 0, 0])
m = b
assert m.cols == m.rows
assert m.cols == 2
assert m[:] == [x, 0, 0, 0]
assert a == b
S = sparse_eye(3)
S.row_del(1)
assert S == SparseMatrix([[1, 0, 0], [0, 0, 1]])
S = sparse_eye(3)
S.col_del(1)
assert S == SparseMatrix([[1, 0], [0, 0], [0, 1]])
S = SparseMatrix.eye(3)
S[2, 1] = 2
S.col_swap(1, 0)
assert S == SparseMatrix([[0, 1, 0], [1, 0, 0], [2, 0, 1]])
a = SparseMatrix(1, 2, [1, 2])
b = a.copy()
c = a.copy()
assert a[0] == 1
a.row_del(0)
assert a == SparseMatrix(0, 2, [])
b.col_del(1)
assert b == SparseMatrix(1, 1, [1])
assert SparseMatrix([[1, 2, 3], [1, 2], [1]]) == Matrix([[1, 2, 3],
[1, 2, 0],
[1, 0, 0]])
assert SparseMatrix(4, 4, {(1, 1): sparse_eye(2)}) == Matrix([[0, 0, 0, 0],
[0, 1, 0, 0],
[0, 0, 1, 0],
[0, 0, 0,
0]])
raises(ValueError, lambda: SparseMatrix(1, 1, {(1, 1): 1}))
assert SparseMatrix(1, 2, [1, 2]).tolist() == [[1, 2]]
assert SparseMatrix(2, 2, [1, [2, 3]]).tolist() == [[1, 0], [2, 3]]
raises(ValueError, lambda: SparseMatrix(2, 2, [1]))
raises(ValueError, lambda: SparseMatrix(1, 1, [[1, 2]]))
assert SparseMatrix([.1]).has(Float)
# autosizing
assert SparseMatrix(None, {(0, 1): 0}).shape == (0, 0)
assert SparseMatrix(None, {(0, 1): 1}).shape == (1, 2)
assert SparseMatrix(None, None, {(0, 1): 1}).shape == (1, 2)
raises(ValueError, lambda: SparseMatrix(None, 1, [[1, 2]]))
raises(ValueError, lambda: SparseMatrix(1, None, [[1, 2]]))
raises(ValueError, lambda: SparseMatrix(3, 3, {
(0, 0): ones(2),
(1, 1): 2
}))
# test_determinant
x, y = Symbol('x'), Symbol('y')
assert SparseMatrix(1, 1, [0]).det() == 0
assert SparseMatrix([[1]]).det() == 1
assert SparseMatrix(((-3, 2), (8, -5))).det() == -1
assert SparseMatrix(((x, 1), (y, 2 * y))).det() == 2 * x * y - y
assert SparseMatrix(((1, 1, 1), (1, 2, 3), (1, 3, 6))).det() == 1
assert SparseMatrix(((3, -2, 0, 5), (-2, 1, -2, 2), (0, -2, 5, 0),
(5, 0, 3, 4))).det() == -289
assert SparseMatrix(((1, 2, 3, 4), (5, 6, 7, 8), (9, 10, 11, 12),
(13, 14, 15, 16))).det() == 0
assert SparseMatrix(((3, 2, 0, 0, 0), (0, 3, 2, 0, 0), (0, 0, 3, 2, 0),
(0, 0, 0, 3, 2), (2, 0, 0, 0, 3))).det() == 275
assert SparseMatrix(((1, 0, 1, 2, 12), (2, 0, 1, 1, 4), (2, 1, 1, -1, 3),
(3, 2, -1, 1, 8), (1, 1, 1, 0, 6))).det() == -55
assert SparseMatrix(((-5, 2, 3, 4, 5), (1, -4, 3, 4, 5), (1, 2, -3, 4, 5),
(1, 2, 3, -2, 5), (1, 2, 3, 4, -1))).det() == 11664
assert SparseMatrix(((2, 7, -1, 3, 2), (0, 0, 1, 0, 1), (-2, 0, 7, 0, 2),
(-3, -2, 4, 5, 3), (1, 0, 0, 0, 1))).det() == 123
# test_slicing
m0 = sparse_eye(4)
assert m0[:3, :3] == sparse_eye(3)
assert m0[2:4, 0:2] == sparse_zeros(2)
m1 = SparseMatrix(3, 3, lambda i, j: i + j)
assert m1[0, :] == SparseMatrix(1, 3, (0, 1, 2))
assert m1[1:3, 1] == SparseMatrix(2, 1, (2, 3))
m2 = SparseMatrix([[0, 1, 2, 3], [4, 5, 6, 7], [8, 9, 10, 11],
[12, 13, 14, 15]])
assert m2[:, -1] == SparseMatrix(4, 1, [3, 7, 11, 15])
assert m2[-2:, :] == SparseMatrix([[8, 9, 10, 11], [12, 13, 14, 15]])
assert SparseMatrix([[1, 2], [3, 4]])[[1], [1]] == Matrix([[4]])
# test_submatrix_assignment
m = sparse_zeros(4)
m[2:4, 2:4] = sparse_eye(2)
assert m == SparseMatrix([(0, 0, 0, 0), (0, 0, 0, 0), (0, 0, 1, 0),
(0, 0, 0, 1)])
assert len(m._smat) == 2
m[:2, :2] = sparse_eye(2)
assert m == sparse_eye(4)
m[:, 0] = SparseMatrix(4, 1, (1, 2, 3, 4))
assert m == SparseMatrix([(1, 0, 0, 0), (2, 1, 0, 0), (3, 0, 1, 0),
(4, 0, 0, 1)])
m[:, :] = sparse_zeros(4)
assert m == sparse_zeros(4)
m[:, :] = ((1, 2, 3, 4), (5, 6, 7, 8), (9, 10, 11, 12), (13, 14, 15, 16))
assert m == SparseMatrix(
((1, 2, 3, 4), (5, 6, 7, 8), (9, 10, 11, 12), (13, 14, 15, 16)))
m[:2, 0] = [0, 0]
assert m == SparseMatrix(
((0, 2, 3, 4), (0, 6, 7, 8), (9, 10, 11, 12), (13, 14, 15, 16)))
# test_reshape
m0 = sparse_eye(3)
assert m0.reshape(1, 9) == SparseMatrix(1, 9, (1, 0, 0, 0, 1, 0, 0, 0, 1))
m1 = SparseMatrix(3, 4, lambda i, j: i + j)
assert m1.reshape(4, 3) == \
SparseMatrix([(0, 1, 2), (3, 1, 2), (3, 4, 2), (3, 4, 5)])
assert m1.reshape(2, 6) == \
SparseMatrix([(0, 1, 2, 3, 1, 2), (3, 4, 2, 3, 4, 5)])
# test_applyfunc
m0 = sparse_eye(3)
assert m0.applyfunc(lambda x: 2 * x) == sparse_eye(3) * 2
assert m0.applyfunc(lambda x: 0) == sparse_zeros(3)
# test__eval_Abs
assert abs(SparseMatrix(((x, 1), (y, 2 * y)))) == SparseMatrix(
((Abs(x), 1), (Abs(y), 2 * Abs(y))))
# test_LUdecomp
testmat = SparseMatrix([[0, 2, 5, 3], [3, 3, 7, 4], [8, 4, 0, 2],
[-2, 6, 3, 4]])
L, U, p = testmat.LUdecomposition()
assert L.is_lower
assert U.is_upper
assert (L * U).permute_rows(p, 'backward') - testmat == sparse_zeros(4)
testmat = SparseMatrix([[6, -2, 7, 4], [0, 3, 6, 7], [1, -2, 7, 4],
[-9, 2, 6, 3]])
L, U, p = testmat.LUdecomposition()
assert L.is_lower
assert U.is_upper
assert (L * U).permute_rows(p, 'backward') - testmat == sparse_zeros(4)
x, y, z = Symbol('x'), Symbol('y'), Symbol('z')
M = Matrix(((1, x, 1), (2, y, 0), (y, 0, z)))
L, U, p = M.LUdecomposition()
assert L.is_lower
assert U.is_upper
assert (L * U).permute_rows(p, 'backward') - M == sparse_zeros(3)
# test_LUsolve
A = SparseMatrix([[2, 3, 5], [3, 6, 2], [8, 3, 6]])
x = SparseMatrix(3, 1, [3, 7, 5])
b = A * x
soln = A.LUsolve(b)
assert soln == x
A = SparseMatrix([[0, -1, 2], [5, 10, 7], [8, 3, 4]])
x = SparseMatrix(3, 1, [-1, 2, 5])
b = A * x
soln = A.LUsolve(b)
assert soln == x
# test_inverse
A = sparse_eye(4)
assert A.inv() == sparse_eye(4)
assert A.inv(method="CH") == sparse_eye(4)
assert A.inv(method="LDL") == sparse_eye(4)
A = SparseMatrix([[2, 3, 5], [3, 6, 2], [7, 2, 6]])
Ainv = SparseMatrix(Matrix(A).inv())
assert A * Ainv == sparse_eye(3)
assert A.inv(method="CH") == Ainv
assert A.inv(method="LDL") == Ainv
A = SparseMatrix([[2, 3, 5], [3, 6, 2], [5, 2, 6]])
Ainv = SparseMatrix(Matrix(A).inv())
assert A * Ainv == sparse_eye(3)
assert A.inv(method="CH") == Ainv
assert A.inv(method="LDL") == Ainv
# test_cross
v1 = Matrix(1, 3, [1, 2, 3])
v2 = Matrix(1, 3, [3, 4, 5])
assert v1.cross(v2) == Matrix(1, 3, [-2, 4, -2])
assert v1.norm(2)**2 == 14
# conjugate
a = SparseMatrix(((1, 2 + I), (3, 4)))
assert a.C == SparseMatrix([[1, 2 - I], [3, 4]])
# mul
assert a * Matrix(2, 2, [1, 0, 0, 1]) == a
assert a + Matrix(2, 2, [1, 1, 1, 1]) == SparseMatrix([[2, 3 + I], [4, 5]])
# col join
assert a.col_join(sparse_eye(2)) == SparseMatrix([[1, 2 + I], [3, 4],
[1, 0], [0, 1]])
# symmetric
assert not a.is_symmetric(simplify=False)
# test_cofactor
assert sparse_eye(3) == sparse_eye(3).cofactor_matrix()
test = SparseMatrix([[1, 3, 2], [2, 6, 3], [2, 3, 6]])
assert test.cofactor_matrix() == \
SparseMatrix([[27, -6, -6], [-12, 2, 3], [-3, 1, 0]])
test = SparseMatrix([[1, 2, 3], [4, 5, 6], [7, 8, 9]])
assert test.cofactor_matrix() == \
SparseMatrix([[-3, 6, -3], [6, -12, 6], [-3, 6, -3]])
# test_jacobian
x = Symbol('x')
y = Symbol('y')
L = SparseMatrix(1, 2, [x**2 * y, 2 * y**2 + x * y])
syms = [x, y]
assert L.jacobian(syms) == Matrix([[2 * x * y, x**2], [y, 4 * y + x]])
L = SparseMatrix(1, 2, [x, x**2 * y**3])
assert L.jacobian(syms) == SparseMatrix([[1, 0],
[2 * x * y**3, x**2 * 3 * y**2]])
# test_QR
A = Matrix([[1, 2], [2, 3]])
Q, S = A.QRdecomposition()
R = Rational
assert Q == Matrix([[5**R(-1, 2), (R(2) / 5) * (R(1) / 5)**R(-1, 2)],
[2 * 5**R(-1, 2), (-R(1) / 5) * (R(1) / 5)**R(-1, 2)]])
assert S == Matrix([[5**R(1, 2), 8 * 5**R(-1, 2)],
[0, (R(1) / 5)**R(1, 2)]])
assert Q * S == A
assert Q.T * Q == sparse_eye(2)
R = Rational
# test nullspace
# first test reduced row-ech form
M = SparseMatrix([[5, 7, 2, 1], [1, 6, 2, -1]])
out, tmp = M.rref()
assert out == Matrix([[1, 0, -R(2) / 23, R(13) / 23],
[0, 1, R(8) / 23, R(-6) / 23]])
M = SparseMatrix([[1, 3, 0, 2, 6, 3, 1], [-2, -6, 0, -2, -8, 3, 1],
[3, 9, 0, 0, 6, 6, 2], [-1, -3, 0, 1, 0, 9, 3]])
out, tmp = M.rref()
assert out == Matrix([[1, 3, 0, 0, 2, 0, 0], [0, 0, 0, 1, 2, 0, 0],
[0, 0, 0, 0, 0, 1, R(1) / 3], [0, 0, 0, 0, 0, 0, 0]])
# now check the vectors
basis = M.nullspace()
assert basis[0] == Matrix([-3, 1, 0, 0, 0, 0, 0])
assert basis[1] == Matrix([0, 0, 1, 0, 0, 0, 0])
assert basis[2] == Matrix([-2, 0, 0, -2, 1, 0, 0])
assert basis[3] == Matrix([0, 0, 0, 0, 0, R(-1) / 3, 1])
# test eigen
x = Symbol('x')
y = Symbol('y')
sparse_eye3 = sparse_eye(3)
assert sparse_eye3.charpoly(x) == PurePoly(((x - 1)**3))
assert sparse_eye3.charpoly(y) == PurePoly(((y - 1)**3))
# test values
M = Matrix([(0, 1, -1), (1, 1, 0), (-1, 0, 1)])
vals = M.eigenvals()
assert sorted(vals.keys()) == [-1, 1, 2]
R = Rational
M = Matrix([[1, 0, 0], [0, 1, 0], [0, 0, 1]])
assert M.eigenvects() == [
(1, 3, [Matrix([1, 0, 0]),
Matrix([0, 1, 0]),
Matrix([0, 0, 1])])
]
M = Matrix([[5, 0, 2], [3, 2, 0], [0, 0, 1]])
assert M.eigenvects() == [(1, 1, [Matrix([R(-1) / 2,
R(3) / 2, 1])]),
(2, 1, [Matrix([0, 1, 0])]),
(5, 1, [Matrix([1, 1, 0])])]
assert M.zeros(3, 5) == SparseMatrix(3, 5, {})
A = SparseMatrix(
10, 10, {
(0, 0): 18,
(0, 9): 12,
(1, 4): 18,
(2, 7): 16,
(3, 9): 12,
(4, 2): 19,
(5, 7): 16,
(6, 2): 12,
(9, 7): 18
})
assert A.row_list() == [(0, 0, 18), (0, 9, 12), (1, 4, 18), (2, 7, 16),
(3, 9, 12), (4, 2, 19), (5, 7, 16), (6, 2, 12),
(9, 7, 18)]
assert A.col_list() == [(0, 0, 18), (4, 2, 19), (6, 2, 12), (1, 4, 18),
(2, 7, 16), (5, 7, 16), (9, 7, 18), (0, 9, 12),
(3, 9, 12)]
assert SparseMatrix.eye(2).nnz() == 2
def test_sparse_matrix():
def sparse_eye(n):
return SparseMatrix.eye(n)
def sparse_zeros(n):
return SparseMatrix.zeros(n)
# creation args
raises(TypeError, lambda: SparseMatrix(1, 2))
a = SparseMatrix((
(1, 0),
(0, 1)
))
assert SparseMatrix(a) == a
from sympy.matrices import MutableSparseMatrix, MutableDenseMatrix
a = MutableSparseMatrix([])
b = MutableDenseMatrix([1, 2])
assert a.row_join(b) == b
assert a.col_join(b) == b
assert type(a.row_join(b)) == type(a)
assert type(a.col_join(b)) == type(a)
# make sure 0 x n matrices get stacked correctly
sparse_matrices = [SparseMatrix.zeros(0, n) for n in range(4)]
assert SparseMatrix.hstack(*sparse_matrices) == Matrix(0, 6, [])
sparse_matrices = [SparseMatrix.zeros(n, 0) for n in range(4)]
assert SparseMatrix.vstack(*sparse_matrices) == Matrix(6, 0, [])
# test element assignment
a = SparseMatrix((
(1, 0),
(0, 1)
))
a[3] = 4
assert a[1, 1] == 4
a[3] = 1
a[0, 0] = 2
assert a == SparseMatrix((
(2, 0),
(0, 1)
))
a[1, 0] = 5
assert a == SparseMatrix((
(2, 0),
(5, 1)
))
a[1, 1] = 0
assert a == SparseMatrix((
(2, 0),
(5, 0)
))
assert a._smat == {(0, 0): 2, (1, 0): 5}
# test_multiplication
a = SparseMatrix((
(1, 2),
(3, 1),
(0, 6),
))
b = SparseMatrix((
(1, 2),
(3, 0),
))
c = a*b
assert c[0, 0] == 7
assert c[0, 1] == 2
assert c[1, 0] == 6
assert c[1, 1] == 6
assert c[2, 0] == 18
assert c[2, 1] == 0
try:
eval('c = a @ b')
except SyntaxError:
pass
else:
assert c[0, 0] == 7
assert c[0, 1] == 2
assert c[1, 0] == 6
assert c[1, 1] == 6
assert c[2, 0] == 18
assert c[2, 1] == 0
x = Symbol("x")
c = b * Symbol("x")
assert isinstance(c, SparseMatrix)
assert c[0, 0] == x
assert c[0, 1] == 2*x
assert c[1, 0] == 3*x
assert c[1, 1] == 0
c = 5 * b
assert isinstance(c, SparseMatrix)
assert c[0, 0] == 5
assert c[0, 1] == 2*5
assert c[1, 0] == 3*5
assert c[1, 1] == 0
#test_power
A = SparseMatrix([[2, 3], [4, 5]])
assert (A**5)[:] == [6140, 8097, 10796, 14237]
A = SparseMatrix([[2, 1, 3], [4, 2, 4], [6, 12, 1]])
assert (A**3)[:] == [290, 262, 251, 448, 440, 368, 702, 954, 433]
# test_creation
x = Symbol("x")
a = SparseMatrix([[x, 0], [0, 0]])
m = a
assert m.cols == m.rows
assert m.cols == 2
assert m[:] == [x, 0, 0, 0]
b = SparseMatrix(2, 2, [x, 0, 0, 0])
m = b
assert m.cols == m.rows
assert m.cols == 2
assert m[:] == [x, 0, 0, 0]
assert a == b
S = sparse_eye(3)
S.row_del(1)
assert S == SparseMatrix([
[1, 0, 0],
[0, 0, 1]])
S = sparse_eye(3)
S.col_del(1)
assert S == SparseMatrix([
[1, 0],
[0, 0],
[0, 1]])
S = SparseMatrix.eye(3)
S[2, 1] = 2
S.col_swap(1, 0)
assert S == SparseMatrix([
[0, 1, 0],
[1, 0, 0],
[2, 0, 1]])
a = SparseMatrix(1, 2, [1, 2])
b = a.copy()
c = a.copy()
assert a[0] == 1
a.row_del(0)
assert a == SparseMatrix(0, 2, [])
b.col_del(1)
assert b == SparseMatrix(1, 1, [1])
# test_determinant
x, y = Symbol('x'), Symbol('y')
assert SparseMatrix(1, 1, [0]).det() == 0
assert SparseMatrix([[1]]).det() == 1
assert SparseMatrix(((-3, 2), (8, -5))).det() == -1
assert SparseMatrix(((x, 1), (y, 2*y))).det() == 2*x*y - y
assert SparseMatrix(( (1, 1, 1),
(1, 2, 3),
(1, 3, 6) )).det() == 1
assert SparseMatrix(( ( 3, -2, 0, 5),
(-2, 1, -2, 2),
( 0, -2, 5, 0),
( 5, 0, 3, 4) )).det() == -289
assert SparseMatrix(( ( 1, 2, 3, 4),
( 5, 6, 7, 8),
( 9, 10, 11, 12),
(13, 14, 15, 16) )).det() == 0
assert SparseMatrix(( (3, 2, 0, 0, 0),
(0, 3, 2, 0, 0),
(0, 0, 3, 2, 0),
(0, 0, 0, 3, 2),
(2, 0, 0, 0, 3) )).det() == 275
assert SparseMatrix(( (1, 0, 1, 2, 12),
(2, 0, 1, 1, 4),
(2, 1, 1, -1, 3),
(3, 2, -1, 1, 8),
(1, 1, 1, 0, 6) )).det() == -55
assert SparseMatrix(( (-5, 2, 3, 4, 5),
( 1, -4, 3, 4, 5),
( 1, 2, -3, 4, 5),
( 1, 2, 3, -2, 5),
( 1, 2, 3, 4, -1) )).det() == 11664
assert SparseMatrix(( ( 2, 7, -1, 3, 2),
( 0, 0, 1, 0, 1),
(-2, 0, 7, 0, 2),
(-3, -2, 4, 5, 3),
( 1, 0, 0, 0, 1) )).det() == 123
# test_slicing
m0 = sparse_eye(4)
assert m0[:3, :3] == sparse_eye(3)
assert m0[2:4, 0:2] == sparse_zeros(2)
m1 = SparseMatrix(3, 3, lambda i, j: i + j)
assert m1[0, :] == SparseMatrix(1, 3, (0, 1, 2))
assert m1[1:3, 1] == SparseMatrix(2, 1, (2, 3))
m2 = SparseMatrix(
[[0, 1, 2, 3], [4, 5, 6, 7], [8, 9, 10, 11], [12, 13, 14, 15]])
assert m2[:, -1] == SparseMatrix(4, 1, [3, 7, 11, 15])
assert m2[-2:, :] == SparseMatrix([[8, 9, 10, 11], [12, 13, 14, 15]])
assert SparseMatrix([[1, 2], [3, 4]])[[1], [1]] == Matrix([[4]])
# test_submatrix_assignment
m = sparse_zeros(4)
m[2:4, 2:4] = sparse_eye(2)
assert m == SparseMatrix([(0, 0, 0, 0),
(0, 0, 0, 0),
(0, 0, 1, 0),
(0, 0, 0, 1)])
assert len(m._smat) == 2
m[:2, :2] = sparse_eye(2)
assert m == sparse_eye(4)
m[:, 0] = SparseMatrix(4, 1, (1, 2, 3, 4))
assert m == SparseMatrix([(1, 0, 0, 0),
(2, 1, 0, 0),
(3, 0, 1, 0),
(4, 0, 0, 1)])
m[:, :] = sparse_zeros(4)
assert m == sparse_zeros(4)
m[:, :] = ((1, 2, 3, 4), (5, 6, 7, 8), (9, 10, 11, 12), (13, 14, 15, 16))
assert m == SparseMatrix((( 1, 2, 3, 4),
( 5, 6, 7, 8),
( 9, 10, 11, 12),
(13, 14, 15, 16)))
m[:2, 0] = [0, 0]
assert m == SparseMatrix((( 0, 2, 3, 4),
( 0, 6, 7, 8),
( 9, 10, 11, 12),
(13, 14, 15, 16)))
# test_reshape
m0 = sparse_eye(3)
assert m0.reshape(1, 9) == SparseMatrix(1, 9, (1, 0, 0, 0, 1, 0, 0, 0, 1))
m1 = SparseMatrix(3, 4, lambda i, j: i + j)
assert m1.reshape(4, 3) == \
SparseMatrix([(0, 1, 2), (3, 1, 2), (3, 4, 2), (3, 4, 5)])
assert m1.reshape(2, 6) == \
SparseMatrix([(0, 1, 2, 3, 1, 2), (3, 4, 2, 3, 4, 5)])
# test_applyfunc
m0 = sparse_eye(3)
assert m0.applyfunc(lambda x: 2*x) == sparse_eye(3)*2
assert m0.applyfunc(lambda x: 0 ) == sparse_zeros(3)
# test__eval_Abs
assert abs(SparseMatrix(((x, 1), (y, 2*y)))) == SparseMatrix(((Abs(x), 1), (Abs(y), 2*Abs(y))))
# test_LUdecomp
testmat = SparseMatrix([[ 0, 2, 5, 3],
[ 3, 3, 7, 4],
[ 8, 4, 0, 2],
[-2, 6, 3, 4]])
L, U, p = testmat.LUdecomposition()
assert L.is_lower
assert U.is_upper
assert (L*U).permute_rows(p, 'backward') - testmat == sparse_zeros(4)
testmat = SparseMatrix([[ 6, -2, 7, 4],
[ 0, 3, 6, 7],
[ 1, -2, 7, 4],
[-9, 2, 6, 3]])
L, U, p = testmat.LUdecomposition()
assert L.is_lower
assert U.is_upper
assert (L*U).permute_rows(p, 'backward') - testmat == sparse_zeros(4)
x, y, z = Symbol('x'), Symbol('y'), Symbol('z')
M = Matrix(((1, x, 1), (2, y, 0), (y, 0, z)))
L, U, p = M.LUdecomposition()
assert L.is_lower
assert U.is_upper
assert (L*U).permute_rows(p, 'backward') - M == sparse_zeros(3)
# test_LUsolve
A = SparseMatrix([[2, 3, 5],
[3, 6, 2],
[8, 3, 6]])
x = SparseMatrix(3, 1, [3, 7, 5])
b = A*x
soln = A.LUsolve(b)
assert soln == x
A = SparseMatrix([[0, -1, 2],
[5, 10, 7],
[8, 3, 4]])
x = SparseMatrix(3, 1, [-1, 2, 5])
b = A*x
soln = A.LUsolve(b)
assert soln == x
# test_inverse
A = sparse_eye(4)
assert A.inv() == sparse_eye(4)
assert A.inv(method="CH") == sparse_eye(4)
assert A.inv(method="LDL") == sparse_eye(4)
A = SparseMatrix([[2, 3, 5],
[3, 6, 2],
[7, 2, 6]])
Ainv = SparseMatrix(Matrix(A).inv())
assert A*Ainv == sparse_eye(3)
assert A.inv(method="CH") == Ainv
assert A.inv(method="LDL") == Ainv
A = SparseMatrix([[2, 3, 5],
[3, 6, 2],
[5, 2, 6]])
Ainv = SparseMatrix(Matrix(A).inv())
assert A*Ainv == sparse_eye(3)
assert A.inv(method="CH") == Ainv
assert A.inv(method="LDL") == Ainv
# test_cross
v1 = Matrix(1, 3, [1, 2, 3])
v2 = Matrix(1, 3, [3, 4, 5])
assert v1.cross(v2) == Matrix(1, 3, [-2, 4, -2])
assert v1.norm(2)**2 == 14
# conjugate
a = SparseMatrix(((1, 2 + I), (3, 4)))
assert a.C == SparseMatrix([
[1, 2 - I],
[3, 4]
])
# mul
assert a*Matrix(2, 2, [1, 0, 0, 1]) == a
assert a + Matrix(2, 2, [1, 1, 1, 1]) == SparseMatrix([
[2, 3 + I],
[4, 5]
])
# col join
assert a.col_join(sparse_eye(2)) == SparseMatrix([
[1, 2 + I],
[3, 4],
[1, 0],
[0, 1]
])
# symmetric
assert not a.is_symmetric(simplify=False)
# test_cofactor
assert sparse_eye(3) == sparse_eye(3).cofactor_matrix()
test = SparseMatrix([[1, 3, 2], [2, 6, 3], [2, 3, 6]])
assert test.cofactor_matrix() == \
SparseMatrix([[27, -6, -6], [-12, 2, 3], [-3, 1, 0]])
test = SparseMatrix([[1, 2, 3], [4, 5, 6], [7, 8, 9]])
assert test.cofactor_matrix() == \
SparseMatrix([[-3, 6, -3], [6, -12, 6], [-3, 6, -3]])
# test_jacobian
x = Symbol('x')
y = Symbol('y')
L = SparseMatrix(1, 2, [x**2*y, 2*y**2 + x*y])
syms = [x, y]
assert L.jacobian(syms) == Matrix([[2*x*y, x**2], [y, 4*y + x]])
L = SparseMatrix(1, 2, [x, x**2*y**3])
assert L.jacobian(syms) == SparseMatrix([[1, 0], [2*x*y**3, x**2*3*y**2]])
# test_QR
A = Matrix([[1, 2], [2, 3]])
Q, S = A.QRdecomposition()
R = Rational
assert Q == Matrix([
[ 5**R(-1, 2), (R(2)/5)*(R(1)/5)**R(-1, 2)],
[2*5**R(-1, 2), (-R(1)/5)*(R(1)/5)**R(-1, 2)]])
assert S == Matrix([
[5**R(1, 2), 8*5**R(-1, 2)],
[ 0, (R(1)/5)**R(1, 2)]])
assert Q*S == A
assert Q.T * Q == sparse_eye(2)
R = Rational
# test nullspace
# first test reduced row-ech form
M = SparseMatrix([[5, 7, 2, 1],
[1, 6, 2, -1]])
out, tmp = M.rref()
assert out == Matrix([[1, 0, -R(2)/23, R(13)/23],
[0, 1, R(8)/23, R(-6)/23]])
M = SparseMatrix([[ 1, 3, 0, 2, 6, 3, 1],
[-2, -6, 0, -2, -8, 3, 1],
[ 3, 9, 0, 0, 6, 6, 2],
[-1, -3, 0, 1, 0, 9, 3]])
out, tmp = M.rref()
assert out == Matrix([[1, 3, 0, 0, 2, 0, 0],
[0, 0, 0, 1, 2, 0, 0],
[0, 0, 0, 0, 0, 1, R(1)/3],
[0, 0, 0, 0, 0, 0, 0]])
# now check the vectors
basis = M.nullspace()
assert basis[0] == Matrix([-3, 1, 0, 0, 0, 0, 0])
assert basis[1] == Matrix([0, 0, 1, 0, 0, 0, 0])
assert basis[2] == Matrix([-2, 0, 0, -2, 1, 0, 0])
assert basis[3] == Matrix([0, 0, 0, 0, 0, R(-1)/3, 1])
# test eigen
x = Symbol('x')
y = Symbol('y')
sparse_eye3 = sparse_eye(3)
assert sparse_eye3.charpoly(x) == PurePoly(((x - 1)**3))
assert sparse_eye3.charpoly(y) == PurePoly(((y - 1)**3))
# test values
M = Matrix([( 0, 1, -1),
( 1, 1, 0),
(-1, 0, 1)])
vals = M.eigenvals()
assert sorted(vals.keys()) == [-1, 1, 2]
R = Rational
M = Matrix([[1, 0, 0],
[0, 1, 0],
[0, 0, 1]])
assert M.eigenvects() == [(1, 3, [
Matrix([1, 0, 0]),
Matrix([0, 1, 0]),
Matrix([0, 0, 1])])]
M = Matrix([[5, 0, 2],
[3, 2, 0],
[0, 0, 1]])
assert M.eigenvects() == [(1, 1, [Matrix([R(-1)/2, R(3)/2, 1])]),
(2, 1, [Matrix([0, 1, 0])]),
(5, 1, [Matrix([1, 1, 0])])]
assert M.zeros(3, 5) == SparseMatrix(3, 5, {})
A = SparseMatrix(10, 10, {(0, 0): 18, (0, 9): 12, (1, 4): 18, (2, 7): 16, (3, 9): 12, (4, 2): 19, (5, 7): 16, (6, 2): 12, (9, 7): 18})
assert A.row_list() == [(0, 0, 18), (0, 9, 12), (1, 4, 18), (2, 7, 16), (3, 9, 12), (4, 2, 19), (5, 7, 16), (6, 2, 12), (9, 7, 18)]
assert A.col_list() == [(0, 0, 18), (4, 2, 19), (6, 2, 12), (1, 4, 18), (2, 7, 16), (5, 7, 16), (9, 7, 18), (0, 9, 12), (3, 9, 12)]
assert SparseMatrix.eye(2).nnz() == 2
def intersection(self, o):
""" The intersection with other geometrical entity.
Parameters
==========
Point, Point3D, LinearEntity, LinearEntity3D, Plane
Returns
=======
List
Examples
========
>>> from sympy import Point, Point3D, Line, Line3D, Plane
>>> a = Plane(Point3D(1, 2, 3), normal_vector=(1, 1, 1))
>>> b = Point3D(1, 2, 3)
>>> a.intersection(b)
[Point3D(1, 2, 3)]
>>> c = Line3D(Point3D(1, 4, 7), Point3D(2, 2, 2))
>>> a.intersection(c)
[Point3D(2, 2, 2)]
>>> d = Plane(Point3D(6, 0, 0), normal_vector=(2, -5, 3))
>>> e = Plane(Point3D(2, 0, 0), normal_vector=(3, 4, -3))
>>> d.intersection(e)
[Line3D(Point3D(78/23, -24/23, 0), Point3D(147/23, 321/23, 23))]
"""
from sympy.geometry.line import LinearEntity, LinearEntity3D
if not isinstance(o, GeometryEntity):
o = Point(o, dim=3)
if isinstance(o, Point):
if o in self:
return [o]
else:
return []
if isinstance(o, (LinearEntity, LinearEntity3D)):
if o in self:
p1, p2 = o.p1, o.p2
if isinstance(o, Segment):
o = Segment3D(p1, p2)
elif isinstance(o, Ray):
o = Ray3D(p1, p2)
elif isinstance(o, Line):
o = Line3D(p1, p2)
else:
raise ValueError('unhandled linear entity: %s' % o.func)
return [o]
else:
x, y, z = map(Dummy, 'xyz')
t = Dummy() # unnamed else it may clash with a symbol in o
a = Point3D(o.arbitrary_point(t))
b = self.equation(x, y, z)
# TODO: Replace solve with solveset, when this line is tested
c = solve(b.subs(list(zip((x, y, z), a.args))), t)
if not c:
return []
else:
p = a.subs(t, c[0])
if p not in self:
return [] # e.g. a segment might not intersect a plane
return [p]
if isinstance(o, Plane):
if self.equals(o):
return [self]
if self.is_parallel(o):
return []
else:
x, y, z = map(Dummy, 'xyz')
a, b = Matrix([self.normal_vector]), Matrix([o.normal_vector])
c = list(a.cross(b))
d = self.equation(x, y, z)
e = o.equation(x, y, z)
result = list(linsolve([d, e], x, y, z))[0]
for i in (x, y, z): result = result.subs(i, 0)
return [Line3D(Point3D(result), direction_ratio=c)]
def intersection(self, o):
""" The intersection with other geometrical entity.
Parameters
==========
Point, Point3D, LinearEntity, LinearEntity3D, Plane
Returns
=======
List
Examples
========
>>> from sympy import Point, Point3D, Line, Line3D, Plane
>>> a = Plane(Point3D(1, 2, 3), normal_vector=(1, 1, 1))
>>> b = Point3D(1, 2, 3)
>>> a.intersection(b)
[Point3D(1, 2, 3)]
>>> c = Line3D(Point3D(1, 4, 7), Point3D(2, 2, 2))
>>> a.intersection(c)
[Point3D(2, 2, 2)]
>>> d = Plane(Point3D(6, 0, 0), normal_vector=(2, -5, 3))
>>> e = Plane(Point3D(2, 0, 0), normal_vector=(3, 4, -3))
>>> d.intersection(e)
[Line3D(Point3D(78/23, -24/23, 0), Point3D(147/23, 321/23, 23))]
"""
from sympy.geometry.line3d import LinearEntity3D
from sympy.geometry.line import LinearEntity
if isinstance(o, (Point, Point3D)):
if o in self:
return [Point3D(o)]
else:
return []
if isinstance(o, (LinearEntity, LinearEntity3D)):
if o in self:
p1, p2 = o.p1, o.p2
if isinstance(o, Segment):
o = Segment3D(p1, p2)
elif isinstance(o, Ray):
o = Ray3D(p1, p2)
elif isinstance(o, Line):
o = Line3D(p1, p2)
else:
raise ValueError('unhandled linear entity: %s' % o.func)
return [o]
else:
x, y, z = map(Dummy, 'xyz')
t = Dummy() # unnamed else it may clash with a symbol in o
a = Point3D(o.arbitrary_point(t))
b = self.equation(x, y, z)
c = solve(b.subs(list(zip((x, y, z), a.args))), t)
if not c:
return []
else:
p = a.subs(t, c[0])
if p not in self:
return [] # e.g. a segment might not intersect a plane
return [p]
if isinstance(o, Plane):
if o == self:
return [self]
if self.is_parallel(o):
return []
else:
x, y, z = map(Dummy, 'xyz')
a, b = Matrix([self.normal_vector]), Matrix([o.normal_vector])
c = list(a.cross(b))
d = self.equation(x, y, z)
e = o.equation(x, y, z)
f = solve((d.subs(z, 0), e.subs(z, 0)), [x, y])
if len(f) == 2:
return [Line3D(Point3D(f[x], f[y], 0), direction_ratio=c)]
g = solve((d.subs(y, 0), e.subs(y, 0)),[x, z])
if len(g) == 2:
return [Line3D(Point3D(g[x], 0, g[z]), direction_ratio=c)]
h = solve((d.subs(x, 0), e.subs(x, 0)),[y, z])
if len(h) == 2:
return [Line3D(Point3D(0, h[y], h[z]), direction_ratio=c)]
def generate_forward_kinematics(self):
# Get link parameters.
n_links = self.n
n_dofs = 2 * (self.n - 1)
link_radius = self.r
link_length = self.l
# Create transforms gₛₗ(0) from base frame to each link l.
g_sl0 = [None] * n_links
g_sl0[0] = eye_rat(4)
for i in range(1, n_links):
g_sl = eye_rat(4)
g_sl[0, 3] = Rational(i * (link_length + 2 * link_radius))
if i % 2 == 1:
g_sl[0:3, 0:3] = exp3(Matrix(3, 1, [1, 0, 0]), pi / 2)
g_sl0[i] = g_sl
if DEBUG:
print('g_sl0[i]')
pprint(g_sl0[i])
# Create revolute joint twists for each DOF, 2 per link.
joint_twists = []
w1 = Matrix(3, 1, [0, 0, 1])
w2 = Matrix(3, 1, [0, -1, 0])
for i in range(1, n_links):
q1 = Matrix(3, 1, [g_sl0[i - 1][0, 3] + link_length / 2, 0, 0])
x1 = zeros(6, 1)
x1[0:3, 0] = -w1.cross(q1)
x1[3:6, 0] = w1
for j in range(6):
x1[j, 0] = Rational(x1[j, 0])
x2 = zeros(6, 1)
q2 = Matrix(3, 1, [g_sl0[i][0, 3] - link_length / 2, 0, 0])
x2[0:3, 0] = -w2.cross(q2)
x2[3:6, 0] = w2
for j in range(6):
x2[j, 0] = Rational(x2[j, 0])
joint_twists.append([x1, x2])
if DEBUG:
print('x1')
pprint(x1)
print('x2')
pprint(x2)
# Create symbolic DOFs.
q = []
k = 0
for i in range(1, n_links):
q1 = symbols('q%d' % k)
k += 1
q2 = symbols('q%d' % k)
k += 1
q.append([q1, q2])
# Create transforms gₛₗ(θ) from base frame to each link l.
g_sl = [None] * n_links
g_sl[0] = eye_rat(4)
exps = [None] * n_links
exps[0] = eye_rat(4)
for i in range(1, n_links):
q1 = q[i - 1][0]
q2 = q[i - 1][1]
x1 = joint_twists[i - 1][0]
x2 = joint_twists[i - 1][1]
exp_x1 = exp6(x1, q1)
exp_x2 = exp6(x2, q2)
if DEBUG:
print('exp x1')
pprint(exp_x1)
print('exp x2')
pprint(exp_x2)
# exps[i] = exps[i-1] * exp_x1 * exp_x2
exps[i] = exps[i - 1] * trigsimp(exp_x1 * exp_x2)
# exps[i] = trigsimp(exps[i])
# g_sl[i] = trigsimp(exps[i] * g_sl0[i])
# g_sl[i] = exps[i] * g_sl0[i]
g_sl[i] = exps[i - 1] * trigsimp(exp_x1 * exp_x2 * g_sl0[i])
if DEBUG:
print('g_sl[%d]' % i)
pprint(g_sl[i])
# Assign transforms to each link.
dofs = []
for q1, q2 in q:
dofs.append(q1)
dofs.append(q2)
for i in range(n_links):
self.links[i].set_tf_expr(lambdify(dofs, g_sl[i]))
def intersection(self, o):
""" The intersection with other geometrical entity.
Parameters
==========
Point, Point3D, LinearEntity, LinearEntity3D, Plane
Returns
=======
List
Examples
========
>>> from sympy import Point, Point3D, Line, Line3D, Plane
>>> a = Plane(Point3D(1, 2, 3), normal_vector=[1, 1, 1])
>>> b = Point3D(1, 2, 3)
>>> a.intersection(b)
Point3D(1, 2, 3)
>>> c = Line3D(Point3D(1, 4, 7), Point3D(2, 2, 2))
>>> a.intersection(c)
Point3D(2, 2, 2)
>>> d = Plane(Point3D(6, 0, 0), normal_vector=[2, -5, 3])
>>> e = Plane(Point3D(2, 0, 0), normal_vector=[3, 4, -3])
>>> d.intersection(e)
[Line3D(Point3D(78/23, -24/23, 0), Point3D(147/23, 321/23, 23))]
"""
from sympy.geometry.line3d import LinearEntity3D
from sympy.geometry.line import LinearEntity
if isinstance(o, Point) or isinstance(o, Point3D):
if o in self:
return o
else:
return []
if isinstance(o, LinearEntity3D):
t = symbols('t')
x, y, z = symbols("x y z")
if o in self:
return [o]
else:
a = o.arbitrary_point(t)
b = self.equation(x, y, z)
c = solve(b.subs([(x, a.x), (y, a.y), (z, a.z)]))
if c == []:
return []
else:
a = a.subs(t, c[0])
if a in o:
return a
else:
return []
if isinstance(o, LinearEntity):
t = symbols('t')
x, y, z = symbols("x y z")
if o in self:
return [o]
else:
a = self.equation(x, y, z).subs(z, 0)
b = o.equation(x, y)
c = solve((a, b), [x, y])
if c is {}:
return []
else:
return [Point(c[x], c[y])]
if isinstance(o, Plane):
if o == self:
return self
if self.is_parallel(o):
return []
else:
x, y, z = symbols("x y z")
a, b = Matrix([self.normal_vector]), Matrix([o.normal_vector])
c = list(a.cross(b))
d = self.equation(x, y, z)
e = o.equation(x, y, z)
f = solve((d.subs(z, 0), e.subs(z, 0)), [x, y])
g = solve((d.subs(y, 0), e.subs(y, 0)), [x, z])
h = solve((d.subs(x, 0), e.subs(x, 0)), [y, z])
if len(f) == 2:
return [Line3D(Point3D(f[x], f[y], 0), direction_ratio=c)]
if len(g) == 2:
return [Line3D(Point3D(g[x], 0, g[z]), direction_ratio=c)]
if len(h) == 2:
return [Line3D(Point3D(0, h[y], h[z]), direction_ratio=c)]
L_ * (F_1 - F_2), L_ * (F_1 - F_3),
# b_ * (M_1 - M_2 + M_3 - M_4)
(M_1 - M_2 + M_3 - M_4)
])
omega = Matrix([phi_dot, theta_dot, psi_dot])
radius = L_ / 2.0
Ix = 2 * mass_ * (radius * radius) / 5.0 + 2 * radius * radius * mass_
Iy = 2 * mass_ * (radius * radius) / 5.0 + 2 * radius * radius * mass_
Iz = 2 * mass_ * (radius * radius) / 5.0 + 4 * radius * radius * mass_
# Eigen::Matrix3d I, Iinv;
I = Matrix([[Ix, 0, 0], [0, Iy, 0], [0, 0, Iz]])
Iinv = Matrix([[1 / Ix, 0, 0], [0, 1 / Iy, 0], [0, 0, 1 / Iz]])
omega_dot = Iinv * (tau - omega.cross(I * omega))
# Eigen::VectorXd state_dot(12);
state_dot = Matrix([
x_dot, y_dot, z_dot,
phi_dot, theta_dot, psi_dot,
pos_ddot[0], pos_ddot[1], pos_ddot[2],
omega_dot[0], omega_dot[1], omega_dot[2]
])
statevars = [
x_, y_, z_, phi, theta, psi, x_dot, y_dot, z_dot, phi_dot, theta_dot, psi_dot
]
fx = []
for var in statevars:
def intersection(self, o):
""" The intersection with other geometrical entity.
Parameters
==========
Point, Point3D, LinearEntity, LinearEntity3D, Plane
Returns
=======
List
Examples
========
>>> from sympy import Point, Point3D, Line, Line3D, Plane
>>> a = Plane(Point3D(1, 2, 3), normal_vector=(1, 1, 1))
>>> b = Point3D(1, 2, 3)
>>> a.intersection(b)
[Point3D(1, 2, 3)]
>>> c = Line3D(Point3D(1, 4, 7), Point3D(2, 2, 2))
>>> a.intersection(c)
[Point3D(2, 2, 2)]
>>> d = Plane(Point3D(6, 0, 0), normal_vector=(2, -5, 3))
>>> e = Plane(Point3D(2, 0, 0), normal_vector=(3, 4, -3))
>>> d.intersection(e)
[Line3D(Point3D(78/23, -24/23, 0), Point3D(147/23, 321/23, 23))]
"""
from sympy.geometry.line3d import LinearEntity3D
from sympy.geometry.line import LinearEntity
if isinstance(o, (Point, Point3D)):
if o in self:
return [Point3D(o)]
else:
return []
if isinstance(o, (LinearEntity, LinearEntity3D)):
if o in self:
p1, p2 = o.p1, o.p2
if isinstance(o, Segment):
o = Segment3D(p1, p2)
elif isinstance(o, Ray):
o = Ray3D(p1, p2)
elif isinstance(o, Line):
o = Line3D(p1, p2)
else:
raise ValueError('unhandled linear entity: %s' % o.func)
return [o]
else:
x, y, z = map(Dummy, 'xyz')
t = Dummy() # unnamed else it may clash with a symbol in o
a = Point3D(o.arbitrary_point(t))
b = self.equation(x, y, z)
c = solve(b.subs(list(zip((x, y, z), a.args))), t)
if not c:
return []
else:
p = a.subs(t, c[0])
if p not in self:
return [] # e.g. a segment might not intersect a plane
return [p]
if isinstance(o, Plane):
if o == self:
return [self]
if self.is_parallel(o):
return []
else:
x, y, z = map(Dummy, 'xyz')
a, b = Matrix([self.normal_vector]), Matrix([o.normal_vector])
c = list(a.cross(b))
d = self.equation(x, y, z)
e = o.equation(x, y, z)
f = solve((d.subs(z, 0), e.subs(z, 0)), [x, y])
if len(f) == 2:
return [Line3D(Point3D(f[x], f[y], 0), direction_ratio=c)]
g = solve((d.subs(y, 0), e.subs(y, 0)), [x, z])
if len(g) == 2:
return [Line3D(Point3D(g[x], 0, g[z]), direction_ratio=c)]
h = solve((d.subs(x, 0), e.subs(x, 0)), [y, z])
if len(h) == 2:
return [Line3D(Point3D(0, h[y], h[z]), direction_ratio=c)]
w = Matrix([0, 0, -65]) # Vector de velocidad angular
th = rad(30).evalf() # Ángulo en radianes
b = 300 # Base de la placa
h = 180 # Alto de la placa
# Para la velocidad del punto $Q$ sabemos que:
#
# $$ \vec{v}_Q = \vec{\omega} \times \vec{r}_{Q/O}$$
#
# Entonces:
# In[11]:
rQO = Matrix([b * cos(th), b * sin(th), 0]) # Vector r_Q/O
vQ = w.cross(rQO) # Velocidad del punto del punto Q
vQ
# De manera similar se puede proceder para las velocidades de los puntos $M$ y $P$.
# In[12]:
beta = atan(h / b)
tQ = th + beta # ángulo formado por el vector r_M/O con respecto a la horizontal
tP = th + pi / 2 # ángulo formado por el vector r_P/O con respecto a la horizontal
r = sqrt(h**2 + b**2) # diagonal del rectángulo
rMO = Matrix([r * cos(tQ), r * sin(tQ), 0])
rPO = Matrix([r * cos(tP), r * sin(tP), 0])
vM = w.cross(rMO) # velocidad de M
vP = w.cross(rPO) # velocidad de P
def intersection(self, o):
""" The intersection with other geometrical entity.
Parameters
==========
Point, Point3D, LinearEntity, LinearEntity3D, Plane
Returns
=======
List
Examples
========
>>> from sympy import Point, Point3D, Line, Line3D, Plane
>>> a = Plane(Point3D(1, 2, 3), normal_vector=[1, 1, 1])
>>> b = Point3D(1, 2, 3)
>>> a.intersection(b)
Point3D(1, 2, 3)
>>> c = Line3D(Point3D(1, 4, 7), Point3D(2, 2, 2))
>>> a.intersection(c)
Point3D(2, 2, 2)
>>> d = Plane(Point3D(6, 0, 0), normal_vector=[2, -5, 3])
>>> e = Plane(Point3D(2, 0, 0), normal_vector=[3, 4, -3])
>>> d.intersection(e)
[Line3D(Point3D(78/23, -24/23, 0), Point3D(147/23, 321/23, 23))]
"""
from sympy.geometry.line3d import LinearEntity3D
from sympy.geometry.line import LinearEntity
if isinstance(o, Point) or isinstance(o, Point3D):
if o in self:
return o
else:
return []
if isinstance(o, LinearEntity3D):
t = symbols('t')
x, y, z = symbols("x y z")
if o in self:
return [o]
else:
a = o.arbitrary_point(t)
b = self.equation(x, y, z)
c = solve(b.subs([(x, a.x), (y, a.y), (z, a.z)]))
if c == []:
return []
else:
a = a.subs(t, c[0])
if a in o:
return a
else:
return []
if isinstance(o, LinearEntity):
t = symbols('t')
x, y, z = symbols("x y z")
if o in self:
return [o]
else:
a = self.equation(x, y, z).subs(z,0)
b = o.equation(x, y)
c = solve((a,b),[x, y])
if c is {}:
return []
else:
return [Point(c[x], c[y])]
if isinstance(o, Plane):
if o == self:
return self
if self.is_parallel(o):
return []
else:
x, y, z = symbols("x y z")
a, b= Matrix([self.normal_vector]), Matrix([o.normal_vector])
c = list(a.cross(b))
d = self.equation(x, y, z)
e = o.equation(x, y, z)
f = solve((d.subs(z,0), e.subs(z,0)),[x, y])
g = solve((d.subs(y,0), e.subs(y,0)),[x, z])
h = solve((d.subs(x,0), e.subs(x,0)),[y, z])
if len(f) == 2:
return [Line3D(Point3D(f[x], f[y], 0), direction_ratio=c)]
if len(g) == 2:
return [Line3D(Point3D(g[x], 0, g[z]), direction_ratio=c)]
if len(h) == 2:
return [Line3D(Point3D(0, h[y], h[z]), direction_ratio=c)]
# Recuerde que si requiere ver las expresiones resultantes como fracciones decimales debe usar `evalf`.
#
# El producto escalar de dos vectores puede calcularlo utilizando el método `dot`, por
# ejemplo $ \vec{u} \cdot \vec{v} $ lo puede especificar como:
# In[51]:
u.dot(v)
# Para calcular el producto vectorial utilice el método `cross`, por ejemplo
# $ \vec{u} \times \vec{v} $:
# In[52]:
u.cross(v)
# Recuerde que el producto vectorial no es conmutativo, por tanto, $\vec{v} \times \vec{u} $ resultará en un vector
# diferente al obtenido anteriormente, como puede verificar enseguida:
# In[53]:
v.cross(u)
# ## Matrices
#
# Las matrices son arreglos rectangulares de números o cantidades simbólicas. En SymPy,
# se definen utilizando la clase `Matrix`, pasándole como argumentos una lista de
# listas, donde cada sublista corresponde a una fila de la matriz.
#
# Por ejemplo, vamos a definir las matrices $A$, $B$ y $C$, dadas por:
