#################################################################################### #nworld, something the node/beam live in #################################################################################### #notes: # TOO MANY BEAMS PER NODE IS VERY BAD (max 10, 15 beam per node) # Always work in triangles. # Round shapes need pressurization from center(adjust center beams restlenght++ after spawn) # If it explodes, reduce rigidity and augment iterations. #dependency #################################################################################### import math, vector, collision from vector import * from collision import * #constant #################################################################################### BEAMITERATE = 6 #~1-10 # constraint iterations for beams BOXITERATE = 1 # 1 # number of constraint iterations for boundaries GRAVITY = 6.67*10**-11 # world earth gravity constant EARTHMASS = 5.974*10**24 # mass of world earth EARTHRADIUS = 6.38*10**6 # radius of world earth DRAG = 0.9985 #~0.9985 # world energy drag FRICTION = 0.5 #~0.5 # world boundary collision friction RESTITUTION = 0.75 #~0.75 # world boundary collision restitution DT = 0.20 #~0.20 # default delta T, simulation speed beamtype = 1 breaktype = 2 springtype = 0 blue = (50,50,255) red = (255,50,50) green = (50,255,50) ## demo collision lines pointA = vector(0.0,512.0) pointB = vector(1024.0,185.0) origAB = vector(1024,512.0) pointC = vector(0.0,100.0) pointD = vector(395.0,512.0) origCD = vector(0.0,512.0) #container #################################################################################### ## le monde est la dedans class nworld(object): def __init__(self, ax,ay, bx,by): self.nodeLst = [] # list of nodes self.beamLst = [] # list of beams self.box = [ax,ay,bx,by] # boundaries self.DT = DT # simulation speed self.DT2 = self.DT**2 # precalculate square self.drag = DRAG # system energy drag self.gravity = EARTHMASS * GRAVITY * (1/EARTHRADIUS**2) # precalc world gravity (~=9.78) self.wind = vector(0.0,0.0) # stores wind self.restitution = RESTITUTION # world boundaries bounciness self.friction = FRICTION # world boundaries friction ## Node class class node: def __init__(self, p, mass): self.p = p # current position self.op = self.p # old position self.force = vector(0.0,0.0) # force accumulator self.accel = vector(0.0,0.0) # acceleration vector self.mass = float(mass) # mass of node MUST BE FLOAT self.invmass = 1/self.mass # precalculate inverse of mass self.nailed = 0 # pinned to position 0 or 1 self.col = red ## Beam class class beam: def __init__(self, na, nb, typeof): self.nA = na # node A self.nB = nb # node B self.rigidity = 0.5 # beam rigidity 1 to 0 self.restlenght = Vdist(self.nA.p, self.nB.p) # save original lenght self.currlenght = self.restlenght # store current lenght self.pushforce = 0 # store current force on beam self.typeof = typeof #function #################################################################################### ## update world def update(world): gravity(world) # calculate gravity force & add to forceaccumulator wind(world) # add wind force to force accumulator accelerate(world) # calculate acceleration integrate(world) # integrate positions and acceleration ##/ constraints HERE!\## for i in range(BEAMITERATE): # beam constraint relaxation loop, more iterations = stiffer beamconstraints(world) # keep beam lenght original boxconstraints(world) # world boundaries for i in range(BOXITERATE): # box constraint relaxation loop boxconstraints(world) # world boundaries pointlineconstraints(world) ##\ constraints HERE!/## ## calculate gravity force on node.mass and add it to force accumulator for all nodes def gravity(world): for nugget in world.nodeLst: nugget.force = Vadd(nugget.force, vector(0, nugget.mass * world.gravity)) ## nail node on world def pin(nugget): nugget.invmass = 0 nugget.nailed = 1 ## set node free def unpin(nugget): nugget.invmass = 1/nugget.mass nugget.nailed = 0 ## apply force to a specific node def push(nugget, bump): nugget.force = Vadd(nugget.force, bump) ## impulse force on a specific node scaled independent of mass (force of 30 accelerates 1kg or 100kg the same) def impulse(nugget, bump): nugget.force = Vadd(nugget.force, VmulScale(bump, nugget.mass)) ## add wind force to force accumulator for all nodes def wind(world): for nugget in world.nodeLst: nugget.force = Vadd(nugget.force, world.wind) ## accelerate node with forces acting on mass for all nodes def accelerate(world): for nugget in world.nodeLst: nugget.force = VmulScale(nugget.force, world.drag) # world energy drag to remove jitter nugget.accel = VmulScale(VmulScale(nugget.force, nugget.invmass), world.DT2) nugget.force = vector(0,0) # reset node forces to zero ## integrate new position with accel for all nodes: (np = (DRAG+1*p) - (DRAG*op) + (a*dt2) def integrate(world): for nugget in world.nodeLst: nugget.op, nugget.p = nugget.p, Vadd(Vsub(VmulScale(nugget.p, world.drag+1), VmulScale(nugget.op, world.drag)), nugget.accel) ## world/screen limits, NOT TRUE COLLISION, THIS IS FOR WORLD BOUNDARIES ONLY! def boxconstraints(world): for nugget in world.nodeLst: if nugget.p.x < world.box[0]: # if out of bounds: impactvel = (nugget.op.x - nugget.p.x)*world.restitution # impact velocity adjusted with restitution coeeficient nugget.p.x = world.box[0] # new pos outside bounds to satisfy constraint. nugget.op.x = nugget.p.x - impactvel # old pos adjusted to penetration depth - adjusted impact velocity nugget.op.y -= (nugget.op.y - nugget.p.y)*world.friction # some friction nugget.col = green if nugget.p.y < world.box[1]: impactvel = (nugget.op.y - nugget.p.y)*world.restitution nugget.p.y = world.box[1] nugget.op.y = nugget.p.y - impactvel nugget.op.x -= (nugget.op.x - nugget.p.x)*world.friction nugget.col = green if nugget.p.x > world.box[2]: impactvel = (nugget.op.x - nugget.p.x)*world.restitution nugget.p.x = world.box[2] nugget.op.x = nugget.p.x - impactvel nugget.op.y -= (nugget.op.y - nugget.p.y)*world.friction nugget.col = green if nugget.p.y > world.box[3]: impactvel = (nugget.op.y - nugget.p.y)*world.restitution nugget.p.y = world.box[3] nugget.op.y = nugget.p.y - impactvel nugget.op.x -= (nugget.op.x - nugget.p.x)*world.friction nugget.col = green ## point/line constraints test #### rebuild to test list of lines and some grid optimisation def pointlineconstraints(world): for nugget in world.nodeLst: rest = 0.25 # restitution(need to include this in ~line.rest) if (collidePointLineSQ(nugget.p, origAB, pointA, pointB)): nugget.col = green # color yes collided intersect = VLineIntersect(nugget.p, nugget.op, pointA, pointB) # get intersection point nearpointP = VclosestPoint(nugget.p, pointA, pointB) # get nearest point on line from position nearpointOP = VclosestPoint(nugget.op, pointA, pointB) # get nearest point on line from old pos xlateP = VmulScale(Vsub(nearpointP, nugget.p), 2) # get pos translation vector xlateOP = VmulScale(Vsub(nearpointOP, nugget.op), 2) # get old pos xlate vector nugget.op = Vadd(nugget.op , xlateOP) # translate old pos nugget.p = Vadd(nugget.p, xlateP) # translate pos restituteVOP = VmulScale(Vsub(intersect, nugget.op), rest) # calculate bounce vector and pull points, restituteVP = VmulScale(Vsub(intersect, nugget.p), rest) # towards intersection point to reduce velocity, nugget.op = Vadd(nugget.op, restituteVOP) # for translated oldpos and trasnlated pos. nugget.p = Vadd(nugget.p, restituteVP) # apply translation toward intersection point if (collidePointLineSQ(nugget.p, origCD, pointC, pointD)): nugget.col = green intersect = VLineIntersect(nugget.p, nugget.op, pointC, pointD) nearpointP = VclosestPoint(nugget.p, pointC, pointD) nearpointOP = VclosestPoint(nugget.op, pointC, pointD) xlateP = VmulScale(Vsub(nearpointP, nugget.p), 2) xlateOP = VmulScale(Vsub(nearpointOP, nugget.op), 2) nugget.op = Vadd(nugget.op , xlateOP) nugget.p = Vadd(nugget.p, xlateP) restituteVOP = VmulScale(Vsub(intersect, nugget.op), rest) restituteVP = VmulScale(Vsub(intersect, nugget.p), rest) nugget.op = Vadd(nugget.op, restituteVOP) nugget.p = Vadd(nugget.p, restituteVP) ## beam lenght constraint, ####not optimized#### def beamconstraints(world): FORCEbreak = 2.0 LENGHTbreak = 0.5 FORCEdeform = 1.25 LENGHTdeform = 0.2 STABILITYbreak = 3 for bar in world.beamLst: if bar.typeof == beamtype or bar.typeof == breaktype: deltabeam = Vsub(bar.nB.p, bar.nA.p) # find beam vector bar.currlenght = math.sqrt(Vdot(deltabeam, deltabeam)) # find its lenght bar.pushforce = 0.1 if bar.currlenght != 0: # spawned lenght VS current bar.pushforce = (bar.currlenght-bar.restlenght) / (bar.currlenght*(bar.nA.invmass+bar.nB.invmass)) else: print "stability override!" pushvector = VmulScale(deltabeam, bar.rigidity*bar.pushforce) # get forces out of this bar.nA.p = Vadd(bar.nA.p, VmulScale(pushvector, bar.nA.invmass)) # distribute forces bar.nB.p = Vsub(bar.nB.p, VmulScale(pushvector, bar.nB.invmass)) #### test DEFORM AND BREAK CODE DeltaLenght = (bar.restlenght - bar.currlenght) / (bar.restlenght+0.00001) # % of deformation if bar.typeof == breaktype and abs(DeltaLenght) > LENGHTdeform and abs(bar.pushforce) > FORCEdeform: bar.restlenght = bar.restlenght - (bar.restlenght*(DeltaLenght*bar.rigidity)*0.25) print "DEFORMED! delta lenght=", DeltaLenght if abs(bar.pushforce*bar.rigidity) > FORCEbreak and abs(DeltaLenght) > LENGHTbreak: print "BREAKAGE! force=",bar.pushforce*bar.rigidity #world.beamLst.remove(bar) ### ca chie la liste bar.nA = world.nodeLst[0] # fak on fait juste changer les 2 nodes du beam par des null bar.nB = world.nodeLst[1] if abs(DeltaLenght) > STABILITYbreak: print "STABILITY BREAKAGE! force=",bar.pushforce*bar.rigidity #world.beamLst.remove(bar) ### ca chie la liste bar.nA = world.nodeLst[0] bar.nB = world.nodeLst[1] ### optimization notes: (without mass for now) #using only one iteration and approximating the square root removes the stiffness #that appears otherwise when the sticks are perfectly stiff @ low iterations. #Pseudo-code using sqrt approximation # delta = x2-x1; if bar.typeof == springtype: deltabeam = Vsub(bar.nA.p, bar.nB.p) dotdelta = Vdot(deltabeam, deltabeam) restlenSQ = bar.restlenght**2 masses = bar.nA.invmass + bar.nB.invmass # delta*=restlength*restlength/(delta*delta+restlength*restlength)-0.5; deltaSQ = restlenSQ / (dotdelta + restlenSQ)-0.5 bar.pushforce = deltaSQ pushvector = VmulScale(deltabeam, deltaSQ*bar.rigidity) # x1 += delta; # x2 -= delta; bar.nA.p = Vadd(bar.nA.p, VmulScale(pushvector, bar.nA.invmass)) # distribute forces bar.nB.p = Vsub(bar.nB.p, VmulScale(pushvector, bar.nB.invmass)) ## demo functions #################################################################################### #### demo force to move nodes def demopush(world): for nugget in world.nodeLst: # give a little bump for demo impulse(nugget , vector(500.0, -500.0)) #### demo function to make free nodes def demonodes(world): world.nodeLst.append(node(vector(100.0,0.0),0.1)) # build some nodes with mass world.nodeLst.append(node(vector(610.0,0.0),10.2)) world.nodeLst.append(node(vector(520.0,0.0),1.4)) # MASS MUST BE FLOAT world.nodeLst.append(node(vector(530.0,20.0),11.6)) world.nodeLst.append(node(vector(140.0,0.0),2.0)) # more nodes with mass world.nodeLst.append(node(vector(150.0,0.0),4.0)) world.nodeLst.append(node(vector(160.0,0.0),8.0)) world.nodeLst.append(node(vector(170.0,0.0),16.0)) world.nodeLst.append(node(vector(50.0,0.0),100.0)) # most heavy node to explode system >:) #### demo function to make 6 beam square def demosquare(world): a = node(vector(900.0,100.0),10.0) b = node(vector(900.0,150.0),10.0) c = node(vector(950.0,100.0),10.0) d = node(vector(950.0,150.0),10.0) world.nodeLst.append(a) world.nodeLst.append(b) world.nodeLst.append(c) world.nodeLst.append(d) world.beamLst.append(beam(a,b,springtype)) world.beamLst.append(beam(b,c,springtype)) world.beamLst.append(beam(c,a,springtype)) world.beamLst.append(beam(d,c,springtype)) world.beamLst.append(beam(d,b,springtype)) world.beamLst.append(beam(a,d,springtype)) #### demo function to make shape of node beams def demoshape(world): center = node(vector(850.0,100.0), 30.0) world.nodeLst.append(center) r = 0.0 l = 4.0 #number of shape sides c,e = None,None first = None first2 = None for i in range(int(l)): r = i*2*math.pi/l x = (math.sin(r) * 65)+center.p.x y = (math.cos(r) * 65)+center.p.y b = node(vector(x,y),10.0) x = (math.sin(r) * 35)+center.p.x y = (math.cos(r) * 35)+center.p.y d = node(vector(x,y),10.0) world.nodeLst.append(b) world.nodeLst.append(d) world.beamLst.append(beam(center,b,breaktype)) world.beamLst.append(beam(b,d,breaktype)) if c: world.beamLst.append(beam(b,c,breaktype)) world.beamLst.append(beam(d,e,breaktype)) world.beamLst.append(beam(c,d,breaktype)) else: first = b first2 = d c = b e = d world.beamLst.append(beam(c,first,breaktype)) world.beamLst.append(beam(e,first2,breaktype)) world.beamLst.append(beam(c,first2,breaktype)) #################################################################################### #TEST CODE #################################################################################### if __name__ == "__main__": import pyg, nworld from nworld import * ## world init world = nworld(0,0, pyg.width, pyg.height-10) # create world (start coords, end coords) ## demo toggle functions demonodes(world) # create some free nodes demoshape(world) # create some node/beam shape demosquare(world) # create a square pin(world.nodeLst[3]) # nail a node down #demopush(world) # impulse it all around for demo ## demo counters counter = 0 d=5 a=1 ## demo hydro beam setup world.beamLst.append(beam(world.nodeLst[19],world.nodeLst[3], beamtype)) # hydro beam hydro = 26 # position in list world.beamLst[hydro].rigidity = 0.5 # demo hydro beam rigidity ## chain beam world.beamLst.append(beam(world.nodeLst[20],world.nodeLst[9],springtype)) #### main while pyg.main(): ## draw world pyg.line(world.box[0],world.box[3],world.box[2],world.box[3], green) # draw ground line pyg.line(pointA.x, pointA.y, pointB.x, pointB.y, blue) pyg.line(pointC.x, pointC.y, pointD.x, pointD.y, red) ## demo timed impulse on a node counter += 0.2 if counter > 100: counter = 0 a = -a impulse(world.nodeLst[5], vector(4000*a,-4000)) ## demo hydro beam if world.beamLst[hydro].restlenght > 460: # change beam lenght in realtime world.beamLst[hydro].restlenght = 460 d = -d if world.beamLst[hydro].restlenght < 5: world.beamLst[hydro].restlenght = 5 d = -d world.beamLst[hydro].restlenght += d ## physics update update(world) # one physics tick ## draw objects for i in world.nodeLst: # render nodes pyg.circle(i.p.x, i.p.y, 2, i.col) i.col = red # reset collision color pyg.line(i.p.x, i.p.y, i.op.x, i.op.y, blue) for i in world.beamLst: # render beams colorratio = 4500 cf = int(i.pushforce * colorratio) # make some force color if cf > 127: cf = 127 elif cf < -127: cf = -127 # testing force color color = 127-cf, 127, cf+127 pyg.line(i.nA.p.x,i.nA.p.y, i.nB.p.x,i.nB.p.y, color)