我需要用车轮模型(在此情况下：一个带有小圆盘的不旋转圆盘，上面带有弹簧圈来模拟轮胎)来编码2D物理引擎(暂时不旋转)。到现在为止，它运转良好(假设我选择了足够短的时间步长)，但是现在我必须增加摩擦力(可以是完全摩擦力：轮胎和地板之间没有相对速度)。
因此，当我计算碰撞时，我想在力加速之前知道速度。因此，代替了(强制)>(碰撞)>(从加速度更改速度)>(更新位置)，我使用(力)>(从加速度更改速度)>(碰撞)>(更新位置)。但是，无论时间长短，我都会得到奇怪的结果，尤其是在发生碰撞时。
我可能在第一步的步骤上会遇到麻烦，但是我想这会更加复杂。
[在这里的代码中，我试图专注于主要方面(但是也不是那么少)，所以我消除了摩擦，例如，问题似乎出在我的步骤上。
在tkinter窗口中，如果要测试，可以使用几个时间步骤(例如，第一个完全失败)。
提前感谢
PS：我知道弹簧非常坚固(k = 1e7)，那应该是一个轮子。import numpy as np
import math as m
import random as rd
import tkinter as tk
import time
def CC2(coords,size=500,zoom=160,offset=[100,100]):#Change x,y coordinates into canvas coordinates
x = int(coords[0]*zoom+offset[0])
y = int((size-coords[1]*zoom)-offset[1])
return x,y
def CC4(coords):#Change from (x1,y1),(x2,y2)
return CC2(coords[0]),CC2(coords[1])
def normalize(vec):#Normalize the vector
return (1/norm(vec))*vec
def norm(vec):#Norm of the vector
return m.sqrt(sum(vec**2))
def sqnorm(vec):#Square norm
return sum(vec**2)
class Scene:
def __init__(self,objectlist,canvas):
self.can = canvas
self.objects = objectlist
self.time = 0#Scene timer
g = 9.81
self.gravity = np.array([0,-g])
def makeStep(self,dt=0.01,display = True):
#Acceleration from gravity
for obj in self.objects:
if obj.invmass != 0:
obj.accel = self.gravity.copy()
#Get accelerations from other forces (here : spring joints)
for obj in self.objects:
if obj.invmass != 0:
#From special joints i.e. spring joints
for joint in obj.joints:#Joint → Force
j = joint.objId
o1 = self.objects[j]
force = joint.getForce(o1,obj)
o1.accel += o1.invmass*force
obj.accel -= obj.invmass*force
"""
Works quite well when the following loop is AFTER the collisions
But in order to add (full) friction properly I wanted to know the speed AFTER applying the forces hence the acceleration
(I can maybe do otherwise but it's more complicated and might not work either...)
"""
#Change speeds from acceleration
for obj in self.objects:
obj.accelerate(dt)
#Apply collisions and change speeds
self.findCollisions(dt)
#Move objects
for obj in self.objects:
obj.move(dt)
if display:
self.display()
self.time += dt
def play(self,dt=0.0001,total_time=5,get_energies=False):#Play the simulation (dt is the time step)
realtime = time.time()
starting_time=realtime
last_display = realtime
while self.time-starting_time <= total_time:
#Just for display
display = False
if time.time()-last_display >= 0.1:
display = True
last_display = time.time()
#Next step
self.makeStep(dt,display)
def findCollisions(self,dt):#Find all collisions, get normal vectors from getCollision and call resolveCollision
n = len(self.objects)
for i in range(n):
o2 = self.objects[i]
joints = o2.joints
for j in range(i):# j
o1 = self.objects[j]#Objects 1 & 2
if o1.classCollide(o2):#Classes compatible for collision
if o1.bboxIntersect(o2):
normal = self.getCollision(o1,o2)
self.resolveCollision(o1,o2,normal)#Resolve collision
def resolveCollision(self,o1,o2,normal):#Change speed and position to resolve collision
if normal.any():#normal is not 0,0 (collision)
depth = norm(normal)
normal = 1/depth*normal
relative_speed = o2.speed - o1.speed
normal_speed = relative_speed @ normal#Norm of projection of relative speed
total_invmass = o1.invmass + o2.invmass#Sum of inverse masses
if normal_speed > 0:#Real collision:
e=1
coef = (1+e)*normal_speed
o1.speed += coef*(o1.invmass/total_invmass)*normal
o2.speed += -coef*(o2.invmass/total_invmass)*normal
if 0.001
correction = 0.2*depth/total_invmass*normal
o1.center += o1.invmass*correction
o2.center -= o2.invmass*correction
def getCollision(self,o1,o2,display=False):#Intersection between objects with intersecting bbox: returns normal vector with norm = penetration depth (directed towards o1)
if o1.type == "box" and o2.type == "box":
delta = o2.center-o1.center
dim_sum = o1.dimensions+o2.dimensions#Sum of half-widths and heights
dsides = [delta[0]+dim_sum[0],-delta[0]+dim_sum[0],delta[1]+dim_sum[1],-delta[1]+dim_sum[1]]#Left, right, bottom, top, bottom, left, right of o1
imin = np.argmin(dsides)
if imin == 0:#Left
normal = np.array([dsides[0],0])#Orientation : right = positive
elif imin == 1:#Right
normal = np.array([-dsides[1],0])
elif imin == 2:#Bottom
normal = np.array([0,dsides[2]])
else:#Top
normal = np.array([0,-dsides[3]])
return normal
if o1.type == "disc":
return o1.getCollisionVector(o2)
if o2.type == "disc":
return -o2.getCollisionVector(o1)
def display(self):#Just display the scene
self.can.delete('all')
for obj in self.objects:
color = "yellow"
if obj.type == "box":
if obj.invmass==0:#Unmoveable
color = "black"
can.create_rectangle(CC4(obj.bbox()),fill=color)
if obj.type == "disc":
can.create_oval(CC4(obj.bbox()),fill="springgreen")
for joint in obj.joints:
can.create_line(CC2(obj.center),CC2(self.objects[joint.objId].center+joint.offset),dash=(3,2))
fen.update()
## Objects
class Object2D:#Abstract class for circles and boxes
def bboxIntersect(self,object2):#Intersection of bounding boxes
bbox1 = self.bbox()
bbox2 = object2.bbox()
if (bbox1[1][0]bbox2[1][0]):#No intersecting on x axis
return False
if (bbox1[1][1]bbox2[1][1]):#No intersecting on y axis
return False
return True
def move(self,dt):
if self.invmass == 0:
return None
self.center += dt*self.speed
def accelerate(self,dt):
if self.invmass == 0:
return None
self.speed += self.accel*dt
def classCollide(self,obj):
if (self.cls == "nc1" or obj.cls == "nc1"):#No collision at all
return False
if (self.cls == "nc2" and obj.cls == "nc2"):#No collision inside this class
return False
return True
class Box(Object2D):
def __init__(self,mass,center,width,height,initspeed=[0.0,0.0],joints=[],cls=""):
self.invmass = 1/mass
self.center = np.array(center,dtype=float)#x,y
self.hheight = height/2#Half height
self.hwidth = width/2
self.dimensions=np.array([self.hwidth,self.hheight])
self.speed = np.array(initspeed,dtype=float)#Initial speed (x,y)
self.accel = np.zeros(2)#x,y acceleration
self.type = "box"
self.joints = joints
self.cls=cls
def bbox(self):
return (self.center[0]-self.hwidth,self.center[1]-self.hheight),(self.center[0]+self.hwidth,self.center[1]+self.hheight)
class Disc(Object2D):
def __init__(self,mass,center,radius,initspeed=[0.0,0.0],joints = [],cls=""):
self.invmass = 1/mass
self.center = np.array(center,dtype=float)#x,y
self.radius = radius
self.speed = np.array(initspeed,dtype=float)#Initial speed (x,y)
self.accel = np.zeros(2)#x,y acceleration
self.type = "disc"
self.joints = joints
self.cls=cls
def bbox(self):
return (self.center[0]-self.radius,self.center[1]-self.radius),(self.center[0]+self.radius,self.center[1]+self.radius)
def getCollisionVector(self,obj):
if obj.type == "box":#VS BOX
box = obj
bbox = box.bbox()
delta = self.center-box.center
if (bbox[0][0] <= self.center[0] <= bbox[1][0]):#Vertical collision
return np.sign(delta[1])*np.array([0,self.radius+box.hheight-abs(delta[1])])
if (bbox[0][1] <= self.center[1] <= bbox[1][1]):#Horizontal collision
return np.sign(delta[0])*np.array([self.radius+box.hwidth-abs(delta[0]),0])
#else find closest corner
if delta[1] > 0:#Top
if delta[0] > 0:#Right
delta_corner = self.center - (box.center+box.dimensions)
else:#Left
delta_corner = self.center - (box.center+np.array([-box.hwidth,box.hheight]))
else:#Bottom
if delta[0] > 0:#Right
delta_corner = self.center - (box.center+np.array([box.hwidth,-box.hheight]))
else:#Left
delta_corner = self.center - (box.center-box.dimensions)
distance = norm(delta_corner)
if distance > self.radius:#No collision
return np.zeros(2)
return (self.radius-distance)/distance*delta_corner
elif obj.type == "disc":#VS DISC
delta = self.center - obj.center
norm_delta = norm(delta)
depth = self.radius + obj.radius - norm_delta
if depth > 0:#Collision
return depth*normalize(delta)
return np.zeros(2)
class Floor(Box):
def __init__(self,y,xmin=-500,xmax=500):
self.invmass = 0#Infinite mass
self.y = y
self.hwidth = (xmax-xmin)/2
self.hheight = 50
self.dimensions=np.array([self.hwidth,self.hheight])
self.center = np.array([(xmin+xmax)/2,y-50])
self.type = "box"
self.accel = np.zeros(2)
self.speed = np.zeros(2)
self.joints = []
self.cls=""
## Forces & joints
class SpringJoint:
def __init__(self,objId,k,l0,damper=10,offset=[0,0]):
self.objId = objId
self.l0 = l0
self.k = k
self.offset = np.array(offset)
self.damper = damper
def getForce(self,o1,o2):
delta = o2.center - (o1.center+self.offset)
normal = normalize(delta)
diff = delta - self.l0*normal
delta_speed = o2.speed - o1.speed
return self.k*diff + self.damper*delta_speed@normal*normal
## Objects definitions
#Test wheel with spring : generates a "wheel" model
def getWheel(Radius,IntRadius,IntMass,ExtMass,kr,ks,x=0,y=0.5,n=14,initspeed=[0,0]):
arc = 2*m.pi*Radius/n
r = 0.35*arc
l0s = 2*(Radius-r)*m.sin(m.pi/n)
R = IntRadius - r
l0r = Radius - r
core = Disc(IntMass,[x,y],R,initspeed=initspeed)
tyre= []
for k in range(n):
a = k/n*2*m.pi
tyre.append(Disc(ExtMass/n,[x+l0r*m.cos(a),y+l0r*m.sin(a)],r,joints=[SpringJoint(0,kr,l0r),SpringJoint(k%n,ks,l0s)],cls="nc2"))
#Discs from the outside don't interact with each other except with the spring joints
tyre[-1].joints.append(SpringJoint(1,ks,l0s))
del tyre[0].joints[1]
return [core] + tyre
#Objects in the scene
#☺Simple wheel with n=5
objects = getWheel(0.5,0.25,500,1,1e7,1e7,y=0.5,initspeed=[5,0],n=5) + [Floor(0)]
## Scene
fen = tk.Tk()
can = tk.Canvas(fen,width = 1000,height=500)
can.pack()
scene = Scene(objects,can)
scene.display()
tk.Button(fen,text="Go quick (10**-3 s)",command = lambda : scene.play(0.001,3,get_energies)).pack()
tk.Button(fen,text="Go medium (10**-4)",command = lambda : scene.play(0.0001,3,get_energies)).pack()
tk.Button(fen,text="Go slowly (3*10**-5)",command = lambda : scene.play(0.00003,1,get_energies)).pack()
tk.Button(fen,text="Go very slowly (10**-5)",command = lambda : scene.play(0.00001,1,get_energies)).pack()
tk.Button(fen,text="Do 0.01s",command = lambda : scene.play(0.0001,0.01,get_energies)).pack()
tk.Button(fen,text="Do 1 step",command = lambda : scene.play(0.01,0.01,get_energies)).pack()
fen.mainloop()
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
