
    h/                         S r SSKrSSKrSSKrSSKJr   SSKJrJ	r	J
r
  SrS\-  \-  rS\-  \-  rS	\-  \-  rS
rSr/ SQr/ SQr/ SQr/ SQr/ SQrSrSrSr " S S5      rg! \ a  r\" S5      \eSrCff = f)z
Top-down car dynamics simulation.

Some ideas are taken from this great tutorial http://www.iforce2d.net/b2dtut/top-down-car by Chris Campbell.
This simulation is a bit more detailed, with wheels rotation.

Created by Oleg Klimov
    N)DependencyNotInstalled)
fixtureDefpolygonShaperevoluteJointDefzqBox2D is not installed, you can install it by run `pip install swig` followed by `pip install "gymnasium[box2d]"`g{Gz?i i  i@B       ))P   )7   r
   )r	   )r   r   ))   )<   r   )r   n   )r   r   ))ix   )   r   )   r   )r   ))   r   )2   )r   )r   )r   r   )r   )r   r   )ir   ))r   )r   r   )r   r   )r   r   )r   r   r   )M   r   r   )f   r   r   c                   H    \ rS rSrS rS rS rS rS rSS jr	S r
S	 rS
rg)Car5   c                    Xl         U R                   R                  X44U[        [        [         VVs/ s H  u  pVU[
        -  U[
        -  4PM     snnS9SS9[        [        [         VVs/ s H  u  pVU[
        -  U[
        -  4PM     snnS9SS9[        [        [         VVs/ s H  u  pVU[
        -  U[
        -  4PM     snnS9SS9[        [        [         VVs/ s H  u  pVU[
        -  U[
        -  4PM     snnS9SS9/S9U l	        SU R                  l
        / U l        SU l        [        * [        74[        7[        74[        7[        * 4[        * [        * 4/n[         GHU  u  pU	S:  a  SOSn
U R                   R                  X8[
        -  -   XI[
        -  -   4U[        [        U VVs/ s H  u  pVXZ-  [
        -  Xj-  [
        -  4PM     snnS9SS	S
SS9S9nU
[        -  [
        -  Ul        ["        Ul
        SUl        SUl        SUl        SUl        SUl        S Ul        S Ul        [3        U R                  UU[
        -  U	[
        -  4SSSS[
        -  [
        -  SSSS9
nU R                   R5                  U5      Ul        [9        5       Ul        Xl        U R                  R?                  U5        GMX     U R                  U R                  /-   U l         / U l!        g s  snnf s  snnf s  snnf s  snnf s  snnf )N)verticesg      ?)shapedensity)positionanglefixtures)g?        r(   r(   r   皙?       )r#   r$   categoryBitsmaskBitsrestitution)r   r   Tix gٿg?)
bodyAbodyBlocalAnchorAlocalAnchorBenableMotorenableLimitmaxMotorTorque
motorSpeed
lowerAngle
upperAngle)"worldCreateDynamicBodyr   r   
HULL_POLY1SIZE
HULL_POLY2
HULL_POLY3
HULL_POLY4hullcolorwheels
fuel_spentWHEEL_WWHEEL_RWHEELPOS	wheel_radWHEEL_COLORgasbrakesteerphaseomega
skid_startskid_particler   CreateJointjointsettilesuserDataappenddrawlist	particles)selfr9   
init_angleinit_xinit_yxy
WHEEL_POLYwxwyfront_kwrjds                [/home/james-whalen/.local/lib/python3.13/site-packages/gymnasium/envs/box2d/car_dynamics.py__init__Car.__init__6   s   $)
"&**">">%&CM!N:411t8QX"6:!N  	 &CM!N:411t8QX"6:!N  	 &CM!N:411t8QX"6:!N  	 &CM!N:411t8QX"6:!N  	' #? #
	< *		Xx Xx Xx Xx 	

 hFB!VcG

,, 9,fDy.@A #& )3"(2 [4/t1CD(2"  !'" # - A  "G+d2AK!AGAEAGAGAGAGAL"AO"ii 4id3#  (4/$6C jj,,S1AGeAGJKKq!U V tyyk1[ "O "O "O "O,"s   K(+K.$K4K:1"L c                     [         R                  " USS5      nU R                  SS  H.  nXR                  -
  nUS:  a  SnU=R                  U-  sl        M0     g)zkcontrol: rear wheel drive

Args:
    gas (float): How much gas gets applied. Gets clipped between 0 and 1.
r   r+         r)   N)npcliprB   rI   )rX   rI   rb   diffs       rd   rI   Car.gas   sO     ggc1a Qq!A;DczEETME	 "    c                 6    U R                    H	  nXl        M     g)z}control: brake

Args:
    b (0..1): Degree to which the brakes are applied. More than 0.9 blocks the wheels to zero rotation
N)rB   rJ   )rX   brb   s      rd   rJ   	Car.brake   s     AG rn   c                 P    XR                   S   l        XR                   S   l        g)zocontrol: steer

Args:
    s (-1..1): target position, it takes time to rotate steering wheel from side-to-side
r   r+   N)rB   rK   )rX   ss     rd   rK   	Car.steer   s       !A Arn   c                    U R                    GHh  n[        R                  " UR                  UR                  R
                  -
  5      n[        UR                  UR                  R
                  -
  5      nU[        SU-  S5      -  UR                  l        Sn[        S-  nUR                   H"  n[        U[        UR                  -  5      nSnM$     UR                  S5      nUR                  S5      n	UR                  n
US   U
S   -  US	   U
S	   -  -   nU	S   U
S   -  U	S	   U
S	   -  -   nU=R                  U[         -  UR"                  -  [$        -  [        UR                  5      S
-   -  -  sl        U =R&                  U[         -  UR"                  -  -  sl        UR(                  S:  a  SUl        OUR(                  S:  a  Sn[        R                  " UR                  5      * nXR(                  -  n[        U5      [        UR                  5      :  a  [        UR                  5      nU=R                  X4-  -  sl        U=R*                  UR                  U-  -  sl        UR                  UR,                  -  nU* U-   nU* nUS[.        -  [.        -  -  nUS[.        -  [.        -  -  n[        R0                  " [        R2                  " U5      [        R2                  " U5      -   5      n[        U5      SU-  :  a  UR4                  (       a  UR4                  R6                  U:X  ae  [9        UR4                  R:                  5      S:  aB  UR4                  R:                  R=                  UR>                  S   UR>                  S	   45        OaUR@                  c  UR>                  Ul         OBU RC                  UR@                  UR>                  U5      Ul        S Ul         OS Ul         S Ul        [        U5      U:  a  UU-  nUU-  nUnUU-  nUU-  nU=R                  X-  UR,                  -  [$        -  -  sl        URE                  UU	S   -  XS   -  -   UU	S	   -  XS	   -  -   4S5        GMk     g )Ng      I@g      @Tg333333?F)r   r+   )r+   r   r   r+   g      @g?r   i  g       @   )#rB   rj   signrK   rQ   r&   absminr6   FRICTION_LIMITrS   maxroad_frictionGetWorldVectorlinearVelocityrM   ENGINE_POWERrI   WHEEL_MOMENT_OF_INERTIArC   rJ   rL   rG   r<   sqrtsquarerO   grasslenpolyrU   r%   rN   _create_particleApplyForceToCenter)rX   dtrb   dirvalr   friction_limittileforwsidevvfvsBRAKE_FORCEvrf_forcep_forceforces                     rd   stepCar.step   s   A''!''AGGMM12Cagg-.C!$s4#:s';!;AGG E+c1N!$"NT5G5G$G" 	   ##F+D##F+D  Aa1Q4$q'AaD.0Ba1Q4$q'AaD.0B GG%% ** qww<#%	'G OOrL015588Oww#~1 wwqww''!GG+s8c!''l*agg,C39$GGqww|#G1;;&BcBhGcG v}t++Gv}t++GGGBIIg.71CCDE 5zC.00OO--6AOO001B6OO((//A

10NO\\)#$::AL&*&;&;ajj%'AO $(AL#"&5zN*5 5 &5 5 GGr|akk14KKKG  d1g%q'(99d1g%q'(99 o rn   c           	         SS K nU(       a  U R                   H  nUR                   Vs/ s H,  oR                  R	                  U5      R                  U5      PM.     n	nU	 V
s/ s H  n
U
S   U-  US   -   U
S   U-  US   -   4PM!     n	n
UR                  R                  XR                  U	SSS9  M     U R                   GH  nUR                   GH  nUR                  R                  nUR                  R                   Vs/ s H  oU-  PM	     nnU V
s/ s H  oS   U
S   4PM     nn
U Vs/ s H,  oR                  R	                  U5      R                  U5      PM.     nnU V
s/ s H  n
U
S   U-  US   -   U
S   U-  US   -   4PM!     nn
UR                   Vs/ s H  n[        US-  5      PM     nnUR                  R!                  UUUS9  SUR"                  ;  a  GM  UR$                  nUR$                  S	-   n[        R&                  " U5      n[        R&                  " U5      n[        R(                  " U5      n[        R(                  " U5      nUS:  a	  US:  a  GM  US:  a  [*        R,                  " U5      nUS:  a  [*        R,                  " U5      n[.        * [0        -  [2        7U-  [0        -  4[.        7[0        -  [2        7U-  [0        -  4[.        7[0        -  [2        7U-  [0        -  4[.        * [0        -  [2        7U-  [0        -  4/nU Vs/ s H  oU-  PM	     nnU V
s/ s H  oS   U
S   4PM     nn
U Vs/ s H,  oR                  R	                  U5      R                  U5      PM.     nnU V
s/ s H  n
U
S   U-  US   -   U
S   U-  US   -   4PM!     nn
UR                  R!                  U[4        US9  GM     GM     g s  snf s  sn
f s  snf s  sn
f s  snf s  sn
f s  snf s  snf s  sn
f s  snf s  sn
f )
Nr   r+   rh   F)rA   pointswidthclosed   )rA   r   rL   g333333?)pygame.drawrW   r   mathVector2
rotate_raddrawlinesrA   rV   r'   body	transformr#   r"   intpolygon__dict__rL   sincosrj   rw   rD   r<   rE   WHEEL_WHITE)rX   surfacezoomtranslationr&   draw_particlespygamepcr   coordsobjftransr   pathrA   a1a2s1s2c1c2
white_polys                           rd   r   Car.draw  s   ^^JK&&Q&Q++A.99%@&Q #'
 #' q	D(;q>9q	D(;q>9 #'   !!774q "  $ ==C\\((+,77+;+;<+;a	+;<=ABT6F1I.TBJNO$Q++A.99%@$O #'
 #' q	D(;q>9q	D(;q>9 #'   03yy9y!QWy9##G5#F#,,.YYYY_XXb\XXb\XXb\XXb\6b1f6B6BX_whmd&:;X_whmd&:;X_whmd&:;X_whmd&:;	
 2<<Aai
<CMN:ay&)4:
NFPFPKK''*55e<j   #-
 #- q	D(;q>9q	D(;q>9 #-   ##G;z#Rc " ! R =BO :0 =NsA   3O"&OO O%/3O*(&O/O4O92O>3P&Pc                 x    " S S5      nU" 5       nU(       d  [         O[        Ul        SUl        US   US   4US   US   4/Ul        X5l        U R                  R                  U5        [        U R                  5      S:  a6  U R                  R                  S5        [        U R                  5      S:  a  M6  U$ )Nc                       \ rS rSrSrg)&Car._create_particle.<locals>.ParticleiR   N)__name__
__module____qualname____firstlineno____static_attributes__r   rn   rd   Particler   R  s    rn   r   r+   r   rv   )
rH   	MUD_COLORrA   ttlr   r   rW   rU   r   pop)rX   point1point2r   r   r   s         rd   r   Car._create_particleQ  s    	 	 J%*+	!9fQi(6!9fQi*@Aa $..!B&NNq! $..!B&rn   c                     U R                   R                  U R                  5        S U l        U R                   H  nU R                   R                  U5        M      / U l        g )N)r9   DestroyBodyr@   rB   )rX   rb   s     rd   destroyCar.destroy_  sF    

tyy)	AJJ""1% rn   )rV   rC   r@   rW   rB   r9   N)T)r   r   r   r   re   rI   rJ   rK   r   r   r   r   r   r   rn   rd   r   r   5   s1    Un!^@CSJrn   r   )__doc__r   Box2Dnumpyrj   gymnasium.errorr   Box2D.b2r   r   r   ImportErrorer<   r   r   rz   rE   rD   rF   r;   r=   r>   r?   rH   r   r   r   r   rn   rd   <module>r      s       2CC 4$&+, dNT  
;A
>
	
 @
	o oC  
 {s   
A# #A7)	A22A7