Graphics 640,480,32,2
SetBuffer BackBuffer()

;Variabeln
Global delta_t# = 0.01
Global M_PI# = 3.1415926
Global DRAG# = 5.0
Global RESISTANCE# = 30.0
Global CA_R# = -5.20
Global CA_F# = -5.0
Global MAX_GRIP# = 2.0

Global CAR_cartype = 1
Global CAR_cartype_b# = 1.0
Global CAR_cartype_c# = 1.0
Global CAR_cartype_wheelbase# = CAR_cartype_b# + CAR_cartype_c#
Global CAR_cartype_h# = 1.0
Global CAR_cartype_mass# = 1500
Global CAR_cartype_inertia# = 1500
Global CAR_cartype_width# = 1.5
Global CAR_cartype_length# = 3.0
Global CAR_cartype_wheellength# = 0.7
Global CAR_cartype_wheelwidth# = 0.3
Global CAR_car_position_wc_x# = 400.0
Global CAR_car_position_wc_y# = 300.0
Global CAR_car_velocity_wc_x# = 0
Global CAR_car_velocity_wc_y# = 0
Global CAR_car_angle# = 90
Global CAR_car_angularvelocity# = 0
Global CAR_car_steerangle# = 0
Global CAR_car_throttle# = 0
Global CAR_car_brake# = 0

Global front_slip = 0
Global rear_slip = 0

Global velocity_x#
Global velocity_y#
Global yawspeed#
Global sn#, cs#
Global xpos#, ypos#
Global rot_angle#
Global sideslip#
Global slipanglefront#
Global slipanglerear#
Global flatf_x#, flatf_y#
Global flatr_x#, flatr_y#
Global weight#
Global ftraction_x#, ftraction_y#
Global resistance_x#, resistance_y#
Global force_x#, force_y#
Global torque#
Global acceleration_x#, acceleration_y#, angular_acceleration#
Global acceleration_wc_x#, acceleration_wc_y#


