Python 简单2D物理引擎的摩擦和碰撞问题

我需要编写一个2d物理引擎（在没有旋转的情况下）和一个车轮模型（这里：一个不旋转的圆盘，圆盘上连接着小圆盘，弹簧在一个圆圈内模拟轮胎）。 到目前为止，它运行得相当好（考虑到我选择的时间步长足够短），但现在我必须添加摩擦力（可以是完全摩擦力：轮胎和地板之间没有相对速度）

所以当我计算碰撞时，我想知道由于力的作用，加速度之前的速度。所以不是（力）>（碰撞）>（从加速度改变速度）>（更新位置）， 我使用了（力）>（从加速度改变速度）>（碰撞）>（更新位置）。 但是，无论时间步长如何，我都会有奇怪的结果，尤其是在碰撞时

我可能会和一阶步骤有摩擦，但我想它会更复杂

在这里的代码中，我试图把重点放在主要的事情上（但也不是很小），所以我消除了摩擦，例如，因为问题似乎是按照我的步骤顺序出现的

在tkinter窗口中，如果要测试，有几个时间步骤可用（例如，第一个完全失败）

提前谢谢

注：我知道弹簧非常坚固（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<i
                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<depth:#Positional correction
                    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[0][0] or bbox1[0][0]>bbox2[1][0]):#No intersecting on x axis
            return False
        if (bbox1[1][1]<bbox2[0][1] or bbox1[0][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()

将numpy导入为np
导入数学为m
将随机导入为rd
将tkinter作为tk导入
导入时间
def CC2（坐标，大小=500，缩放=160，偏移量=100100]）：#将x，y坐标更改为画布坐标
x=int（坐标[0]*缩放+偏移[0]）
y=int（（大小坐标[1]*缩放）-偏移量[1]）
返回x，y
def CC4（坐标）：#从（x1，y1）、（x2，y2）更改
返回CC2（coords[0]），CC2（coords[1]）
def规格化（vec）：#规格化向量
返回值（1/norm（vec））*vec
def范数（vec）：#向量的范数
返回平方米（总和（向量**2））
def sqnorm（vec）：#平方范数
返回和（向量**2）
课堂场景：
定义初始化（自、对象列表、画布）：
self.can=canvas
self.objects=objectlist
self.time=0#场景计时器
g=9.81
self.gravity=np.array（[0，-g]）
def生成步骤（自，dt=0.01，显示=True）：
#重力加速度
对于self.objects中的obj：
如果obj.invmass！=0:
obj.accel=self.gravity.copy（）
#从其他力获得加速度（此处：弹簧接头）
对于self.objects中的obj：
如果obj.invmass！=0:
#来自特殊接头，即弹簧接头
对于对象关节中的关节：#关节→ 强迫
j=关节.objId
o1=自我对象[j]
力=关节力（o1，obj）
o1.加速度+=o1.惯性质量*力
obj.accel-=obj.invmass*力
"""
当以下循环在碰撞之后运行时，效果非常好
但是为了适当地增加（完全）摩擦力，我想知道施加力后的速度，也就是加速度
（我或许可以做其他事情，但它更复杂，也可能不起作用……）
"""
#改变加速速度
对于self.objects中的obj：
目标加速（dt）
#应用碰撞并改变速度
self.findCollisions（dt）
#移动对象
对于self.objects中的obj：
目标移动（dt）
如果显示：
self.display（）
自身时间+=dt
def播放（self，dt=0.0001，总时间=5，获取能量=False）：#播放模拟（dt为时间步长）
realtime=time.time（）
启动时间=实时
上次显示=实时
自启动时间=0.1时：
显示=真
上次显示=time.time（）
#下一步
self.makeStep（dt，显示）
def findCollisions（self，dt）：#查找所有碰撞，从getCollision获取法向量并调用resolveCollision
n=len（self.objects）
对于范围（n）中的i：
o2=自我对象[i]
接头=氧气接头
对于范围（i）内的j:#j 0:#真实碰撞：
e=1
系数=（1+e）*正常速度
o1.速度+=系数*（o1.惯性质量/总惯性质量）*正常
o2.速度+=-coef*（o2.进气质量/总进气质量）*正常
如果0.001 0:#碰撞
返回深度*规格化（增量）
返回np.0（2）
班级楼层（包厢）：
定义初始化（self，y，xmin=-500，xmax=500）：
self.invmass=0#无限质量
self.y=y
self.hwidth=（xmax xmin）/2
self.hhheight=50
self.dimensions=np.array（[self.hwidth，self.hheight]）
self.center=np.array（[（xmin+xmax）/2，y-50]）
self.type=“box”
自加速=np.零（2）
自速度=np.零（2）
self.joints=[]
self.cls=“”
##力与接头
类弹簧接头：
定义初始值（自、对象、k、l0、阻尼=10、偏移量=[0,0]）：
self.objId=objId
self.l0=l0
self.k=k
self.offset=np.array（offset）
自阻尼器=阻尼器
def getForce（自身、氧气、氧气）：
增量=氧中心-（氧中心+自偏移）
正常=正常化（增量）
差值=增量-自身l0*正常
delta_速度=o2.speed-o1.speed
返回自k*diff+self.damper*delta_speed@normal*正常的
##对象定义
#带弹簧的测试车轮：生成“车轮”模型
def getWheel（半径、内半径、内质量、外质量、kr、ks、x=0、y=0.5、n=14、初始速度=[0,0]）：
弧=2*m.pi*半径/n
r=0.35*弧度
l0s=2*（半径-r）*m.sin（m.pi/n）
R=内侧-R
l0r=半径-r
核心=磁盘（IntMass，[x，y]，R，initspeed=initspeed）
轮胎=[]
对于范围（n）内的k：
a=k/n*2*m.pi
轮胎。附加（圆盘（外质量/n，[x+l0r*m.cos（a），y+l0r*m.sin（a）]，r，接头=[SpringJoint（0，kr，l0r），SpringJoint（k%n，ks，l0s）]，cls=“nc2”））
#除弹簧接头外，外部的制动盘不会相互作用
轮胎[-1]。接头