+/Physics

hard-body physics simulation

git clone https://git.julia.blue/+/Physics

P1n3appl3Began adapting for polygons. Also added broadphase AABB checking10473c3

master
8.9 KiB314 linesraw
1from abc import abstractmethod
2import pygame
3import math
4import numpy
5
6
7class Scene:
8	shapes = []
9	width = 0
10	height = 0
11	bounds = True
12	boundRect = {}
13	fps = 1. / 60
14	maxSpeed = 700
15
16	def __init__(self, w, h, s=[]):
17		self.width = w
18		self.height = h
19		for i in s:
20			self.shapes.append(i)
21
22	def removeEntity(self, e):
23		self.shapes[:] = [i for i in self.shapes if not (i is e)]
24
25	def addEntity(self, e):
26		self.shapes.append(e)
27
28	def draw(self, s):
29		for e in self.shapes:
30			e.draw(s)
31			x, y = e.center()
32			pygame.draw.line(s, (255 - e.color[0], 255 - e.color[1], 255 - e.color[2]), (x, y),
33			                 (x + e.dx / (e.mass * 5000), y + e.dy / (e.mass * 5000)), 2)
34
35	def step(self):
36		collisionPairs = []
37		for s in range(len(self.shapes)):
38			# self.applyGravity(.1)
39			# self.applyRelativeGravity()
40			# self.applyFriction(.00005)
41			self.shapes[s].move(self.fps)
42			self.shapes[s].rotate(self.shapes[s].center(), self.shapes[s].dz)
43			self.shapes[s].color = (255, 255, 255)
44			broad = AABB(self.shapes[s])
45			for s2 in range(len(self.shapes)):
46				broad2 = AABB(self.shapes[s2])
47				if s != s2:
48					if not ((s2, s) in collisionPairs):
49						if broad.intersect(broad2):
50							collisionPairs.append((s, s2))
51
52			if self.bounds:
53				center = self.shapes[s].center()
54				if broad.right > self.width:
55					self.shapes[s].setPos((self.width - (broad.right - center[0]), center[1]))
56					self.shapes[s].dx *= -self.shapes[s].restitution
57				if broad.top > self.height:
58					self.shapes[s].setPos((center[0], self.height - (broad.top - center[1])))
59					self.shapes[s].dy *= -self.shapes[s].restitution
60				if broad.left < 0:
61					self.shapes[s].setPos((center[0] - broad.left, center[1]))
62					self.shapes[s].dx *= -self.shapes[s].restitution
63				if broad.bottom < 0:
64					self.shapes[s].setPos((center[0], center[1] - broad.bottom))
65					self.shapes[s].dy *= -self.shapes[s].restitution
66
67		for c in collisionPairs:
68			if Scene.collision(self.shapes[c[0]], self.shapes[c[1]]):
69				self.shapes[c[0]].color = (255, 0, 0)
70				self.shapes[c[1]].color = (255, 0, 0)
71			# self.circleBounce(self.shapes[c[-1][0]], self.shapes[c[-1][1]])
72
73	def applyGravity(self, strength):
74		for s in self.shapes:
75			s.dy = min(s.dy + strength, self.maxSpeed)
76
77	def applyRelativeGravity(self):
78		for s in self.shapes:
79			for s2 in self.shapes:
80				x, y = s.center()
81				x2, y2 = s2.center()
82				if not (s is s2):
83					norm = numpy.array([x - x2, y - y2]) / math.sqrt((x - x2) ** 2 + (y - y2) ** 2)
84					s2V = numpy.array([s2.dx, s2.dy]) + norm / (5000. * s.mass)
85
86					s2.dx = s2V[0]
87					s2.dy = s2V[1]
88
89	def applyFriction(self, strength):
90		for s in self.shapes:
91			s.dx *= 1 - strength
92			s.dy *= 1 - strength
93
94	@staticmethod
95	def circleBounce(a, b):
96		dist = math.sqrt((a.x - b.x) ** 2 + (a.y - b.y) ** 2)
97		norm = numpy.array([a.x - b.x, a.y - b.y]) / dist
98		minimumTransVector = norm * (a.radius + b.radius - dist)
99		av = numpy.array([a.dx, a.dy])
100		bv = numpy.array([b.dx, b.dy])
101		aMag = numpy.linalg.norm(av)
102		bMag = numpy.linalg.norm(bv)
103		a.setPos(tuple(numpy.array([a.x, a.y]) + minimumTransVector * aMag / (aMag + bMag)))
104		b.setPos(tuple(numpy.array([b.x, b.y]) - minimumTransVector * bMag / (aMag + bMag)))
105		r = min(a.restitution, b.restitution)
106		p = 2 * r * ((av[0] * norm[0] + av[1] * norm[1]) - (bv[0] * norm[0] + bv[1] * norm[1])) / (a.mass + b.mass)
107		av = av - p * a.mass * norm
108		bv = bv + p * b.mass * norm
109		a.dx = av[0]
110		a.dy = av[1]
111		b.dx = bv[0]
112		b.dy = bv[1]
113
114	@staticmethod
115	def polyBounce(a, b, minTrans):
116		# todo: make SAT return minTrans so that this works
117		dist = math.sqrt((a.x - b.x) ** 2 + (a.y - b.y) ** 2)
118		norm = numpy.array([a.x - b.x, a.y - b.y]) / dist
119		minimumTransVector = norm * minTrans
120		av = numpy.array([a.dx, a.dy])
121		bv = numpy.array([b.dx, b.dy])
122		aMag = numpy.linalg.norm(av)
123		bMag = numpy.linalg.norm(bv)
124		a.setPos(tuple(numpy.array([a.x, a.y]) + minimumTransVector * aMag / (aMag + bMag)))
125		b.setPos(tuple(numpy.array([b.x, b.y]) - minimumTransVector * bMag / (aMag + bMag)))
126		r = min(a.restitution, b.restitution)
127		p = 2 * r * ((av[0] * norm[0] + av[1] * norm[1]) - (bv[0] * norm[0] + bv[1] * norm[1])) / (a.mass + b.mass)
128		av = av - p * a.mass * norm
129		bv = bv + p * b.mass * norm
130		a.dx = av[0]
131		a.dy = av[1]
132		b.dx = bv[0]
133		b.dy = bv[1]
134
135	@staticmethod
136	def SAT(v, a, b):
137		aa = []
138		bb = []
139		for i in a:
140			aa.append(numpy.dot(v, i))
141		for i in b:
142			bb.append(numpy.dot(v, i))
143		return max(aa) > min(bb) and max(bb) > min(aa)
144
145	@staticmethod
146	def collision(a, b):
147		if b.collisionMask % a.collisionGroup != 0 or a.collisionMask % b.collisionGroup != 0:
148			return False
149		newA = a
150		newB = b
151		if isinstance(a, Circle) and isinstance(b, Circle):
152			return (a.radius + b.radius) ** 2 > (a.x - b.x) ** 2 + (a.y - b.y) ** 2
153		elif isinstance(a, Polygon) and isinstance(b, Circle):
154			# todo: add voronoi region real circle checking
155			newB = RegularPolygon((b.x, b.y), 10, b.radius)
156		elif isinstance(a, Circle) and isinstance(b, Polygon):
157			# todo: add voronoi region real circle checking
158			newA = RegularPolygon((a.x, a.y), 10, a.radius)
159
160		for i in range(len(newA.points)):
161			temp = tuple(numpy.subtract(newA.points[i], newA.points[i + 1 if i < len(newA.points) - 1 else 0]))
162			if not (Scene.SAT((temp[1] * -1, temp[0]), newA.points, newB.points)):
163				return False
164		for i in range(len(newB.points)):
165			temp = tuple(numpy.subtract(newB.points[i], newB.points[i + 1 if i < len(newB.points) - 1 else 0]))
166			if not (Scene.SAT((temp[1] * -1, temp[0]), newA.points, newB.points)):
167				return False
168		return True
169
170
171class Entity:
172	"""Parent class for all physics objects"""
173	dx = 0.
174	dy = 0.
175	dz = 0.
176	color = (255, 255, 255)
177	fixed = False
178	mass = 1.
179	restitution = 1.
180	collisionGroup = 1
181	collisionMask = 1
182
183	def addCollisionMask(self, n):
184		self.collisionMask |= (n + 1)
185
186	def removeCollisionMask(self, n):
187		self.collisionMask -= (n + 1) ** 2
188
189	def setCollisionGroup(self, n):
190		self.collisionGroup = 2 ** n
191
192	def __init__(self):
193		return
194
195	@abstractmethod
196	def draw(self, s):
197		return
198
199	@abstractmethod
200	def rotate(self, (x, y), ang=math.pi / 4):
201		return
202
203	@abstractmethod
204	def center(self):
205		return
206
207	@abstractmethod
208	def move(self, fps):
209		return
210
211	@abstractmethod
212	def setPos(self, (x, y)):
213		return
214
215
216class Circle(Entity):
217	radius = 1
218	x = 0.
219	y = 0.
220
221	def __init__(self, (x, y), r, c=(255, 255, 255)):
222		self.x = x
223		self.y = y
224		self.radius = r
225		self.color = c
226		self.mass = 1. / int(r ** 2)
227
228	def draw(self, s):
229		pygame.draw.circle(s, self.color, (int(self.x), int(self.y)), self.radius)
230
231	def collision(self, other):
232		if isinstance(other, Circle):
233			return (self.radius + other.radius) ** 2 > (self.x - other.x) ** 2 + (self.y - other.y) ** 2
234		else:
235			return False
236
237	def rotate(self, (x, y), ang=math.pi / 4):
238		return
239
240	def center(self):
241		return self.x, self.y
242
243	def setPos(self, pos):
244		self.x, self.y = pos
245
246	def move(self, fps):
247		self.x += self.dx * fps
248		self.y += self.dy * fps
249
250
251class Polygon(Entity):
252	points = [(0., 0.)]
253
254	def __init__(self, p, c=(255, 255, 255)):
255		self.points = p
256		self.color = c
257		self.mass = 1. / self.area()
258
259	def draw(self, s):
260		pygame.draw.polygon(s, self.color, self.points, 0)
261
262	def center(self):
263		return sum(((float(i[0]) for i in self.points))) / len(self.points), sum(
264			(float(i[1]) for i in self.points)) / len(self.points)
265
266	def area(self):
267		return abs(sum(self.points[i][0] * self.points[i + 1 if i < len(self.points) - 1 else 0][1] - self.points[i][1] *
268		           self.points[i + 1 if i < len(self.points) - 1 else 0][0] for i in range(len(self.points))) / 2)
269
270	def rotate(self, (x, y), ang=math.pi / 4):
271		self.points = [(x + (p[0] - x) * math.cos(ang) - (p[1] - y) * math.sin(ang),
272		                y + (p[0] - x) * math.sin(ang) + (p[1] - y) * math.cos(ang)) for p in self.points]
273
274	def setPos(self, pos):
275		c = self.center()
276		self.points = [(i[0] - c[0] + pos[0], i[1] - c[1] + pos[1]) for i in self.points]
277
278	def move(self, fps):
279		self.points = [(i[0] + self.dx * fps, i[1] + self.dy * fps) for i in self.points]
280
281
282class RegularPolygon(Polygon):
283	def __init__(self, p, s, r, c=(255, 255, 255)):
284		n = math.pi * 2 / s
285		Polygon.__init__(self, [(math.sin(n * i) * r + p[0], math.cos(n * i) * r + p[1]) for i in range(s)], c)
286
287
288class Rectangle(Polygon):
289	def __init__(self, p, w, h, c=(255, 255, 255)):
290		Polygon.__init__(self, [(p[0], p[1]), (p[0] + w, p[1]), (p[0] + w, p[1] + h), (p[0], p[1] + h)], c)
291
292
293class AABB:
294	top = float('-inf')
295	bottom = float('inf')
296	left = float('inf')
297	right = float('-inf')
298
299	def __init__(self, s):
300		if isinstance(s, Circle):
301			self.top = s.y + s.radius
302			self.bottom = s.y - s.radius
303			self.left = s.x - s.radius
304			self.right = s.x + s.radius
305		else:
306			for p in s.points:
307				self.left = p[0] if p[0] < self.left else self.left
308				self.right = p[0] if p[0] > self.right else self.right
309				self.top = p[1] if p[1] > self.top else self.top
310				self.bottom = p[1] if p[1] < self.bottom else self.bottom
311
312	def intersect(self, other):
313		return not (
314			self.bottom > other.top or self.top < other.bottom or self.left > other.right or self.right < other.left)