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

Question

I need to code a 2d physics engine (for the moment without rotation) with a model of a wheel (here : one non-rotating Disc with small discs attached to it with springs in a circle to simulate the tyre). It worked quite well until now (given that I choose a short enough time step), but now I have to add friction (it can be full friction : no relative speed between the tyre and the floor).

So when I'm computing the collisions, I want to know the speed BEFORE the acceleration due to forces. So instead of (Forces)>(Collisions)>(Change speed from acceleration)>(Update position), I used (Forces)>(Change speed from acceleration)>(Collisions)>(Update position). But then, no matter the time step, I have strange results, especially when colliding.

I could maybe have friction with the first order of steps, but it will be more complicated I guess.

In the code here, I tried to focus on the main things (but it's not THAT minimal either), so I removed friction for example, since the problem seems to be in the order of my steps.

In the tkinter window, there are several time steps available if you want to test (for example the first one completely fails).

Thanks in advance

PS : I know the springs are very strong (k = 1e7), that sould be a wheel.

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()

Answer 1

Edit: misunderstood the question.

Would it help to have the move step before the collision step? Movement should happen right after acceleration.

Try to calculate the acceleration before the collision in order to get frictional forces without ever applying it to the objects.

Answer 2

最终我保持了原来的顺序，并找到了另一种实现摩擦的方法，所以现在效果很好