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