Marco Monster Car Physics

Monkey Forums/Monkey Code/Marco Monster Car Physics

Jesse (Posted 2014) [#1]

I had this code in my computer. Maybe somebody will find it useful. This code was originally converted to BMax by Jim Brown with a little help by me. it's mostly his conversion but my contribution is the conversion To Monkey-x ;) Strict Import mojo ' Run the game ' -------------- Function Main:Int( ) New Game Return 1 End Function Class Game Extends App Field quit:Int Field car:Car Field carClass:CarClass Field lastticks:Int Field lastiticks:Int Method OnCreate:Int() car = New Car car.Init() carClass = New CarClass carClass.Init car.Init carClass lastticks =0 lastiticks = 0 scale = 28 ' PIxels per m SetUpdateRate 60 Return 1 End Method Method OnUpdate:Int() If KeyHit(KEY_F9) ' F9 To reset car.Init carClass Trail.num_trail=0 Endif If KeyHit(KEY_F4) Then DebugStop ' Zoom in / out If KeyDown(KEY_Q) scale += 0.5 If KeyDown(KEY_W) scale -= 0.5 car.ControlViaKeys car.UpdatePhysics Return 1 End Method Method OnRender:Int() Cls(150,190,150) car.Render Draw_Dials car Draw_TextMessages car Return 1 End Method End Class #Rem Car Physics Demo version 0.8 3-06-2001 Copyright (c) 2001 Monstrous Software Platforms: Allegro/DJGPP, Allegro/Linux, Allegro/MSVC Demonstrates rough approximation of car physics Added: Ability to drive in reverse - JimB #End Rem Const TRAIL_SIZE:Int = 200 '/* number of dots in car trail */ Const DELTA_T:Float = 0.01 '/* time between integration steps in physics modelling */ Const INPUT_DELTA_T:Float = 0.1 '/* Delay between keyboard polls */ Const SCREEN_W:Int = 640 Const SCREEN_H:Int = 480 Const TODEG:Float = 180.0 / PI Const TORAD:Float = PI/180.0 Class Vec2 Field x:Float,y:Float End Class Class CarClass Field wheelbase:Float ' wheelbase in m Field b:Float ' in m, distance from CG to front axle Field c:Float ' in m, idem to rear axle Field h:Float ' in m, height of CM from ground Field mass:Float ' in kg Field inertia:Float ' in kg.m Field length:Float,width:Float Field wheellength:Float,wheelwidth:Float ' Method Init:Void() b = 1.0 ' m c = 1.0 ' m wheelbase = b+c h = 1.0 ' m mass = 1750 ' kg inertia = 1204 ' kg.m width = 1.5 ' m length = 3.0 ' m, must be > wheelbase wheellength = 0.7 wheelwidth = 0.3 End Method End Class Class Car Field carClass:CarClass ' pointer to static car data Field position_wc:Vec2 ' position of car centre in world coordinates Field velocity_wc:Vec2 ' velocity vector of car in world coordinates Field angle:Float ' angle of car body orientation (in rads) Field angularvelocity:Float Field steerangle:Float ' angle of steering (Input) Field throttle:Float ' amount of throttle (Input) Field brake:Float ' amount of braking (Input) Field gear:Int ' gear position 1=FORWARD -1=REVERSE ' Method Init:Void() position_wc = New Vec2 velocity_wc = New Vec2 End Method ' Method Init:Void(thiscarClass:CarClass) carClass = thiscarClass gear=1 ' weight per axle = half car mass times 1G (=9.8m/s^2) weight = carClass.mass * 9.8 * 0.5 ' position_wc.x=0 ; position_wc.y=0 velocity_wc.x=0 ; velocity_wc.y=0 angle=0 ; angularvelocity=0 steerangle=0 ; throttle=0 ; brake=0 End Method ' Method ControlViaKeys:void() ' acceleration / throttle If KeyDown(KEY_UP) If throttle<100 Then throttle+=1 brake=0 ElseIf KeyDown(KEY_DOWN) If brake<98.8 Then brake+=1.03 throttle=0 Else If throttle>0 Then throttle-=0.8 If brake<99.88 Then brake+=0.18 EndIf ' Steering If KeyDown(KEY_LEFT) If steerangle>-PI/4.0 Then steerangle-=PI/184.0 Else If KeyDown(KEY_RIGHT) If steerangle<PI/4.0 Then steerangle+=PI/184.0 Else steerangle/=1.04 EndIf ' gears If Abs(velocity.x) <= 0.1 If KeyDown(KEY_A) Then gear=1 ' forward If KeyDown(KEY_Z) Then gear=-1 ' reverse EndIf ' Let front, rear Or both axles slip rear_slip=0 ; front_slip=0 If KeyDown(KEY_R) rear_slip=1 If KeyDown(KEY_F) front_slip=1 If KeyHit(KEY_T) velocity_wc.x+=10 ' quick boost If KeyDown(KEY_SPACE) front_slip=1 ; rear_slip=1 EndIf End Method ' Method UpdatePhysics:void() sn = Sin(angle*TODEG) cs = Cos(angle*TODEG) ' SAE convention: x is to the front of the car, y is to the right, z is down ' transform velocity in world reference frame to velocity in car reference frame velocity.x = cs * velocity_wc.y + sn * velocity_wc.x velocity.y = -sn * velocity_wc.y + cs * velocity_wc.x ' Lateral force on wheels ' ' Resulting velocity of the wheels as result of the yaw rate of the car body ' v = yawrate * r where r is distance of wheel To CG (approx. half wheel base) ' yawrate (ang.velocity) must be in rad/s ' yawspeed = carClass.wheelbase * 0.5 * angularvelocity 'bangz: velocity.x = fVLong_, velocity.y = fVLat_ ' Calculate the side slip angle of the car (a.k.a. beta) If Abs(velocity.x)<=0.05 sideslip = 0 ; slipanglefront=0 ; slipanglerear=0 yawspeed=0 ; angularvelocity=0 Else rot_angle = ATan2( yawspeed, velocity.x)*TORAD sideslip = ATan2( velocity.y, velocity.x)*TORAD ' Calculate slip angles For front And rear wheels (a.k.a. Alpha) slipanglefront = sideslip + rot_angle - steerangle slipanglerear = sideslip - rot_angle EndIf ' lateral force on front wheels = (Ca * slip angle) capped To friction circle * Load flatf.x = 0 flatf.y = CA_F * slipanglefront flatf.y = Min(MAX_GRIP, flatf.y) flatf.y = Max(-MAX_GRIP, flatf.y) flatf.y *= weight If(front_slip) flatf.y *= 0.5 ' lateral force on rear wheels flatr.x = 0 flatr.y = CA_R * slipanglerear flatr.y = Min(MAX_GRIP, flatr.y) flatr.y = Max(-MAX_GRIP, flatr.y) flatr.y *= weight If(rear_slip) flatr.y *= 0.5 ' longtitudinal force on rear wheels - very simple traction model ftraction.x = 60.0*((throttle*3.5) - brake*Sgn(velocity.x)) ftraction.y = 0 If rear_slip Then ftraction.x *= 0.5 ' Forces and torque on body ' ========================================== ' drag and rolling resist resist.x = -( RESISTANCE*velocity.x + DRAG*velocity.x*Abs(velocity.x) ) resist.y = -( RESISTANCE*velocity.y + DRAG*velocity.y*Abs(velocity.y) ) ' sum forces force.x = ftraction.x + Sin(steerangle*TODEG) * flatf.x + flatr.x + resist.x force.y = ftraction.y + Cos(steerangle*TODEG) * flatf.y + flatr.y + resist.y ' torque on body from lateral forces torque = carClass.b * flatf.y - carClass.c * flatr.y ' ========================================== ' Acceleration ' ========================================== ' Newton F = m.a, therefore a = F/m acceleration.x = force.x/carClass.mass acceleration.y = force.y/carClass.mass angular_acceleration = torque / carClass.inertia ' ========================================== ' Velocity and position ' ========================================== ' transform acceleration from car reference frame to world reference frame acceleration_wc.x = cs * acceleration.y + sn * acceleration.x acceleration_wc.y = -sn * acceleration.y + cs * acceleration.x ' velocity is integrated acceleration velocity_wc.x+= DELTA_T * acceleration_wc.x velocity_wc.y+= DELTA_T * acceleration_wc.y ' fix continous side slipPIng bug Local veltotal:Float=(Abs(velocity_wc.x)+Abs(velocity_wc.y)) If Int(throttle)=0 And veltotal>0.0 velocity_wc.x/=1.025 velocity_wc.y/=1.025 EndIf ' zero out the velocities when car has almost stopped If Abs(throttle)=0 If Abs(velocity_wc.x)<0.08 Then velocity_wc.x=0 If Abs(velocity_wc.y)<0.08 Then velocity_wc.y=0 EndIf ' position is integrated velocity If gear>0 position_wc.x += DELTA_T * velocity_wc.x position_wc.y += DELTA_T * velocity_wc.y ElseIf gear=-1 position_wc.x -= DELTA_T * velocity_wc.x position_wc.y -= DELTA_T * velocity_wc.y EndIf ' ========================================== ' Angular velocity and heading ' ========================================== ' integrate angular acceleration to get angular velocity angularvelocity += DELTA_T * angular_acceleration ' integrate angular velocity to get angular orientation If gear>0 angle+= DELTA_T * angularvelocity ElseIf gear=-1 angle-= DELTA_T * angularvelocity EndIf Trail.Add position_wc.x,position_wc.y End Method ' Method Render:void() Local corners:Vec2[4] , wheels:Vec2[4] Local w:Vec2[4] Local sn:Float, cs:Float Local i:Int , y:Int ' sn = Sin(angle*TODEG) ; cs = Cos(angle*TODEG) ' screen_pos.x = position_wc.x * scale + SCREEN_W/2 screen_pos.y = -position_wc.y * scale + SCREEN_H/2 ' While(screen_pos.y < 0) screen_pos.y +=SCREEN_H Wend While(screen_pos.y>SCREEN_H) screen_pos.y-=SCREEN_H Wend While(screen_pos.x < 0) screen_pos.x +=SCREEN_W Wend While(screen_pos.x > SCREEN_W) screen_pos.x -=SCREEN_W Wend Trail.Draw Self ' ' Draw car body ' SetColor 160,0,0 ' wheels: 0=fr Left, 1=fr Right, 2 =rear Right, 3=rear Left corners[0] = New Vec2 corners[0].x = -carClass.width/2 corners[0].y = -carClass.length/2 corners[1] = New Vec2 corners[1].x = carClass.width/2 corners[1].y = -carClass.length/2 corners[2] = New Vec2 corners[2].x = carClass.width/2 corners[2].y = carClass.length/2 corners[3] = New Vec2 corners[3].x = -carClass.width/2 corners[3].y = carClass.length/2 For i = 0 To 3 w[i] = New Vec2 w[i].x = cs*corners[i].x - sn*corners[i].y w[i].y = sn*corners[i].x + cs*corners[i].y corners[i].x = w[i].x ; corners[i].y = w[i].y Next For i = 0 To 3 corners[i].x *= scale ; corners[i].y *= scale corners[i].x += screen_pos.x ; corners[i].y += screen_pos.y Next DrawLine corners[0].x, corners[0].y, corners[1].x, corners[1].y DrawLine corners[1].x, corners[1].y, corners[2].x, corners[2].y DrawLine corners[2].x, corners[2].y, corners[3].x, corners[3].y DrawLine corners[3].x, corners[3].y, corners[0].x, corners[0].y ' ' Draw wheels ' SetColor 0,0,160 ' wheels: 0=fr Left, 1=fr Right, 2 =rear Right, 3=rear Left wheels[0] = New Vec2 wheels[0].x = -carClass.width/2 wheels[0].y = -carClass.b wheels[1] = New Vec2 wheels[1].x = carClass.width/2 wheels[1].y = -carClass.b wheels[2] = New Vec2 wheels[2].x = carClass.width/2 wheels[2].y = carClass.c wheels[3] = New Vec2 wheels[3].x = -carClass.width/2 wheels[3].y = carClass.c ' For i = 0 To 3 w[i].x = cs*wheels[i].x - sn*wheels[i].y w[i].y = sn*wheels[i].x + cs*wheels[i].y wheels[i].x = w[i].x wheels[i].y = w[i].y Next ' For i = 0 To 3 wheels[i].x *= scale wheels[i].y *= scale wheels[i].x += screen_pos.x wheels[i].y += screen_pos.y Next ' draw_wheel( 0, wheels[0].x, wheels[0].y, front_slip) draw_wheel( 1, wheels[1].x, wheels[1].y, front_slip) draw_wheel( 2, wheels[2].x, wheels[2].y, rear_slip) draw_wheel( 3, wheels[3].x, wheels[3].y, rear_slip) #Rem ' "wheel spokes" To show Ackermann centre of turn ' DrawLine wheels[0].x, wheels[0].y, wheels[0].x -Cos(angle+steerangle)*100, wheels[0].y -Sin(angle+steerangle)*100,col); DrawLine wheels[3].x, wheels[3].y, wheels[3].x -Cos(angle)*100, wheels[3].y -Sin(angle)*100,col); #End Rem End Method ' Method draw_wheel:Void(nr:Int,x:Int,y:Int,crossed:Int) Local r:Float SetColor 0,160,0 If nr<2 Then r=steerangle Else r=0 draw_rect angle+r,carClass.wheelwidth*scale, carClass.wheellength*scale,x,y,crossed End Method End class Class TrailPoint Field x:Float,y:Float,angle:Float End Class Class Trail Global num_trail:Int Global tp:TrailPoint[TRAIL_SIZE] ' Function Add:void(x:Float,y:Float) If num_trail<TRAIL_SIZE-1 If tp[num_trail] = Null Then tp[num_trail]=New TrailPoint tp[num_trail].x=x tp[num_trail].y=y num_trail+=1 Else For Local i:Int = 0 Until TRAIL_SIZE-2 tp[i].x=tp[i+1].x tp[i].y=tp[i+1].y Next tp[num_trail-1].x=x tp[num_trail-1].y=y EndIf End Function ' Function Draw:Void(thiscar:Car) Local x:Int,y:Int SetColor 230,230,230 For Local i:Int=0 Until num_trail x=(tp[i].x-thiscar.position_wc.x)*scale+screen_pos.x y=-(tp[i].y-thiscar.position_wc.y)*scale+screen_pos.y DrawCircle x, y, 2 Next End Function End Class Global screen_pos:Vec2 = New Vec2 Global scale:Float Global str:String #Rem Lots of globals, so their value may be printed on screen Normally, most of these variables should be private to the physics function #End Rem Global velocity:Vec2 = New Vec2 Global acceleration_wc:Vec2 = New Vec2 Global rot_angle:Float Global sideslip:Float Global slipanglefront:Float Global slipanglerear:Float Global force:Vec2 = New Vec2 Global rear_slip:Int Global front_slip:Int Global resist:Vec2 = New Vec2 Global acceleration:Vec2 = New Vec2 Global torque:Float Global angular_acceleration:Float Global sn:Float, cs:Float Global yawspeed:Float Global weight:Float Global ftraction:Vec2 = New Vec2 Global flatf:Vec2 = New Vec2, flatr:Vec2 = New Vec2 '################################################# ' These constants are arbitrary values, not realistic ones Const DRAG:Float = 5.0 ' factor For air resist (drag) Const RESISTANCE:Float = 30.0 ' factor For rolling resist Const CA_R:Float=-5.20 ' cornering stiffness Const CA_F:Float=-5.0 ' cornering stiffness Const MAX_GRIP:Float =4.0 ' maximum (normalised) friction force, =diameter of friction circle ' ####################################################### Function draw_rect:void(angle:Float, w:Int,l:Int,x:Int,y:Int,crossed:Int) Local c:Vec2[4],c2:Vec2[4] Local sn:Float,cs:Float Local i:Int sn=Sin(angle*TODEG) ; cs=Cos(angle*TODEG) c[0]=New Vec2 c[0].x =-w/2 ; c[0].y=l/2 c[1] = New Vec2 c[1].x = w/2 ; c[1].y=l/2 c[2] = New Vec2 c[2].x = w/2 ; c[2].y=-l/2 c[3] = New Vec2 c[3].x = -w/2 ; c[3].y=-l/2 ' For i = 0 To 3 c2[i]=New Vec2 c2[i].x=cs*c[i].x-sn*c[i].y c2[i].y=sn*c[i].x+cs*c[i].y c[i].x=c2[i].x ; c[i].y=c2[i].y Next ' For i = 0 To 3 c[i].x+=x ; c[i].y+=y Next DrawLine c[0].x,c[0].y, c[1].x, c[1].y DrawLine c[1].x,c[1].y, c[2].x, c[2].y DrawLine c[2].x,c[2].y, c[3].x, c[3].y DrawLine c[3].x,c[3].y, c[0].x, c[0].y ' If crossed DrawLine c[0].x,c[0].y,c[2].x,c[2].y DrawLine c[1].x,c[1].y,c[3].x,c[3].y EndIf End Function Function Draw_Dials:Void(thiscar:Car) SetColor 0, 100,100 ' Velocity vector dial ' Const VDIAL_X:Int = 550 Const VDIAL_Y:Int = 120 DrawCircle VDIAL_X, VDIAL_Y, 50 DrawLine VDIAL_X, VDIAL_Y, VDIAL_X+velocity.x, VDIAL_Y-velocity.y ' Const VWDIAL_X:Int = 550 Const VWDIAL_Y:Int = 260 DrawCircle VWDIAL_X, VWDIAL_Y, 50 SetColor 0, 0, 100 DrawLine VWDIAL_X, VWDIAL_Y, VWDIAL_X+thiscar.velocity_wc.x, VWDIAL_Y-thiscar.velocity_wc.y ' Const BRAKE_X:Int = 420 Const BRAKE_Y:Int = 120 SetColor 70, 0, 0 DrawRect BRAKE_X, BRAKE_Y-100, 20, 100 SetColor 120, 0,0 DrawRect BRAKE_X+2, BRAKE_Y-thiscar.brake, 16, thiscar.brake ' Const THROTTLE_X:Int = 440 Const THROTTLE_Y:Int = 120 SetColor 0, 70, 0 DrawRect THROTTLE_X, THROTTLE_Y-100, 20, 100 SetColor 0, 120,0 DrawRect THROTTLE_X+2, THROTTLE_Y-thiscar.throttle, 16, thiscar.throttle ' Const GEAR_X:Int = 400 Const GEAR_Y:Int = 40 SetColor 70, 70, 70 DrawLine GEAR_X,GEAR_Y,GEAR_X,GEAR_Y+40 SetColor 0, 0, 0 DrawRect GEAR_X-10,GEAR_Y+20-7-thiscar.gear*18,20,14 ' DrawArcDial "Steer Angle",420,160,thiscar.steerangle DrawArcDial "Side Slip",420,220,sideslip DrawArcDial "Rot Angle",420,280,rot_angle DrawArcDial "SlipAng F",450,360,slipanglefront DrawArcDial "SlipAng R",450,420,slipanglerear End Function Function Draw_TextMessages:Void(thiscar:Car) ' text messages Const TEXT_X:Int =10 SetColor 100, 100, 100 Local y%=10 str = "scale "+scale+" PIxels/m <Q,W>" DrawText str, TEXT_X,y y+=10 str = "velocity x "+velocity.x DrawText str, TEXT_X,y y+=10 str = "sideslip "+sideslip DrawText str, TEXT_X,y y+=10 str = "vel_wc x "+thiscar.velocity_wc.x DrawText str, TEXT_X,y y+=10 str = "vel_wc y "+thiscar.velocity_wc.y DrawText str, TEXT_X,y y+=10 str = "angle "+thiscar.angle*TODEG DrawText str, TEXT_X, y+10 y+=10 str = "ESC=quit Q/W=zoom UP=Throttle DOWN=Brakes A/Z=Gears SPACE=Handbrake" DrawText str, 0, SCREEN_H-20 End Function Function DrawArcDial:void(label$,xp%,yp%,param:Float) Local ncol% SetColor 0, 0, 0 DrawArc xp,yp,220,328,35 SetColor 0, 100, 0 DrawLine xp,yp,xp+Int(Sin(-param*TODEG)*30.0), yp-Int(Cos(-param*TODEG)*30.0) DrawText label,xp-TextWidth(label)/2,yp+4 End Function Function DrawCircle:void(xC%,yC%,radius%,filled:Int = False) If filled = True DrawOval xC - radius, yC - radius, radius * 2, radius * 2 Else If (xC-radius) > SCREEN_W Return If (yC-radius) > SCREEN_H Return If (xC+radius) < 0 Then Return If (yC+radius) < 0 Then Return Local x:Int = 0 Local d:Int = (2*radius) Local y:Int=radius While x<y If d < 0 Then d = d + (4 * x) + 6 Else d = d + 4 * (x - y) + 10 y = y - 1 End If DrawPoint(xC + x, yC + y) DrawPoint(xC + x, yC - y) DrawPoint(xC - x, yC + y) DrawPoint(xC - x, yC - y) DrawPoint(xC + y, yC + x) DrawPoint(xC + y, yC - x) DrawPoint(xC - y, yC + x) DrawPoint(xC - y, yC - x) x=x+1 Wend EndIf End Function Function DrawArc:void(x:Float,y:Float,startAngle:Float,endAngle:Float,radius:Float) If startAngle = endAngle Then Return If startAngle > endAngle Local ta:Float = endAngle endAngle = startAngle ; startAngle = ta EndIf Local angle:Float = endAngle - startAngle Local stp:Float = angle/(angle * PI / 180.0 * radius) Local accumAngle:Float = startAngle Local dx:Float = Cos(accumAngle) * radius Local dy:Float = Sin(accumAngle) * radius 'DrawLine x, y, x + dx, y + dy While accumAngle < (startAngle+angle) dx = Cos((accumAngle))*radius dy = Sin((accumAngle))*radius DrawCircle x+dx,y+dy,1 accumAngle += stp Wend 'DrawLine x, y, x + dx, y + dy End Function

slenkar (Posted 2014) [#2]

thats really cool