pyphysica 0.1.0__py3-none-any.whl
This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
physica/__init__.py
ADDED
|
@@ -0,0 +1,1319 @@
|
|
|
1
|
+
import numpy as np
|
|
2
|
+
import sympy as sp
|
|
3
|
+
from sympy import *
|
|
4
|
+
from sympy import solve
|
|
5
|
+
import inspect
|
|
6
|
+
import random
|
|
7
|
+
import math
|
|
8
|
+
import copy
|
|
9
|
+
from functools import wraps
|
|
10
|
+
|
|
11
|
+
|
|
12
|
+
import numpy as np
|
|
13
|
+
import sympy as sp
|
|
14
|
+
from sympy import solve, sympify
|
|
15
|
+
import math
|
|
16
|
+
|
|
17
|
+
# Canonical free variable
|
|
18
|
+
x = sp.Symbol("x")
|
|
19
|
+
|
|
20
|
+
# Canonical time symbol (used by TimeVector and symbolic equations of motion)
|
|
21
|
+
T = sp.Symbol("T")
|
|
22
|
+
|
|
23
|
+
sp.Symbol.__rshift__ = lambda self, other: (self, other) if isinstance(other, (int, float)) else TypeError("Right-shift only supports numeric literals, not {}".format(type(other)))
|
|
24
|
+
|
|
25
|
+
def sign(integer):
|
|
26
|
+
return "" if integer < 0 else "+"
|
|
27
|
+
|
|
28
|
+
|
|
29
|
+
# ---------------------------------------------------------------------------
|
|
30
|
+
# Quaternion
|
|
31
|
+
# ---------------------------------------------------------------------------
|
|
32
|
+
|
|
33
|
+
class Quaternion:
|
|
34
|
+
def __init__(self, w, x, y, z):
|
|
35
|
+
self.w = sp.sympify(w)
|
|
36
|
+
self.x = sp.sympify(x)
|
|
37
|
+
self.y = sp.sympify(y)
|
|
38
|
+
self.z = sp.sympify(z)
|
|
39
|
+
|
|
40
|
+
def __repr__(self):
|
|
41
|
+
# Use evalf so symbolic components render as floats when possible,
|
|
42
|
+
# but fall back to the symbolic form cleanly.
|
|
43
|
+
def fmt(v):
|
|
44
|
+
try:
|
|
45
|
+
return f"{float(v):.4g}"
|
|
46
|
+
except (TypeError, ValueError):
|
|
47
|
+
return str(v)
|
|
48
|
+
return f"Quaternion({fmt(self.w)}, {fmt(self.x)}, {fmt(self.y)}, {fmt(self.z)})"
|
|
49
|
+
|
|
50
|
+
def norm(self):
|
|
51
|
+
return sp.sqrt(self.w**2 + self.x**2 + self.y**2 + self.z**2)
|
|
52
|
+
|
|
53
|
+
def normalize(self):
|
|
54
|
+
n = self.norm()
|
|
55
|
+
if n == 0:
|
|
56
|
+
raise ZeroDivisionError("Cannot normalize a zero quaternion.")
|
|
57
|
+
self.w /= n
|
|
58
|
+
self.x /= n
|
|
59
|
+
self.y /= n
|
|
60
|
+
self.z /= n
|
|
61
|
+
return self
|
|
62
|
+
|
|
63
|
+
def conjugate(self):
|
|
64
|
+
return Quaternion(self.w, -self.x, -self.y, -self.z)
|
|
65
|
+
|
|
66
|
+
def inverse(self):
|
|
67
|
+
n2 = self.norm() ** 2
|
|
68
|
+
if n2 == 0:
|
|
69
|
+
raise ZeroDivisionError("Cannot invert a zero quaternion.")
|
|
70
|
+
return self.conjugate() * (sp.Integer(1) / n2)
|
|
71
|
+
|
|
72
|
+
def __mul__(self, other):
|
|
73
|
+
if isinstance(other, Quaternion):
|
|
74
|
+
w = self.w*other.w - self.x*other.x - self.y*other.y - self.z*other.z
|
|
75
|
+
x_ = self.w*other.x + self.x*other.w + self.y*other.z - self.z*other.y
|
|
76
|
+
y_ = self.w*other.y - self.x*other.z + self.y*other.w + self.z*other.x
|
|
77
|
+
z_ = self.w*other.z + self.x*other.y - self.y*other.x + self.z*other.w
|
|
78
|
+
return Quaternion(w, x_, y_, z_)
|
|
79
|
+
return Quaternion(self.w*other, self.x*other, self.y*other, self.z*other)
|
|
80
|
+
|
|
81
|
+
def rotate_vector(self, vector):
|
|
82
|
+
qv = Quaternion(0, vector.x, vector.y, vector.z)
|
|
83
|
+
qr = self * qv * self.inverse()
|
|
84
|
+
return Vector3(qr.x, qr.y, qr.z)
|
|
85
|
+
|
|
86
|
+
|
|
87
|
+
# ---------------------------------------------------------------------------
|
|
88
|
+
# Vector3
|
|
89
|
+
# ---------------------------------------------------------------------------
|
|
90
|
+
|
|
91
|
+
class Vector3:
|
|
92
|
+
__slots__ = ("x", "y", "z")
|
|
93
|
+
|
|
94
|
+
def __init__(self, x=0.0, y=0.0, z=0.0):
|
|
95
|
+
self.x = sp.sympify(x)
|
|
96
|
+
self.y = sp.sympify(y)
|
|
97
|
+
self.z = sp.sympify(z)
|
|
98
|
+
|
|
99
|
+
def _fmt(self, v):
|
|
100
|
+
try:
|
|
101
|
+
return f"{float(v):.6g}"
|
|
102
|
+
except (TypeError, ValueError):
|
|
103
|
+
return str(v)
|
|
104
|
+
|
|
105
|
+
def __repr__(self):
|
|
106
|
+
return f"Vector3({self._fmt(self.x)}, {self._fmt(self.y)}, {self._fmt(self.z)})"
|
|
107
|
+
|
|
108
|
+
def __str__(self):
|
|
109
|
+
return f"<{self.x}, {self.y}, {self.z}>"
|
|
110
|
+
|
|
111
|
+
@property
|
|
112
|
+
def components(self):
|
|
113
|
+
return f"{self.x}i + {self.y}j + {self.z}k"
|
|
114
|
+
|
|
115
|
+
def copy(self):
|
|
116
|
+
return Vector3(self.x, self.y, self.z)
|
|
117
|
+
|
|
118
|
+
def as_tuple(self):
|
|
119
|
+
return (self.x, self.y, self.z)
|
|
120
|
+
|
|
121
|
+
def as_array(self):
|
|
122
|
+
return np.array([float(self.x), float(self.y), float(self.z)])
|
|
123
|
+
|
|
124
|
+
def norm(self):
|
|
125
|
+
return sp.sqrt(self.x*self.x + self.y*self.y + self.z*self.z)
|
|
126
|
+
|
|
127
|
+
def norm_sq(self):
|
|
128
|
+
return self.x*self.x + self.y*self.y + self.z*self.z
|
|
129
|
+
|
|
130
|
+
def __abs__(self):
|
|
131
|
+
return self.norm()
|
|
132
|
+
|
|
133
|
+
def normalize(self):
|
|
134
|
+
n = self.norm()
|
|
135
|
+
if n == 0:
|
|
136
|
+
raise ZeroDivisionError("Cannot normalize zero vector")
|
|
137
|
+
self.x /= n
|
|
138
|
+
self.y /= n
|
|
139
|
+
self.z /= n
|
|
140
|
+
return self
|
|
141
|
+
|
|
142
|
+
def normalized(self):
|
|
143
|
+
n = self.norm()
|
|
144
|
+
if n == 0:
|
|
145
|
+
raise ZeroDivisionError("Cannot normalize zero vector")
|
|
146
|
+
return Vector3(self.x/n, self.y/n, self.z/n)
|
|
147
|
+
|
|
148
|
+
def __add__(self, other):
|
|
149
|
+
return Vector3(self.x+other.x, self.y+other.y, self.z+other.z)
|
|
150
|
+
|
|
151
|
+
def __sub__(self, other):
|
|
152
|
+
return Vector3(self.x-other.x, self.y-other.y, self.z-other.z)
|
|
153
|
+
|
|
154
|
+
def __mul__(self, scalar):
|
|
155
|
+
return Vector3(self.x*scalar, self.y*scalar, self.z*scalar)
|
|
156
|
+
|
|
157
|
+
def __rmul__(self, scalar):
|
|
158
|
+
return self * scalar
|
|
159
|
+
|
|
160
|
+
def __truediv__(self, scalar):
|
|
161
|
+
return Vector3(self.x/scalar, self.y/scalar, self.z/scalar)
|
|
162
|
+
|
|
163
|
+
def __neg__(self):
|
|
164
|
+
return Vector3(-self.x, -self.y, -self.z)
|
|
165
|
+
|
|
166
|
+
def dot(self, other):
|
|
167
|
+
return self.x*other.x + self.y*other.y + self.z*other.z
|
|
168
|
+
|
|
169
|
+
def cross(self, other):
|
|
170
|
+
return Vector3(
|
|
171
|
+
self.y*other.z - self.z*other.y,
|
|
172
|
+
self.z*other.x - self.x*other.z,
|
|
173
|
+
self.x*other.y - self.y*other.x
|
|
174
|
+
)
|
|
175
|
+
|
|
176
|
+
def distance_to(self, other):
|
|
177
|
+
return (self - other).norm()
|
|
178
|
+
|
|
179
|
+
def project(self, other):
|
|
180
|
+
d = other.norm_sq()
|
|
181
|
+
if d == 0:
|
|
182
|
+
raise ZeroDivisionError("Projection onto zero vector")
|
|
183
|
+
return other * (self.dot(other) / d)
|
|
184
|
+
|
|
185
|
+
def reject(self, other):
|
|
186
|
+
return self - self.project(other)
|
|
187
|
+
|
|
188
|
+
def angle_to(self, other):
|
|
189
|
+
m = float(self.norm()) * float(other.norm())
|
|
190
|
+
if m == 0:
|
|
191
|
+
raise ZeroDivisionError("Angle undefined for zero vector")
|
|
192
|
+
return math.acos(max(-1.0, min(1.0, float(self.dot(other)) / m)))
|
|
193
|
+
|
|
194
|
+
def __iter__(self):
|
|
195
|
+
yield self.x
|
|
196
|
+
yield self.y
|
|
197
|
+
yield self.z
|
|
198
|
+
|
|
199
|
+
def __getitem__(self, i):
|
|
200
|
+
if i == 0: return self.x
|
|
201
|
+
if i == 1: return self.y
|
|
202
|
+
if i == 2: return self.z
|
|
203
|
+
raise IndexError
|
|
204
|
+
|
|
205
|
+
def __eq__(self, other):
|
|
206
|
+
return isinstance(other, Vector3) and \
|
|
207
|
+
self.x == other.x and self.y == other.y and self.z == other.z
|
|
208
|
+
|
|
209
|
+
def rotated(self, quaternion):
|
|
210
|
+
return quaternion.rotate_vector(self)
|
|
211
|
+
|
|
212
|
+
|
|
213
|
+
# ---------------------------------------------------------------------------
|
|
214
|
+
# Vector2
|
|
215
|
+
# ---------------------------------------------------------------------------
|
|
216
|
+
|
|
217
|
+
class Vector2:
|
|
218
|
+
__slots__ = ("x", "y", "type")
|
|
219
|
+
|
|
220
|
+
def __init__(self, x=0.0, y=0.0):
|
|
221
|
+
self.x = sp.sympify(x)
|
|
222
|
+
self.y = sp.sympify(y)
|
|
223
|
+
self.type = ""
|
|
224
|
+
|
|
225
|
+
def _fmt(self, v):
|
|
226
|
+
try:
|
|
227
|
+
return f"{float(v):.6g}"
|
|
228
|
+
except (TypeError, ValueError):
|
|
229
|
+
return str(v)
|
|
230
|
+
|
|
231
|
+
def __repr__(self):
|
|
232
|
+
return f"Vector2({self._fmt(self.x)}, {self._fmt(self.y)})"
|
|
233
|
+
|
|
234
|
+
def __str__(self):
|
|
235
|
+
return f"<{self.x}, {self.y}>"
|
|
236
|
+
|
|
237
|
+
def copy(self):
|
|
238
|
+
return Vector2(self.x, self.y)
|
|
239
|
+
|
|
240
|
+
def as_tuple(self):
|
|
241
|
+
return (self.x, self.y)
|
|
242
|
+
|
|
243
|
+
def as_array(self):
|
|
244
|
+
return np.array([float(self.x), float(self.y)])
|
|
245
|
+
|
|
246
|
+
def norm(self):
|
|
247
|
+
return sp.sqrt(self.x*self.x + self.y*self.y)
|
|
248
|
+
|
|
249
|
+
def __abs__(self):
|
|
250
|
+
return self.norm()
|
|
251
|
+
|
|
252
|
+
def norm_sq(self):
|
|
253
|
+
return self.x*self.x + self.y*self.y
|
|
254
|
+
|
|
255
|
+
def normalize(self):
|
|
256
|
+
n = self.norm()
|
|
257
|
+
if n == 0:
|
|
258
|
+
raise ZeroDivisionError("Cannot normalize zero vector")
|
|
259
|
+
self.x /= n
|
|
260
|
+
self.y /= n
|
|
261
|
+
return self
|
|
262
|
+
|
|
263
|
+
def normalized(self):
|
|
264
|
+
n = self.norm()
|
|
265
|
+
if n == 0:
|
|
266
|
+
raise ZeroDivisionError("Cannot normalize zero vector")
|
|
267
|
+
return Vector2(self.x/n, self.y/n)
|
|
268
|
+
|
|
269
|
+
def __add__(self, other):
|
|
270
|
+
return Vector2(self.x+other.x, self.y+other.y)
|
|
271
|
+
|
|
272
|
+
def __radd__(self, other):
|
|
273
|
+
if other == 0:
|
|
274
|
+
return self
|
|
275
|
+
else:
|
|
276
|
+
if isinstance(other, Vector2):
|
|
277
|
+
return other.__add__(self)
|
|
278
|
+
else:
|
|
279
|
+
raise TypeError(f"Unsupported operand for types {type(other)} and Vector2.")
|
|
280
|
+
|
|
281
|
+
def __sub__(self, other):
|
|
282
|
+
return Vector2(self.x-other.x, self.y-other.y)
|
|
283
|
+
|
|
284
|
+
def __mul__(self, scalar):
|
|
285
|
+
return Vector2(self.x*scalar, self.y*scalar)
|
|
286
|
+
|
|
287
|
+
def __rmul__(self, scalar):
|
|
288
|
+
return self * scalar
|
|
289
|
+
|
|
290
|
+
def __truediv__(self, scalar):
|
|
291
|
+
return Vector2(self.x/scalar, self.y/scalar)
|
|
292
|
+
|
|
293
|
+
def __neg__(self):
|
|
294
|
+
return Vector2(-self.x, -self.y)
|
|
295
|
+
|
|
296
|
+
def dot(self, other):
|
|
297
|
+
return self.x*other.x + self.y*other.y
|
|
298
|
+
|
|
299
|
+
def cross(self, other):
|
|
300
|
+
return self.x*other.y - self.y*other.x
|
|
301
|
+
|
|
302
|
+
def angle(self):
|
|
303
|
+
return sp.atan2(self.y, self.x)
|
|
304
|
+
|
|
305
|
+
def angle_to(self, other):
|
|
306
|
+
if not isinstance(other, Vector2):
|
|
307
|
+
raise TypeError(f"angle_to requires Vector2, not {type(other)}")
|
|
308
|
+
m = self.norm() * other.norm()
|
|
309
|
+
if m == 0:
|
|
310
|
+
raise ZeroDivisionError("Angle undefined for zero vector")
|
|
311
|
+
return sp.acos(sp.Rational(max(-1, min(1, self.dot(other)/m))))
|
|
312
|
+
|
|
313
|
+
def distance_to(self, other):
|
|
314
|
+
return (self - other).norm()
|
|
315
|
+
|
|
316
|
+
def project(self, other):
|
|
317
|
+
d = other.norm_sq()
|
|
318
|
+
if d == 0:
|
|
319
|
+
raise ZeroDivisionError("Projection onto zero vector")
|
|
320
|
+
return other * (self.dot(other) / d)
|
|
321
|
+
|
|
322
|
+
def reject(self, other):
|
|
323
|
+
return self - self.project(other)
|
|
324
|
+
|
|
325
|
+
def rotate(self, theta):
|
|
326
|
+
c = sp.cos(theta)
|
|
327
|
+
s = sp.sin(theta)
|
|
328
|
+
return Vector2(self.x*c - self.y*s, self.x*s + self.y*c)
|
|
329
|
+
|
|
330
|
+
def perpendicular(self):
|
|
331
|
+
return Vector2(-self.y, self.x)
|
|
332
|
+
|
|
333
|
+
def lerp(self, other, t):
|
|
334
|
+
return Vector2(self.x + (other.x-self.x)*t, self.y + (other.y-self.y)*t)
|
|
335
|
+
|
|
336
|
+
@staticmethod
|
|
337
|
+
def from_polar(r, theta):
|
|
338
|
+
return Vector2(r*sp.cos(theta), r*sp.sin(theta))
|
|
339
|
+
|
|
340
|
+
def __iter__(self):
|
|
341
|
+
yield self.x
|
|
342
|
+
yield self.y
|
|
343
|
+
|
|
344
|
+
def __getitem__(self, i):
|
|
345
|
+
if i == 0: return self.x
|
|
346
|
+
if i == 1: return self.y
|
|
347
|
+
raise IndexError
|
|
348
|
+
|
|
349
|
+
def __eq__(self, other):
|
|
350
|
+
return isinstance(other, Vector2) and self.x == other.x and self.y == other.y
|
|
351
|
+
|
|
352
|
+
|
|
353
|
+
# ---------------------------------------------------------------------------
|
|
354
|
+
# Matrix
|
|
355
|
+
# ---------------------------------------------------------------------------
|
|
356
|
+
|
|
357
|
+
class Matrix:
|
|
358
|
+
def __init__(self, *rows):
|
|
359
|
+
# Accept either Matrix(*rows) or Matrix(list_of_rows)
|
|
360
|
+
if len(rows) == 1 and isinstance(rows[0], list) and rows[0] and isinstance(rows[0][0], list):
|
|
361
|
+
rows = rows[0]
|
|
362
|
+
|
|
363
|
+
if not rows:
|
|
364
|
+
raise ValueError("Matrix cannot be empty")
|
|
365
|
+
if not all(len(row) == len(rows[0]) for row in rows):
|
|
366
|
+
raise ValueError("Rows must have equal length")
|
|
367
|
+
|
|
368
|
+
self.data = [list(row) for row in rows]
|
|
369
|
+
self.rows = len(self.data)
|
|
370
|
+
self.cols = len(self.data[0])
|
|
371
|
+
|
|
372
|
+
def __str__(self):
|
|
373
|
+
return "\n".join(" ".join(str(v) for v in row) for row in self.data)
|
|
374
|
+
|
|
375
|
+
def __repr__(self):
|
|
376
|
+
return f"Matrix({self.data})"
|
|
377
|
+
|
|
378
|
+
def shape(self):
|
|
379
|
+
return (self.rows, self.cols)
|
|
380
|
+
|
|
381
|
+
def copy(self):
|
|
382
|
+
return Matrix([row[:] for row in self.data])
|
|
383
|
+
|
|
384
|
+
def __getitem__(self, idx):
|
|
385
|
+
return self.data[idx]
|
|
386
|
+
|
|
387
|
+
def __eq__(self, other):
|
|
388
|
+
return isinstance(other, Matrix) and self.data == other.data
|
|
389
|
+
|
|
390
|
+
def __add__(self, other):
|
|
391
|
+
if self.shape() != other.shape():
|
|
392
|
+
raise ValueError("Matrix dimensions must match for addition")
|
|
393
|
+
return Matrix([
|
|
394
|
+
[self.data[i][j] + other.data[i][j] for j in range(self.cols)]
|
|
395
|
+
for i in range(self.rows)
|
|
396
|
+
])
|
|
397
|
+
|
|
398
|
+
def __sub__(self, other):
|
|
399
|
+
if self.shape() != other.shape():
|
|
400
|
+
raise ValueError("Matrix dimensions must match for subtraction")
|
|
401
|
+
return Matrix([
|
|
402
|
+
[self.data[i][j] - other.data[i][j] for j in range(self.cols)]
|
|
403
|
+
for i in range(self.rows)
|
|
404
|
+
])
|
|
405
|
+
|
|
406
|
+
def __mul__(self, scalar):
|
|
407
|
+
return Matrix([[scalar * v for v in row] for row in self.data])
|
|
408
|
+
|
|
409
|
+
__rmul__ = __mul__
|
|
410
|
+
|
|
411
|
+
def __matmul__(self, other):
|
|
412
|
+
if isinstance(other, Matrix):
|
|
413
|
+
if self.cols != other.rows:
|
|
414
|
+
raise ValueError(f"Cannot multiply ({self.rows}x{self.cols}) @ ({other.rows}x{other.cols})")
|
|
415
|
+
return Matrix([
|
|
416
|
+
[sum(self.data[i][k] * other.data[k][j] for k in range(self.cols))
|
|
417
|
+
for j in range(other.cols)]
|
|
418
|
+
for i in range(self.rows)
|
|
419
|
+
])
|
|
420
|
+
|
|
421
|
+
def __pow__(self, n):
|
|
422
|
+
if self.rows != self.cols:
|
|
423
|
+
raise ValueError("Matrix must be square for exponentiation")
|
|
424
|
+
result = Matrix.identity(self.rows)
|
|
425
|
+
base = self.copy()
|
|
426
|
+
n = int(n)
|
|
427
|
+
while n > 0:
|
|
428
|
+
if n % 2 == 1:
|
|
429
|
+
result = result @ base
|
|
430
|
+
base = base @ base
|
|
431
|
+
n //= 2
|
|
432
|
+
return result
|
|
433
|
+
|
|
434
|
+
@property
|
|
435
|
+
def T(self):
|
|
436
|
+
return Matrix([
|
|
437
|
+
[self.data[j][i] for j in range(self.rows)]
|
|
438
|
+
for i in range(self.cols)
|
|
439
|
+
])
|
|
440
|
+
|
|
441
|
+
def trace(self):
|
|
442
|
+
if self.rows != self.cols:
|
|
443
|
+
raise ValueError("Trace requires a square matrix")
|
|
444
|
+
return sum(self.data[i][i] for i in range(self.rows))
|
|
445
|
+
|
|
446
|
+
def det(self):
|
|
447
|
+
if self.rows != self.cols:
|
|
448
|
+
raise ValueError("Determinant requires a square matrix")
|
|
449
|
+
if self.rows == 1:
|
|
450
|
+
return self.data[0][0]
|
|
451
|
+
if self.rows == 2:
|
|
452
|
+
a, b = self.data[0]
|
|
453
|
+
c, d = self.data[1]
|
|
454
|
+
return a*d - b*c
|
|
455
|
+
total = 0
|
|
456
|
+
for col in range(self.cols):
|
|
457
|
+
sub = [row[:col] + row[col+1:] for row in self.data[1:]]
|
|
458
|
+
total += ((-1)**col) * self.data[0][col] * Matrix(sub).det()
|
|
459
|
+
return total
|
|
460
|
+
|
|
461
|
+
def minor(self, r, c):
|
|
462
|
+
sub = [row[:c] + row[c+1:] for i, row in enumerate(self.data) if i != r]
|
|
463
|
+
return Matrix(sub).det()
|
|
464
|
+
|
|
465
|
+
def cofactors(self):
|
|
466
|
+
return Matrix([
|
|
467
|
+
[((-1)**(i+j)) * self.minor(i, j) for j in range(self.cols)]
|
|
468
|
+
for i in range(self.rows)
|
|
469
|
+
])
|
|
470
|
+
|
|
471
|
+
def adj(self):
|
|
472
|
+
return self.cofactors().T
|
|
473
|
+
|
|
474
|
+
def inv(self):
|
|
475
|
+
d = self.det()
|
|
476
|
+
if d == 0:
|
|
477
|
+
raise ValueError("Matrix is singular (not invertible)")
|
|
478
|
+
return (1/d) * self.adj()
|
|
479
|
+
|
|
480
|
+
def rank(self):
|
|
481
|
+
m = [row[:] for row in self.data]
|
|
482
|
+
rank = 0
|
|
483
|
+
for c in range(self.cols):
|
|
484
|
+
pivot = next((r for r in range(rank, self.rows) if m[r][c] != 0), None)
|
|
485
|
+
if pivot is None:
|
|
486
|
+
continue
|
|
487
|
+
m[rank], m[pivot] = m[pivot], m[rank]
|
|
488
|
+
pv = m[rank][c]
|
|
489
|
+
m[rank] = [v / pv for v in m[rank]]
|
|
490
|
+
for r in range(self.rows):
|
|
491
|
+
if r != rank:
|
|
492
|
+
f = m[r][c]
|
|
493
|
+
m[r] = [iv - f*rv for rv, iv in zip(m[rank], m[r])]
|
|
494
|
+
rank += 1
|
|
495
|
+
return rank
|
|
496
|
+
|
|
497
|
+
def rref(self):
|
|
498
|
+
m = [row[:] for row in self.data]
|
|
499
|
+
lead = 0
|
|
500
|
+
for r in range(self.rows):
|
|
501
|
+
if lead >= self.cols:
|
|
502
|
+
break
|
|
503
|
+
i = r
|
|
504
|
+
while m[i][lead] == 0:
|
|
505
|
+
i += 1
|
|
506
|
+
if i == self.rows:
|
|
507
|
+
i = r
|
|
508
|
+
lead += 1
|
|
509
|
+
if lead == self.cols:
|
|
510
|
+
return Matrix(m)
|
|
511
|
+
m[i], m[r] = m[r], m[i]
|
|
512
|
+
lv = m[r][lead]
|
|
513
|
+
m[r] = [val/lv for val in m[r]]
|
|
514
|
+
for i in range(self.rows):
|
|
515
|
+
if i != r:
|
|
516
|
+
lv = m[i][lead]
|
|
517
|
+
m[i] = [iv - lv*rv for rv, iv in zip(m[r], m[i])]
|
|
518
|
+
lead += 1
|
|
519
|
+
return Matrix(m)
|
|
520
|
+
|
|
521
|
+
def solve(self, b):
|
|
522
|
+
if isinstance(b, Matrix):
|
|
523
|
+
b_data = [row[0] if len(row) == 1 else row for row in b.data]
|
|
524
|
+
else:
|
|
525
|
+
b_data = b
|
|
526
|
+
aug = [self.data[i] + [b_data[i]] for i in range(self.rows)]
|
|
527
|
+
rref = Matrix(aug).rref().data
|
|
528
|
+
return [row[-1] for row in rref]
|
|
529
|
+
|
|
530
|
+
@staticmethod
|
|
531
|
+
def identity(n):
|
|
532
|
+
return Matrix([[1 if i == j else 0 for j in range(n)] for i in range(n)])
|
|
533
|
+
|
|
534
|
+
@staticmethod
|
|
535
|
+
def zeros(r, c):
|
|
536
|
+
return Matrix([[0]*c for _ in range(r)])
|
|
537
|
+
|
|
538
|
+
|
|
539
|
+
# ---------------------------------------------------------------------------
|
|
540
|
+
# Lim — fixed signature: Lim(expr, var, point)
|
|
541
|
+
# ---------------------------------------------------------------------------
|
|
542
|
+
|
|
543
|
+
class Lim:
|
|
544
|
+
"""
|
|
545
|
+
Compute symbolic limits.
|
|
546
|
+
|
|
547
|
+
Usage:
|
|
548
|
+
Lim(sin(x)/x, x, 0) -> 1
|
|
549
|
+
Lim(Func(sin(x)/x), x, 0) -> 1
|
|
550
|
+
"""
|
|
551
|
+
def __init__(self, expr, var=None, point=None):
|
|
552
|
+
# Resolve Func wrappers
|
|
553
|
+
if isinstance(expr, Func):
|
|
554
|
+
expr = expr.expr
|
|
555
|
+
|
|
556
|
+
self.expr = expr
|
|
557
|
+
self.var = var if var is not None else x
|
|
558
|
+
self.point = point
|
|
559
|
+
|
|
560
|
+
def __rshift__(self, target):
|
|
561
|
+
"""Support Lim(expr, x) >> 0 syntax."""
|
|
562
|
+
return Lim(self.expr, self.var, target)
|
|
563
|
+
|
|
564
|
+
def evaluate(self):
|
|
565
|
+
if self.point is None:
|
|
566
|
+
raise ValueError("Limit point not set.")
|
|
567
|
+
return sp.limit(self.expr, self.var, self.point)
|
|
568
|
+
|
|
569
|
+
def __call__(self, expr):
|
|
570
|
+
self.expr = expr
|
|
571
|
+
return self.evaluate()
|
|
572
|
+
|
|
573
|
+
def __eq__(self, other):
|
|
574
|
+
if self.point is None:
|
|
575
|
+
return NotImplemented
|
|
576
|
+
return sp.simplify(self.evaluate() - sp.sympify(other)) == 0
|
|
577
|
+
|
|
578
|
+
def __repr__(self):
|
|
579
|
+
if self.point is not None:
|
|
580
|
+
return str(self.evaluate())
|
|
581
|
+
return f"Lim({self.expr}, {self.var}, ?)"
|
|
582
|
+
|
|
583
|
+
def __str__(self):
|
|
584
|
+
return self.__repr__()
|
|
585
|
+
|
|
586
|
+
|
|
587
|
+
# ---------------------------------------------------------------------------
|
|
588
|
+
# Func
|
|
589
|
+
# ---------------------------------------------------------------------------
|
|
590
|
+
|
|
591
|
+
class Func:
|
|
592
|
+
"""
|
|
593
|
+
A symbolic function wrapper using SymPy.
|
|
594
|
+
|
|
595
|
+
f = Func(x**2 - 1)
|
|
596
|
+
f(3) -> 8
|
|
597
|
+
f[1] -> first derivative as Func
|
|
598
|
+
f[-1] -> indefinite integral as Func
|
|
599
|
+
f[[0, 1]] -> definite integral from 0 to 1
|
|
600
|
+
f @ g -> composition f(g(x))
|
|
601
|
+
f.plot() -> plot (handled by backend)
|
|
602
|
+
f.derivative(n) -> nth derivative
|
|
603
|
+
f.integrate(n) -> nth antiderivative
|
|
604
|
+
f.series(n) -> Taylor series to order n
|
|
605
|
+
f.inverse() -> symbolic inverse (first branch)
|
|
606
|
+
f >> sym -> solve f(x) = 0 for sym
|
|
607
|
+
"""
|
|
608
|
+
def __init__(self, expression, var=None):
|
|
609
|
+
if callable(expression) and not isinstance(expression, sp.Basic):
|
|
610
|
+
raise ValueError("Use symbolic SymPy expressions only, not callables.")
|
|
611
|
+
self.expr = sp.sympify(expression)
|
|
612
|
+
self.var = var if var is not None else x
|
|
613
|
+
self._vars = sorted(self.expr.free_symbols, key=lambda s: s.name)
|
|
614
|
+
|
|
615
|
+
def _lambdify(self):
|
|
616
|
+
"""Build a numeric evaluator — prefers numpy for robustness."""
|
|
617
|
+
return sp.lambdify([self.var], self.expr, modules=["numpy", "sympy"])
|
|
618
|
+
|
|
619
|
+
def __call__(self, val):
|
|
620
|
+
return self._lambdify()(val)
|
|
621
|
+
|
|
622
|
+
def inverse(self):
|
|
623
|
+
y = sp.Symbol('_y')
|
|
624
|
+
sol = sp.solve(self.expr - y, self.var)
|
|
625
|
+
if not sol:
|
|
626
|
+
raise ValueError("Inverse not found or not unique.")
|
|
627
|
+
return Func(sol[0].subs(y, self.var), var=self.var)
|
|
628
|
+
|
|
629
|
+
def solve(self, sym=None):
|
|
630
|
+
sym = sym or self.var
|
|
631
|
+
return sp.solve(self.expr, sym)
|
|
632
|
+
|
|
633
|
+
def __rshift__(self, sym):
|
|
634
|
+
if isinstance(sym, sp.Symbol):
|
|
635
|
+
return sp.solve(self.expr, sym)
|
|
636
|
+
raise TypeError("Right-shift argument must be a sympy.Symbol")
|
|
637
|
+
|
|
638
|
+
def __add__(self, other):
|
|
639
|
+
return Func(self.expr + (other.expr if isinstance(other, Func) else other), var=self.var)
|
|
640
|
+
|
|
641
|
+
def __radd__(self, other):
|
|
642
|
+
return self + other
|
|
643
|
+
|
|
644
|
+
def __sub__(self, other):
|
|
645
|
+
return Func(self.expr - (other.expr if isinstance(other, Func) else other), var=self.var)
|
|
646
|
+
|
|
647
|
+
def __rsub__(self, other):
|
|
648
|
+
return Func(other - self.expr, var=self.var)
|
|
649
|
+
|
|
650
|
+
def __mul__(self, other):
|
|
651
|
+
return Func(self.expr * (other.expr if isinstance(other, Func) else other), var=self.var)
|
|
652
|
+
|
|
653
|
+
def __rmul__(self, other):
|
|
654
|
+
return self * other
|
|
655
|
+
|
|
656
|
+
def __truediv__(self, other):
|
|
657
|
+
return Func(self.expr / (other.expr if isinstance(other, Func) else other), var=self.var)
|
|
658
|
+
|
|
659
|
+
def __rtruediv__(self, other):
|
|
660
|
+
return Func(other / self.expr, var=self.var)
|
|
661
|
+
|
|
662
|
+
def __matmul__(self, other):
|
|
663
|
+
if not isinstance(other, Func):
|
|
664
|
+
raise TypeError(f"Cannot compose Func and {type(other)}")
|
|
665
|
+
return Func(self.expr.subs(self.var, other.expr), var=self.var)
|
|
666
|
+
|
|
667
|
+
def __pow__(self, n):
|
|
668
|
+
if not isinstance(n, int):
|
|
669
|
+
raise TypeError("Only integer powers are supported.")
|
|
670
|
+
if n == 0:
|
|
671
|
+
return Func(sp.S.One, var=self.var)
|
|
672
|
+
base = self if n > 0 else self.inverse()
|
|
673
|
+
result = base
|
|
674
|
+
for _ in range(abs(n) - 1):
|
|
675
|
+
result = result @ base
|
|
676
|
+
return result
|
|
677
|
+
|
|
678
|
+
def __eq__(self, other):
|
|
679
|
+
if isinstance(other, Func):
|
|
680
|
+
return sp.simplify(self.expr - other.expr) == 0
|
|
681
|
+
return NotImplemented
|
|
682
|
+
|
|
683
|
+
def __str__(self):
|
|
684
|
+
return str(self.expr)
|
|
685
|
+
|
|
686
|
+
def __repr__(self):
|
|
687
|
+
return str(self.expr)
|
|
688
|
+
|
|
689
|
+
def simplify(self):
|
|
690
|
+
return Func(sp.simplify(self.expr), var=self.var)
|
|
691
|
+
|
|
692
|
+
def derivative(self, n=1):
|
|
693
|
+
return Func(sp.diff(self.expr, self.var, n), var=self.var)
|
|
694
|
+
|
|
695
|
+
def integrate(self, n=1):
|
|
696
|
+
result = self
|
|
697
|
+
for _ in range(n):
|
|
698
|
+
result = Func(sp.integrate(result.expr, self.var), var=self.var)
|
|
699
|
+
return result
|
|
700
|
+
|
|
701
|
+
def partial(self, var, n=1):
|
|
702
|
+
if not isinstance(var, sp.Symbol):
|
|
703
|
+
raise TypeError("Variable must be a sympy.Symbol")
|
|
704
|
+
return Func(sp.diff(self.expr, var, n))
|
|
705
|
+
|
|
706
|
+
def gradient(self, variables=None):
|
|
707
|
+
variables = variables or self._vars
|
|
708
|
+
return [self.partial(v) for v in variables]
|
|
709
|
+
|
|
710
|
+
def hessian(self, *variables):
|
|
711
|
+
variables = variables or self._vars
|
|
712
|
+
n = len(variables)
|
|
713
|
+
return [[self.partial(variables[i]).partial(variables[j]) for j in range(n)] for i in range(n)]
|
|
714
|
+
|
|
715
|
+
def series(self, n=6):
|
|
716
|
+
return sp.series(self.expr, self.var, n=n)
|
|
717
|
+
|
|
718
|
+
def __getitem__(self, key):
|
|
719
|
+
if isinstance(key, int):
|
|
720
|
+
if key >= 0:
|
|
721
|
+
return Func(sp.diff(self.expr, self.var, key), var=self.var)
|
|
722
|
+
# Negative: nth antiderivative
|
|
723
|
+
result = self
|
|
724
|
+
for _ in range(abs(key)):
|
|
725
|
+
result = Func(sp.integrate(result.expr, self.var), var=self.var)
|
|
726
|
+
return result
|
|
727
|
+
if isinstance(key, (list, tuple)) and len(key) == 2:
|
|
728
|
+
lower, upper = key
|
|
729
|
+
return sp.integrate(self.expr, (self.var, lower, upper))
|
|
730
|
+
raise TypeError("Key must be an int (derivative order) or [lower, upper] (definite integral).")
|
|
731
|
+
|
|
732
|
+
|
|
733
|
+
class TimeVector:
|
|
734
|
+
"""
|
|
735
|
+
A Vector2 whose components may vary with time.
|
|
736
|
+
|
|
737
|
+
Two construction forms:
|
|
738
|
+
TimeVector(x, y) # symbolic components in T
|
|
739
|
+
TimeVector.of(lambda t: Vector2(...)) # callable, evaluated at time t
|
|
740
|
+
|
|
741
|
+
Examples:
|
|
742
|
+
TimeVector(0, 120.0 - 3 * T)
|
|
743
|
+
TimeVector.of(lambda t: Vector2(4 * sp.sin(t), 0))
|
|
744
|
+
|
|
745
|
+
Use .at(t) for a numeric Vector2 at time t, and .symbolic() for the
|
|
746
|
+
expression in T (used by the symbolic equations of motion).
|
|
747
|
+
"""
|
|
748
|
+
|
|
749
|
+
def __init__(self, x=0.0, y=0.0, fn=None):
|
|
750
|
+
self.fn = fn
|
|
751
|
+
self.type = ""
|
|
752
|
+
if fn is None:
|
|
753
|
+
self._x = sp.sympify(x)
|
|
754
|
+
self._y = sp.sympify(y)
|
|
755
|
+
else:
|
|
756
|
+
self._x = None
|
|
757
|
+
self._y = None
|
|
758
|
+
|
|
759
|
+
@classmethod
|
|
760
|
+
def of(cls, fn):
|
|
761
|
+
if not callable(fn):
|
|
762
|
+
raise TypeError("TimeVector.of expects a callable")
|
|
763
|
+
return cls(fn=fn)
|
|
764
|
+
|
|
765
|
+
@property
|
|
766
|
+
def x(self):
|
|
767
|
+
"""Symbolic x-component (expressions in T)."""
|
|
768
|
+
return self.symbolic().x
|
|
769
|
+
|
|
770
|
+
@property
|
|
771
|
+
def y(self):
|
|
772
|
+
"""Symbolic y-component (expressions in T)."""
|
|
773
|
+
return self.symbolic().y
|
|
774
|
+
|
|
775
|
+
@staticmethod
|
|
776
|
+
def _as_vector(value, where):
|
|
777
|
+
if isinstance(value, Vector2):
|
|
778
|
+
return value
|
|
779
|
+
if isinstance(value, (tuple, list)) and len(value) == 2:
|
|
780
|
+
return Vector2(value[0], value[1])
|
|
781
|
+
raise TypeError(f"TimeVector callable must return Vector2 (or a 2-tuple), got {type(value)} from {where}")
|
|
782
|
+
|
|
783
|
+
def symbolic(self):
|
|
784
|
+
"""Return a Vector2 of expressions in T."""
|
|
785
|
+
if self.fn is not None:
|
|
786
|
+
try:
|
|
787
|
+
return self._as_vector(self.fn(T), "T")
|
|
788
|
+
except Exception:
|
|
789
|
+
return self._as_vector(self.fn(0), "t=0")
|
|
790
|
+
return Vector2(self._x, self._y)
|
|
791
|
+
|
|
792
|
+
def at(self, t):
|
|
793
|
+
"""Return the numeric Vector2 at time t."""
|
|
794
|
+
if self.fn is not None:
|
|
795
|
+
return self._as_vector(self.fn(t), f"t={t}")
|
|
796
|
+
return Vector2(self._x.subs(T, t), self._y.subs(T, t))
|
|
797
|
+
|
|
798
|
+
def norm(self):
|
|
799
|
+
return self.symbolic().norm()
|
|
800
|
+
|
|
801
|
+
def copy(self):
|
|
802
|
+
if self.fn is not None:
|
|
803
|
+
return TimeVector(fn=self.fn)
|
|
804
|
+
return TimeVector(self._x, self._y)
|
|
805
|
+
|
|
806
|
+
def __repr__(self):
|
|
807
|
+
if self.fn is not None:
|
|
808
|
+
return f"TimeVector.of({self.fn})"
|
|
809
|
+
return f"TimeVector({self._x}, {self._y})"
|
|
810
|
+
|
|
811
|
+
def __str__(self):
|
|
812
|
+
return str(self.symbolic())
|
|
813
|
+
|
|
814
|
+
|
|
815
|
+
class Time:
|
|
816
|
+
def __init__(self):
|
|
817
|
+
self.t = 0
|
|
818
|
+
|
|
819
|
+
def up(self, val:int | float = 0):
|
|
820
|
+
self.t += val
|
|
821
|
+
|
|
822
|
+
def down(self, val:int | float = 0):
|
|
823
|
+
self.t -= val
|
|
824
|
+
|
|
825
|
+
def reset(self):
|
|
826
|
+
self.t = 0
|
|
827
|
+
|
|
828
|
+
class Object2:
|
|
829
|
+
# ------------------ Initialization & Representation ------------------
|
|
830
|
+
def __init__(self, name="Object", mass=1.0, position:Vector2=None, velocity:Vector2=None, acceleration:Vector2=None, radius=1.0):
|
|
831
|
+
self.name = name
|
|
832
|
+
self.mass = mass
|
|
833
|
+
self.position = position if position is not None else Vector2()
|
|
834
|
+
self.velocity = velocity if velocity is not None else Vector2()
|
|
835
|
+
self.acceleration = acceleration if acceleration is not None else Vector2()
|
|
836
|
+
self.radius = radius
|
|
837
|
+
self.angle = 0.0
|
|
838
|
+
self.angular_velocity = 0.0
|
|
839
|
+
self.torque = 0.0
|
|
840
|
+
self.moment = 0.5 * mass * radius ** 2
|
|
841
|
+
self.time = 0.0
|
|
842
|
+
self.forces: list = []
|
|
843
|
+
|
|
844
|
+
def __repr__(self):
|
|
845
|
+
return f"Object2(name={self.name}, mass={self.mass}, pos={self.position}, vel={self.velocity})"
|
|
846
|
+
|
|
847
|
+
def __str__(self):
|
|
848
|
+
return f"{self.name}: pos={self.position}, vel={self.velocity}, mass={self.mass}"
|
|
849
|
+
|
|
850
|
+
# ------------------ Force Management ------------------
|
|
851
|
+
def apply_force_continuous(self, force):
|
|
852
|
+
"""Add a continuous force. Accepts Vector2 or TimeVector."""
|
|
853
|
+
if not isinstance(force, (Vector2, TimeVector)):
|
|
854
|
+
raise TypeError(f"Force must be Vector2 or TimeVector, got {type(force)}")
|
|
855
|
+
force.type = "continuous"
|
|
856
|
+
if not any(force is existing for existing in self.forces):
|
|
857
|
+
self.forces.append(force)
|
|
858
|
+
self.sortforces()
|
|
859
|
+
return f"Applied continuous force of magnitude {self._vector_at(force, self.time).norm()}"
|
|
860
|
+
|
|
861
|
+
def apply_force_impulse(self, force):
|
|
862
|
+
"""Add an instantaneous impulse force. Accepts Vector2 or TimeVector."""
|
|
863
|
+
if not isinstance(force, (Vector2, TimeVector)):
|
|
864
|
+
raise TypeError(f"Force must be Vector2 or TimeVector, got {type(force)}")
|
|
865
|
+
force.type = "impulse"
|
|
866
|
+
if not any(force is existing for existing in self.forces):
|
|
867
|
+
self.forces.append(force)
|
|
868
|
+
self.sortforces()
|
|
869
|
+
return self.apply_impulse(self._vector_at(force, self.time))
|
|
870
|
+
|
|
871
|
+
@staticmethod
|
|
872
|
+
def _vector_at(force, t):
|
|
873
|
+
"""Resolve a force to a Vector2 at time t (evaluating TimeVector)."""
|
|
874
|
+
if isinstance(force, TimeVector):
|
|
875
|
+
return force.at(t)
|
|
876
|
+
return force
|
|
877
|
+
|
|
878
|
+
@property
|
|
879
|
+
def continuous_forces(self):
|
|
880
|
+
return [force for force in self.forces if force.type == "continuous"]
|
|
881
|
+
|
|
882
|
+
@property
|
|
883
|
+
def impulse_forces(self):
|
|
884
|
+
return [force for force in self.forces if force.type == "impulse"]
|
|
885
|
+
|
|
886
|
+
@property
|
|
887
|
+
def net_force(self):
|
|
888
|
+
"""Sum of all continuous forces, evaluated at the current time."""
|
|
889
|
+
out = Vector2()
|
|
890
|
+
for force in self.continuous_forces:
|
|
891
|
+
out = out + self._vector_at(force, self.time)
|
|
892
|
+
return out
|
|
893
|
+
|
|
894
|
+
def net_force_at(self, t):
|
|
895
|
+
"""Sum of continuous forces evaluated at an arbitrary time t."""
|
|
896
|
+
out = Vector2()
|
|
897
|
+
for force in self.continuous_forces:
|
|
898
|
+
out = out + self._vector_at(force, t)
|
|
899
|
+
return out
|
|
900
|
+
|
|
901
|
+
def clear_forces(self):
|
|
902
|
+
"""Clear all forces"""
|
|
903
|
+
self.forces.clear()
|
|
904
|
+
|
|
905
|
+
def clear_impulses(self):
|
|
906
|
+
"""Remove one-shot impulse forces without touching continuous ones"""
|
|
907
|
+
self.forces = [force for force in self.forces if force.type != "impulse"]
|
|
908
|
+
|
|
909
|
+
def sortforces(self) -> None:
|
|
910
|
+
"""Kept for backwards compatibility; force lists are now computed properties."""
|
|
911
|
+
return None
|
|
912
|
+
|
|
913
|
+
# ------------------ Motion / Physics ------------------
|
|
914
|
+
def update(self, dt):
|
|
915
|
+
return self.integrate(dt)
|
|
916
|
+
|
|
917
|
+
def integrate(self, dt):
|
|
918
|
+
self.acceleration = self.net_force / self.mass
|
|
919
|
+
self.position = self.position + self.velocity*dt + self.acceleration*(0.5*dt*dt)
|
|
920
|
+
self.velocity = self.velocity + self.acceleration*dt
|
|
921
|
+
self.angular_velocity = self.angular_velocity + (self.torque / self.moment)*dt
|
|
922
|
+
self.angle = self.angle + self.angular_velocity*dt
|
|
923
|
+
self.time = self.time + dt
|
|
924
|
+
return self
|
|
925
|
+
|
|
926
|
+
# ------------------ Symbolic (Func) Integration ------------------
|
|
927
|
+
def net_force_symbolic(self):
|
|
928
|
+
"""Return the net continuous force as a Vector2 of expressions in T."""
|
|
929
|
+
out = Vector2()
|
|
930
|
+
for force in self.continuous_forces:
|
|
931
|
+
if isinstance(force, TimeVector):
|
|
932
|
+
out = out + force.symbolic()
|
|
933
|
+
else:
|
|
934
|
+
out = out + force
|
|
935
|
+
return out
|
|
936
|
+
|
|
937
|
+
def equation_of_motion(self):
|
|
938
|
+
"""Return (ax(T), ay(T)) SymPy expressions from Newton's second law."""
|
|
939
|
+
net = self.net_force_symbolic()
|
|
940
|
+
return (sp.simplify(net.x / self.mass), sp.simplify(net.y / self.mass))
|
|
941
|
+
|
|
942
|
+
def acceleration_func(self):
|
|
943
|
+
"""Return (ax, ay) as T-bound Funcs."""
|
|
944
|
+
ax, ay = self.equation_of_motion()
|
|
945
|
+
return Func(ax, var=T), Func(ay, var=T)
|
|
946
|
+
|
|
947
|
+
def velocity_func(self):
|
|
948
|
+
"""Return (vx, vy) as T-bound Funcs, integrating acceleration from rest."""
|
|
949
|
+
ax, ay = self.acceleration_func()
|
|
950
|
+
return ax[-1], ay[-1]
|
|
951
|
+
|
|
952
|
+
def position_func(self):
|
|
953
|
+
"""Return (x, y) as T-bound Funcs, double-integrating acceleration from rest."""
|
|
954
|
+
ax, ay = self.acceleration_func()
|
|
955
|
+
return ax[-2], ay[-2]
|
|
956
|
+
|
|
957
|
+
def closed_form(self):
|
|
958
|
+
"""Solve the equations of motion symbolically with dsolve (zero ICs)."""
|
|
959
|
+
ax, ay = self.equation_of_motion()
|
|
960
|
+
solutions = []
|
|
961
|
+
for component in (ax, ay):
|
|
962
|
+
f = sp.Function("_q")(T)
|
|
963
|
+
|
|
964
|
+
# dsolve can recurse on Float RHS in some SymPy/Python builds; rationalize.
|
|
965
|
+
rhs = component
|
|
966
|
+
if rhs.has(sp.Float):
|
|
967
|
+
rhs = sp.nsimplify(rhs, rational=True)
|
|
968
|
+
try:
|
|
969
|
+
sol = sp.dsolve(sp.Eq(f.diff(T, 2), rhs), f)
|
|
970
|
+
solutions.append(sol.rhs)
|
|
971
|
+
except Exception:
|
|
972
|
+
solutions.append(sp.integrate(sp.integrate(rhs, T), T))
|
|
973
|
+
return tuple(solutions)
|
|
974
|
+
|
|
975
|
+
def describe(self):
|
|
976
|
+
"""Human-readable summary of the symbolic system."""
|
|
977
|
+
net = self.net_force_symbolic()
|
|
978
|
+
ax, ay = self.equation_of_motion()
|
|
979
|
+
lines = [
|
|
980
|
+
f"{self.name} (mass={self.mass})",
|
|
981
|
+
f" net force F(T) = {net}",
|
|
982
|
+
f" acceleration a(T) = ({ax}, {ay})",
|
|
983
|
+
]
|
|
984
|
+
return "\n".join(lines)
|
|
985
|
+
|
|
986
|
+
def apply_gravity(self, gravity):
|
|
987
|
+
self.apply_force_continuous(Vector2(0, gravity))
|
|
988
|
+
|
|
989
|
+
def apply_friction(self, normal_force, coefficient):
|
|
990
|
+
"""Apply kinetic friction opposite to velocity"""
|
|
991
|
+
speed = self.speed()
|
|
992
|
+
if speed == 0:
|
|
993
|
+
return Vector2()
|
|
994
|
+
magnitude = abs(coefficient * normal_force)
|
|
995
|
+
friction = self.velocity.normalized() * -magnitude
|
|
996
|
+
self.apply_force_continuous(friction)
|
|
997
|
+
return friction
|
|
998
|
+
|
|
999
|
+
def apply_drag(self, fluid_density, drag_coeff, area):
|
|
1000
|
+
"""Apply quadratic drag: F = -1/2 rho C A |v| v"""
|
|
1001
|
+
speed = self.speed()
|
|
1002
|
+
if speed == 0:
|
|
1003
|
+
return Vector2()
|
|
1004
|
+
drag = self.velocity.normalized() * (-0.5 * fluid_density * drag_coeff * area * speed ** 2)
|
|
1005
|
+
self.apply_force_continuous(drag)
|
|
1006
|
+
return drag
|
|
1007
|
+
|
|
1008
|
+
def move_to(self, position):
|
|
1009
|
+
"""Directly set position"""
|
|
1010
|
+
self.position = position
|
|
1011
|
+
|
|
1012
|
+
def set_velocity(self, velocity):
|
|
1013
|
+
"""Directly set velocity"""
|
|
1014
|
+
self.velocity = velocity
|
|
1015
|
+
|
|
1016
|
+
# ------------------ State / Info ------------------
|
|
1017
|
+
def state(self):
|
|
1018
|
+
"""Return dictionary with position, velocity, mass, radius, name"""
|
|
1019
|
+
return {
|
|
1020
|
+
"name": self.name,
|
|
1021
|
+
"mass": self.mass,
|
|
1022
|
+
"radius": self.radius,
|
|
1023
|
+
"position": self.position.copy(),
|
|
1024
|
+
"velocity": self.velocity.copy(),
|
|
1025
|
+
"acceleration": self.acceleration.copy(),
|
|
1026
|
+
"angle": self.angle,
|
|
1027
|
+
"angular_velocity": self.angular_velocity,
|
|
1028
|
+
}
|
|
1029
|
+
|
|
1030
|
+
def kinetic_energy(self):
|
|
1031
|
+
"""Return kinetic energy: 1/2 m v^2"""
|
|
1032
|
+
return 0.5 * self.mass * self.speed() ** 2
|
|
1033
|
+
|
|
1034
|
+
def momentum(self):
|
|
1035
|
+
"""Return momentum: m * v"""
|
|
1036
|
+
return self.velocity * self.mass
|
|
1037
|
+
|
|
1038
|
+
def speed(self):
|
|
1039
|
+
"""Return magnitude of velocity"""
|
|
1040
|
+
return self.velocity.norm()
|
|
1041
|
+
|
|
1042
|
+
def distance_to(self, other):
|
|
1043
|
+
"""Return Euclidean distance to another Object2"""
|
|
1044
|
+
return self.position.distance_to(other.position)
|
|
1045
|
+
|
|
1046
|
+
def direction_to(self, other):
|
|
1047
|
+
"""Return normalized Vector2D pointing to another Object2"""
|
|
1048
|
+
return (other.position - self.position).normalized()
|
|
1049
|
+
|
|
1050
|
+
# ------------------ Collision & Interaction ------------------
|
|
1051
|
+
def check_collision(self, other):
|
|
1052
|
+
"""Return True if colliding with another Object2"""
|
|
1053
|
+
return self.distance_to(other) <= self.radius + other.radius
|
|
1054
|
+
|
|
1055
|
+
def resolve_collision(self, other, restitution=0.9):
|
|
1056
|
+
"""Basic elastic collision response between two equal-radius bodies"""
|
|
1057
|
+
if not self.check_collision(other):
|
|
1058
|
+
return False
|
|
1059
|
+
normal = (other.position - self.position)
|
|
1060
|
+
if normal.norm() == 0:
|
|
1061
|
+
normal = Vector2(1, 0)
|
|
1062
|
+
normal = normal.normalized()
|
|
1063
|
+
relative = (other.velocity - self.velocity).dot(normal)
|
|
1064
|
+
if relative > 0:
|
|
1065
|
+
return False
|
|
1066
|
+
impulse = -(1 + restitution) * relative / (1 / self.mass + 1 / other.mass)
|
|
1067
|
+
self.velocity = self.velocity - normal * (impulse / self.mass)
|
|
1068
|
+
other.velocity = other.velocity + normal * (impulse / other.mass)
|
|
1069
|
+
overlap = self.radius + other.radius - self.distance_to(other)
|
|
1070
|
+
if overlap > 0:
|
|
1071
|
+
correction = normal * (overlap / 2)
|
|
1072
|
+
self.position = self.position - correction
|
|
1073
|
+
other.position = other.position + correction
|
|
1074
|
+
return True
|
|
1075
|
+
|
|
1076
|
+
def apply_spring(self, other, k, rest_length):
|
|
1077
|
+
"""Apply spring force to other object using Hooke's Law: F = -k x"""
|
|
1078
|
+
offset = other.position - self.position
|
|
1079
|
+
length = offset.norm()
|
|
1080
|
+
if length == 0:
|
|
1081
|
+
return other
|
|
1082
|
+
extension = length - rest_length
|
|
1083
|
+
direction = offset.normalized()
|
|
1084
|
+
other.apply_force_continuous(direction * (k * extension))
|
|
1085
|
+
self.apply_force_continuous(direction * (-k * extension))
|
|
1086
|
+
return other
|
|
1087
|
+
|
|
1088
|
+
def apply_torque(self, torque):
|
|
1089
|
+
"""Apply rotational torque"""
|
|
1090
|
+
self.torque = torque
|
|
1091
|
+
return self
|
|
1092
|
+
|
|
1093
|
+
# ------------------ Dunder Methods for Arithmetic / Comparison ------------------
|
|
1094
|
+
def __add__(self, other):
|
|
1095
|
+
result = self.clone()
|
|
1096
|
+
if isinstance(other, Object2):
|
|
1097
|
+
result.position = self.position + other.position
|
|
1098
|
+
result.velocity = self.velocity + other.velocity
|
|
1099
|
+
else:
|
|
1100
|
+
result.position = self.position + other
|
|
1101
|
+
result.velocity = self.velocity + other
|
|
1102
|
+
return result
|
|
1103
|
+
|
|
1104
|
+
def __sub__(self, other):
|
|
1105
|
+
result = self.clone()
|
|
1106
|
+
if isinstance(other, Object2):
|
|
1107
|
+
result.position = self.position - other.position
|
|
1108
|
+
result.velocity = self.velocity - other.velocity
|
|
1109
|
+
else:
|
|
1110
|
+
result.position = self.position - other
|
|
1111
|
+
result.velocity = self.velocity - other
|
|
1112
|
+
return result
|
|
1113
|
+
|
|
1114
|
+
def __mul__(self, scalar):
|
|
1115
|
+
result = self.clone()
|
|
1116
|
+
result.position = self.position * scalar
|
|
1117
|
+
result.velocity = self.velocity * scalar
|
|
1118
|
+
result.mass = self.mass * scalar
|
|
1119
|
+
return result
|
|
1120
|
+
|
|
1121
|
+
def __truediv__(self, scalar):
|
|
1122
|
+
result = self.clone()
|
|
1123
|
+
result.position = self.position / scalar
|
|
1124
|
+
result.velocity = self.velocity / scalar
|
|
1125
|
+
result.mass = self.mass / scalar
|
|
1126
|
+
return result
|
|
1127
|
+
|
|
1128
|
+
def __eq__(self, other):
|
|
1129
|
+
if not isinstance(other, Object2):
|
|
1130
|
+
return NotImplemented
|
|
1131
|
+
return self.name == other.name and self.mass == other.mass and self.position == other.position
|
|
1132
|
+
|
|
1133
|
+
def __lt__(self, other):
|
|
1134
|
+
return self.mass < other.mass
|
|
1135
|
+
|
|
1136
|
+
def __le__(self, other):
|
|
1137
|
+
return self.mass <= other.mass
|
|
1138
|
+
|
|
1139
|
+
def __gt__(self, other):
|
|
1140
|
+
return self.mass > other.mass
|
|
1141
|
+
|
|
1142
|
+
def __ge__(self, other):
|
|
1143
|
+
return self.mass >= other.mass
|
|
1144
|
+
|
|
1145
|
+
def __hash__(self):
|
|
1146
|
+
return id(self)
|
|
1147
|
+
|
|
1148
|
+
def clone(self):
|
|
1149
|
+
"""Return a deep copy of this object"""
|
|
1150
|
+
return copy.deepcopy(self)
|
|
1151
|
+
|
|
1152
|
+
def zero_velocity(self):
|
|
1153
|
+
"""Stop the object"""
|
|
1154
|
+
self.velocity = Vector2()
|
|
1155
|
+
self.angular_velocity = 0.0
|
|
1156
|
+
return self
|
|
1157
|
+
|
|
1158
|
+
def zero_forces(self):
|
|
1159
|
+
"""Remove all forces without updating"""
|
|
1160
|
+
self.clear_forces()
|
|
1161
|
+
return self
|
|
1162
|
+
|
|
1163
|
+
def apply_impulse(self, impulse):
|
|
1164
|
+
"""Instant velocity change: Δv = J/m"""
|
|
1165
|
+
self.velocity = self.velocity + impulse / self.mass
|
|
1166
|
+
return self.velocity
|
|
1167
|
+
|
|
1168
|
+
|
|
1169
|
+
def _vectors_in(_class):
|
|
1170
|
+
"""Yield the Vector2 attributes declared on a decorated class."""
|
|
1171
|
+
for _, value in vars(_class).items():
|
|
1172
|
+
if isinstance(value, (Vector2, TimeVector)):
|
|
1173
|
+
yield value
|
|
1174
|
+
|
|
1175
|
+
|
|
1176
|
+
def _register(target_object, force, kind: str):
|
|
1177
|
+
"""Attach a copy of a declared force to a target with the given kind."""
|
|
1178
|
+
force = force.copy()
|
|
1179
|
+
force.type = kind
|
|
1180
|
+
if kind == "impulse":
|
|
1181
|
+
target_object.apply_force_impulse(force)
|
|
1182
|
+
else:
|
|
1183
|
+
target_object.apply_force_continuous(force)
|
|
1184
|
+
return force
|
|
1185
|
+
|
|
1186
|
+
|
|
1187
|
+
def continuous(target_object: Object2):
|
|
1188
|
+
"""@continuous(obj) on a class: apply each Vector2 attribute as a
|
|
1189
|
+
continuous (every-step) force on obj. On a function, run it each step."""
|
|
1190
|
+
def decorator(_class):
|
|
1191
|
+
if inspect.isfunction(_class):
|
|
1192
|
+
_class.force_type = "continuous"
|
|
1193
|
+
return _class
|
|
1194
|
+
applied = [_register(target_object, value, "continuous") for value in _vectors_in(_class)]
|
|
1195
|
+
_class.forces = applied
|
|
1196
|
+
return _class
|
|
1197
|
+
return decorator
|
|
1198
|
+
|
|
1199
|
+
|
|
1200
|
+
def impulse(target_object: Object2):
|
|
1201
|
+
"""@impulse(obj) on a class: apply each Vector2 attribute as a one-shot
|
|
1202
|
+
impulse force on obj (Δv = J/m)."""
|
|
1203
|
+
def decorator(_class):
|
|
1204
|
+
if inspect.isfunction(_class):
|
|
1205
|
+
_class.force_type = "impulse"
|
|
1206
|
+
return _class
|
|
1207
|
+
applied = [_register(target_object, value, "impulse") for value in _vectors_in(_class)]
|
|
1208
|
+
_class.forces = applied
|
|
1209
|
+
return _class
|
|
1210
|
+
return decorator
|
|
1211
|
+
|
|
1212
|
+
|
|
1213
|
+
def update(target_object: Object2, dt: int = 1, length=100):
|
|
1214
|
+
"""Two forms:
|
|
1215
|
+
|
|
1216
|
+
@update(obj) # on a function: a per-tick system
|
|
1217
|
+
def physics(o, dt): ...
|
|
1218
|
+
|
|
1219
|
+
@update(obj, dt=0.016, length=100) # on a class holding a Time():
|
|
1220
|
+
class Newtonian:
|
|
1221
|
+
time = Time()
|
|
1222
|
+
The class form advances `time` and `obj` for `length` steps and stores the
|
|
1223
|
+
per-step `obj.state()` in `Newtonian.history`.
|
|
1224
|
+
"""
|
|
1225
|
+
def decorator(_class):
|
|
1226
|
+
if inspect.isfunction(_class):
|
|
1227
|
+
history = []
|
|
1228
|
+
|
|
1229
|
+
@wraps(_class)
|
|
1230
|
+
def system(obj=target_object, step=dt, *args, **kwargs):
|
|
1231
|
+
result = _class(obj, step, *args, **kwargs)
|
|
1232
|
+
if isinstance(obj, Object2):
|
|
1233
|
+
history.append((obj.state(), step))
|
|
1234
|
+
return result
|
|
1235
|
+
|
|
1236
|
+
system.history = history
|
|
1237
|
+
system.target = target_object
|
|
1238
|
+
return system
|
|
1239
|
+
|
|
1240
|
+
time = next((value for value in vars(_class).values() if isinstance(value, Time)), None)
|
|
1241
|
+
if time is None:
|
|
1242
|
+
raise ValueError("Update rule requires a Time object")
|
|
1243
|
+
history = {}
|
|
1244
|
+
for _ in range(length):
|
|
1245
|
+
time.up(dt)
|
|
1246
|
+
target_object.integrate(dt)
|
|
1247
|
+
history[time.t] = target_object.state()
|
|
1248
|
+
_class.time = time
|
|
1249
|
+
_class.history = history
|
|
1250
|
+
return _class
|
|
1251
|
+
return decorator
|
|
1252
|
+
|
|
1253
|
+
|
|
1254
|
+
def simulate(objects, dt=1, length=100, time: Time = None):
|
|
1255
|
+
"""Run every object's integrate(dt) for `length` steps.
|
|
1256
|
+
|
|
1257
|
+
Continuous forces are re-evaluated each step; impulses are cleared once
|
|
1258
|
+
consumed. Returns {t: [state, ...]} so the run can be replayed.
|
|
1259
|
+
"""
|
|
1260
|
+
if isinstance(objects, Object2):
|
|
1261
|
+
objects = [objects]
|
|
1262
|
+
if time is None:
|
|
1263
|
+
time = Time()
|
|
1264
|
+
history = {}
|
|
1265
|
+
for _ in range(length):
|
|
1266
|
+
time.up(dt)
|
|
1267
|
+
states = []
|
|
1268
|
+
for obj in objects:
|
|
1269
|
+
obj.integrate(dt)
|
|
1270
|
+
states.append(obj.state())
|
|
1271
|
+
history[time.t] = states
|
|
1272
|
+
for obj in objects:
|
|
1273
|
+
obj.clear_impulses()
|
|
1274
|
+
return history
|
|
1275
|
+
|
|
1276
|
+
|
|
1277
|
+
def combine(objects):
|
|
1278
|
+
"""Combine the symbolic equations of motion of several objects.
|
|
1279
|
+
|
|
1280
|
+
Returns one System dict with the per-object acceleration components, the
|
|
1281
|
+
total mass, and the summed net force, all as expressions in T.
|
|
1282
|
+
"""
|
|
1283
|
+
if isinstance(objects, Object2):
|
|
1284
|
+
objects = [objects]
|
|
1285
|
+
objects = list(objects)
|
|
1286
|
+
if not objects:
|
|
1287
|
+
raise ValueError("combine() requires at least one object")
|
|
1288
|
+
|
|
1289
|
+
net = Vector2()
|
|
1290
|
+
for obj in objects:
|
|
1291
|
+
net = net + obj.net_force_symbolic()
|
|
1292
|
+
|
|
1293
|
+
return {
|
|
1294
|
+
"objects": objects,
|
|
1295
|
+
"mass": sum(obj.mass for obj in objects),
|
|
1296
|
+
"net_force": net,
|
|
1297
|
+
"accelerations": [obj.equation_of_motion() for obj in objects],
|
|
1298
|
+
}
|
|
1299
|
+
|
|
1300
|
+
|
|
1301
|
+
def describe_system(objects):
|
|
1302
|
+
"""Human-readable multi-object system summary (forces + accelerations in T)."""
|
|
1303
|
+
if isinstance(objects, Object2):
|
|
1304
|
+
objects = [objects]
|
|
1305
|
+
objects = list(objects)
|
|
1306
|
+
combined = combine(objects)
|
|
1307
|
+
lines = [f"System of {len(objects)} object(s), total mass = {combined['mass']}"]
|
|
1308
|
+
for obj in objects:
|
|
1309
|
+
ax, ay = obj.equation_of_motion()
|
|
1310
|
+
lines.append(f" {obj.name}: a(T) = ({ax}, {ay})")
|
|
1311
|
+
lines.append(f" total net force F(T) = {combined['net_force']}")
|
|
1312
|
+
return "\n".join(lines)
|
|
1313
|
+
|
|
1314
|
+
|
|
1315
|
+
|
|
1316
|
+
|
|
1317
|
+
|
|
1318
|
+
|
|
1319
|
+
|