While KeyHit(1)=0

    Cls

    if timer_inp + 100 < millisecs()
      FUNC_do_input()
         timer_inp = millisecs()
    endif
    if timer_phs + 10 < millisecs()
      FUNC_do_physics()
         timer_phs = millisecs()
    endif
   
    ;Anzeigen der Variabeln
    Text 0,0,Str(CAR_car_position_wc_x#)
    Text 0,12,Str(CAR_car_position_wc_y#)
    Text 0,24,Str(CAR_car_steerangle#)
    Text 0,36,"Winkel: " + str(CAR_car_angle#)
      Text 0,46,"Force.x: " + force_x#
      Text 0,56,"Force.y: " + force_y#
      Text 0,66,"Throttle: " + CAR_car_throttle#
      
    xpos# = CAR_car_position_wc_x#
    ypos# = CAR_car_position_wc_y#

    ;Auto zeichnen
    renda1# =( -CAR_car_angle# ) - 60
    If (renda1#<0.0) Then renda1#=360.0+renda1#
    renda2# =( -CAR_car_angle# ) + 60
    If (renda2#>359.0) Then renda2#=renda2#-360.0
    renda3# =( -CAR_car_angle# ) - 120
    If (renda3#<0.0) Then renda3#=360.0+renda3#
    renda4# =( -CAR_car_angle# ) + 120
    If (renda4#>359.0) Then renda4#=renda4#-360.0

    rendx1#=25*Cos(renda1#) : rendy1#=25*Sin(renda1#)
    rendx2#=25*Cos(renda2#) : rendy2#=25*Sin(renda2#)
    rendx3#=25*Cos(renda3#) : rendy3#=25*Sin(renda3#)
    rendx4#=25*Cos(renda4#) : rendy4#=25*Sin(renda4#)

    Color 255,255,255
    Line rendx1#+xpos#,rendy1#+ypos#,rendx2#+xpos#,rendy2#+ypos#
    Line rendx2#+xpos#,rendy2#+ypos#,rendx4#+xpos#,rendy4#+ypos#
    Line rendx4#+xpos#,rendy4#+ypos#,rendx3#+xpos#,rendy3#+ypos#
    Line rendx3#+xpos#,rendy3#+ypos#,rendx1#+xpos#,rendy1#+ypos#
    Flip 0

Wend


Function FUNC_do_input()

   if keydown(200) then; throttle up
   
      if CAR_car_throttle# < 100
         CAR_car_throttle#  = CAR_car_throttle#  + 10;
     Endif
   Endif   
   
   if keydown(208) then; throttle down
   
      if CAR_car_throttle# >= 10
         CAR_car_throttle#  = CAR_car_throttle#  - 10;
     Endif   
  Endif

   if keydown(57) then   ; brake   
      CAR_car_brake# = 100;
      CAR_car_throttle#  = 0;   
   else
      CAR_car_brake# = 0;
   Endif

   if keydown(203) then    
      if CAR_car_steerangle# > - M_PI/4.0 then
         CAR_car_steerangle# = CAR_car_steerangle# - M_PI/32.0;
     Endif   
   elseif keydown(205) then
      if CAR_car_steerangle#<  M_PI/4.0 then
         CAR_car_steerangle# = CAR_car_steerangle# + M_PI/32.0;
     Endif
   Endif
   
  rear_slip = 0;

End Function

Function FUNC_do_physics()

   sn# = sin(CAR_car_angle#);
   cs# = cos(CAR_car_angle#);

   ; 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# * CAR_car_velocity_wc_y# + sn# * CAR_car_velocity_wc_x#;
   velocity_y# = -sn# * CAR_car_velocity_wc_y# + cs# * CAR_car_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# = CAR_cartype_wheelbase# * 0.5 * CAR_car_angularvelocity#;   

   if velocity_x = 0 then      
      rot_angle# = 0;
   else
      rot_angle# = atan2( yawspeed#, velocity_x);
   endif
   
   ; Calculate the side slip angle of the car (a.k.a. beta)
   if velocity_x = 0 then
      sideslip = 0;
   else
      sideslip = atan2( velocity_y#, velocity_x# );      
   endif
   
   ; Calculate slip angles for front and rear wheels (a.k.a. alpha)
   slipanglefront# = sideslip# + rot_angle# - CAR_car_steerangle#;
   slipanglerear#  = sideslip# - rot_angle#

   ; weight per axle = half car mass times 1G (=9.8m/s^2)
   weight# = CAR_cartype_mass# * 9.8 * 0.5;   
   
   ; 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# = flatf_y# * weight#
   if front_slip = 1 then
      flatf_y = flatf_y * 0.5
   Endif

   ; 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 = flatr_y * weight#
   if rear_slip = 1 then
      flatr_y = flatr_y * 0.5
   Endif

   ; longtitudinal force on rear wheels - very simple traction model
   ftraction_x# = 100*(CAR_car_throttle# - CAR_car_brake#*SGN(velocity_x));   
   ftraction_y# = 0;
   if rear_slip = 1 then
      ftraction_x# = ftraction_x# * 0.5
   Endif

; Forces and torque on body
   
   ; drag and rolling resistance
   resistance_x# = -( RESISTANCE#*velocity_x# + DRAG#*velocity_x#*ABS(velocity_x) )
   resistance_y# = -( RESISTANCE#*velocity_y# + DRAG#*velocity_y#*ABS(velocity_y) )

   ; sum forces
   force_x# = ftraction_x# + sin(CAR_car_steerangle#) * flatf_x# + flatr_x# + resistance_x#;
   force_y# = ftraction_y# + cos(CAR_car_steerangle#) * flatf_y# + flatr_y# + resistance_y#;   

   ; torque on body from lateral forces
   torque# = CAR_cartype_b# * flatf_y# - CAR_cartype_c# * flatr_y;

; Acceleration
   
   ; Newton F = m.a, therefore a = F/m
   acceleration_x# = force_x# / CAR_cartype_mass#
   acceleration_y# = force_y# / CAR_cartype_mass#
   
   angular_acceleration# = torque# / CAR_cartype_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
   ;
   CAR_car_velocity_wc_x# = CAR_car_velocity_wc_x# + delta_t * acceleration_wc_x#;
   CAR_car_velocity_wc_y# = CAR_car_velocity_wc_y# + delta_t * acceleration_wc_y#;

   ; position is integrated velocity
   ;
   CAR_car_position_wc_x# = CAR_car_position_wc_x# + delta_t * CAR_car_velocity_wc_x#
   CAR_car_position_wc_y# = CAR_car_position_wc_y# + delta_t * CAR_car_velocity_wc_y#


; Angular velocity and heading

   ; integrate angular acceleration to get angular velocity
   ;
   CAR_car_angularvelocity# = CAR_car_angularvelocity# + delta_t * angular_acceleration#;

   ; integrate angular velocity to get angular orientation
   ;
   CAR_car_angle# =  CAR_car_angle# + delta_t * CAR_car_angularvelocity# ;
   
    If ( CAR_car_position_wc_x# < 0.0 ) CAR_car_position_wc_x# = 640.0
    If ( CAR_car_position_wc_x# > 640.0 ) CAR_car_position_wc_x# = 0.0
    If ( CAR_car_position_wc_y# < 0.0 ) CAR_car_position_wc_y# = 480.0
    If ( CAR_car_position_wc_y# > 480.0 ) CAR_car_position_wc_y# = 0.0

End Function

function max(wert1,wert2)
  if wert1 > wert2 then
     return wert1
   else
     return wert2
   endif
   
   if wert1 = wert2 then return wert1
End Function

function min(wert1,wert2)
  if wert1 < wert2 then
     return wert1
   else
     return wert2
   endif
   
   if wert1 = wert2 then return wert1
End Function