/* Ċ Ġ* ĠCopyright Ġ( c ) Ġ 2 0 1 0 - 2 0 1 6 ĠW illiam ĠBit tle Ġ Ġhttp :// www . dyn 4 j . org / Ċ Ġ* ĠAll Ġrights Ġreserved . Ċ Ġ* ĠĊ Ġ* ĠRedistribution Ġand Ġuse Ġin Ġsource Ġand Ġbinary Ġforms , Ġwith Ġor Ġwithout Ġmodification , Ġare Ġpermitted ĠĊ Ġ* Ġprovided Ġthat Ġthe Ġfollowing Ġconditions Ġare Ġmet : Ċ Ġ* ĠĊ Ġ* ĠĠ Ġ* ĠRedistributions Ġof Ġsource Ġcode Ġmust Ġretain Ġthe Ġabove Ġcopyright Ġnotice , Ġthis Ġlist Ġof Ġconditions ĠĊ Ġ* ĠĠĠĠ Ġand Ġthe Ġfollowing Ġdisclaimer . Ċ Ġ* ĠĠ Ġ* ĠRedistributions Ġin Ġbinary Ġform Ġmust Ġreproduce Ġthe Ġabove Ġcopyright Ġnotice , Ġthis Ġlist Ġof Ġconditions ĠĊ Ġ* ĠĠĠĠ Ġand Ġthe Ġfollowing Ġdisclaimer Ġin Ġthe Ġdocumentation Ġand / or Ġother Ġmaterials Ġprovided Ġwith Ġthe ĠĊ Ġ* ĠĠĠĠ Ġdistribution . Ċ Ġ* ĠĠ Ġ* ĠNeither Ġthe Ġname Ġof Ġdyn 4 j Ġnor Ġthe Ġnames Ġof Ġits Ġcontributors Ġmay Ġbe Ġused Ġto Ġendorse Ġor ĠĊ Ġ* ĠĠĠĠ Ġpromote Ġproducts Ġderived Ġfrom Ġthis Ġsoftware Ġwithout Ġspecific Ġprior Ġwritten Ġpermission . Ċ Ġ* ĠĊ Ġ* ĠTHIS ĠSOFTWARE ĠIS ĠPROVIDED ĠBY ĠTHE ĠCOPYRIGHT ĠHOLDERS ĠAND ĠCONTRIBUTORS Ġ" AS ĠIS " ĠAND ĠANY ĠEXPRESS ĠOR ĠĊ Ġ* ĠIMPLIED ĠWARRANTIES , ĠINCLUDING , ĠBUT ĠNOT ĠLIMITED ĠTO , ĠTHE ĠIMPLIED ĠWARRANTIES ĠOF ĠMERCHANTABILITY ĠAND ĠĊ Ġ* ĠFITNESS ĠFOR ĠA ĠPARTICULAR ĠPURPOSE ĠARE ĠDISCLAIMED . ĠIN ĠNO ĠEVENT ĠSHALL ĠTHE ĠCOPYRIGHT ĠOWNER ĠOR ĠĊ Ġ* ĠCONTRIBUTORS ĠBE ĠLIABLE ĠFOR ĠANY ĠDIRECT , ĠINDIRECT , ĠINCIDENTAL , ĠSPECIAL , ĠEXEMPLARY , ĠOR ĠCONSEQUENTIAL ĠĊ Ġ* ĠDAMAGES Ġ( INCLUDING , ĠBUT ĠNOT ĠLIMITED ĠTO , ĠPROCUREMENT ĠOF ĠSUBSTITUTE ĠGOODS ĠOR ĠSERVICES ; ĠLOSS ĠOF ĠUSE , ĠĊ Ġ* ĠDATA , ĠOR ĠPROFITS ; ĠOR ĠBUSINESS ĠINTERRUPTION ) ĠHOWEVER ĠCAUSED ĠAND ĠON ĠANY ĠTHEORY ĠOF ĠLIABILITY , ĠWHETHER ĠĊ Ġ* ĠIN ĠCONTRACT , ĠSTRICT ĠLIABILITY , ĠOR ĠTORT Ġ( INCLUDING ĠNEGLIGENCE ĠOR ĠOTHERWISE ) ĠARISING ĠIN ĠANY ĠWAY ĠOUT ĠĊ Ġ* ĠOF ĠTHE ĠUSE ĠOF ĠTHIS ĠSOFTWARE , ĠEVEN ĠIF ĠADVISED ĠOF ĠTHE ĠPOSSIBILITY ĠOF ĠSUCH ĠDAMAGE . Ċ Ġ*/ Ċ package Ġorg . dyn 4 j . dynamics Ċ Ċ import Ġorg . dyn 4 j . Epsilon Ċ import Ġorg . dyn 4 j . collision . Coll isions . get Est imated Collision Pairs Ċ import Ġorg . dyn 4 j . dynamics . contact . Contact Constraint Ċ import Ġorg . dyn 4 j . dynamics . contact . Contact Constraint Solver Ċ import Ġorg . dyn 4 j . dynamics . joint . Joint Ċ import Ġorg . dyn 4 j . geometry . Interval Ċ import Ġorg . dyn 4 j . geometry . Vector 2 Ċ import Ġorg . dyn 4 j . resources . Messages Ċ import Ġorg . dyn 4 j . resources . message Ċ import Ġkotlin . jvm . Jvm Field Ċ import Ġkotlin . math . abs Ċ import Ġkotlin . math . min Ċ import Ġkotlin . math . sqrt Ċ Ċ /** Ċ Ġ* ĠUsed Ġto Ġsolve Ġthe Ġcontact Ġconstraints Ġand Ġjoint s Ġfor Ġa Ġgroup Ġof Ġinter connected Ġbodies . Ċ Ġ* Ġ@ author ĠW illiam ĠBit tle Ċ Ġ* Ġ@ version Ġ 3 . 4 . 0 Ċ Ġ* Ġ@ since Ġ 1 . 0 . 0 Ċ Ġ*/ Ċ internal Ġclass ĠIsland Ġconstructor ( initial Capacity : ĠCap acity ? Ġ= ĠCap acity . DEFAULT _ CAP ACITY ) Ġ{ ĊĠĠĠ Ġ/** ĠThe Ġlist Ġof Ġ[ Body ] s Ġon Ġthis Ġ[ Is land ] Ġ Ġ*/ ĊĠĠĠ Ġ@ Jvm Field ĊĠĠĠ Ġval Ġbodies : ĠMutable List < Body > ĊĊĠĠĠ Ġ/** ĠThe Ġlist Ġof Ġ[ Joint ] s Ġon Ġthis Ġ[ Is land ] Ġ Ġ*/ ĊĠĠĠ Ġ@ Jvm Field ĊĠĠĠ Ġval Ġjoint s : ĠMutable List < Joint > ĊĊĠĠĠ Ġ/** ĠThe Ġlist Ġof Ġ[ Contact Constraint ] s Ġon Ġthis Ġ[ Is land ] Ġ Ġ*/ ĊĠĠĠ Ġ@ Jvm Field ĊĠĠĠ Ġval Ġcontact Constraints : ĠMutable List < Contact Constraint > ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* ĠClear s Ġthe Ġis land . ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġfun Ġclear () Ġ{ ĊĠĠĠĠĠĠĠ Ġbodies . clear () ĊĠĠĠĠĠĠĠ Ġjoint s . clear () ĊĠĠĠĠĠĠĠ Ġcontact Constraints . clear () ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* ĠAdds Ġthe Ġgiven Ġ[ Body ] Ġto Ġthe Ġ[ Body ] Ġlist . ĊĠĠĠĠ Ġ* Ġ@ param Ġbody Ġthe Ġ[ Body ] ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġfun Ġadd ( body : ĠBody ) Ġ{ ĊĠĠĠĠĠĠĠ Ġbodies . add ( body ) ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* ĠAdds Ġthe Ġgiven Ġ[ Contact Constraint ] Ġto Ġthe Ġ[ Contact Constraint ] Ġlist . ĊĠĠĠĠ Ġ* Ġ@ param Ġcontact Constraint Ġthe Ġ[ Contact Constraint ] ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġfun Ġadd ( contact Constraint : ĠContact Constraint ) Ġ{ ĊĠĠĠĠĠĠĠ Ġcontact Constraints . add ( contact Constraint ) ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* ĠAdds Ġthe Ġgiven Ġ[ Joint ] Ġto Ġthe Ġ[ Joint ] Ġlist . ĊĠĠĠĠ Ġ* Ġ@ param Ġjoint Ġthe Ġ[ Joint ] ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġfun Ġadd ( joint : ĠJoint ) Ġ{ ĊĠĠĠĠĠĠĠ Ġjoint s . add ( joint ) ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* ĠIntegr ates Ġthe Ġ[ Body ] s , Ġsolves Ġall Ġ[ Contact Constraint ] s Ġand ĊĠĠĠĠ Ġ* Ġ[ Joint ] s , Ġand Ġattempts Ġto Ġsleep Ġmotion less Ġ[ Body ] s . ĊĠĠĠĠ Ġ* Ġ@ param Ġsolver Ġthe Ġcontact Ġconstraint Ġsolver ĊĠĠĠĠ Ġ* Ġ@ param Ġgravity Ġthe Ġgravity Ġvector ĊĠĠĠĠ Ġ* Ġ@ param Ġstep Ġthe Ġtime Ġstep Ġinformation ĊĠĠĠĠ Ġ* Ġ@ param Ġsettings Ġthe Ġcurrent Ġworld Ġsettings ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġfun Ġsolve ( ĊĠĠĠĠĠĠĠ Ġsolver : ĠContact Constraint Solver , ĊĠĠĠĠĠĠĠ Ġgravity : ĠVector 2 , ĊĠĠĠĠĠĠĠ Ġstep : ĠStep , ĊĠĠĠĠĠĠĠ Ġsettings : ĠSettings ĊĠĠĠ Ġ) Ġ{ ĊĠĠĠĠĠĠĠ Ġ// Ġthe Ġnumber Ġof Ġsolver Ġiterations ĊĠĠĠĠĠĠĠ Ġval Ġvelocity Solver Iterations Ġ= Ġsettings . get Velocity Constraint Solver Iterations () ĊĠĠĠĠĠĠĠ Ġval Ġposition Solver Iterations Ġ= Ġsettings . getPosition Constraint Solver Iterations () ĊĠĠĠĠĠĠĠ Ġ// Ġthe Ġsleep Ġsettings ĊĠĠĠĠĠĠĠ Ġval Ġsleep Angular Velocity Ġ= Ġsettings . get Sleep Angular Velocity () ĊĠĠĠĠĠĠĠ Ġval Ġsleep Linear Velocity Squared Ġ= Ġsettings . sleep Linear Velocity Squared ĊĠĠĠĠĠĠĠ Ġval Ġsleep Time Ġ= Ġsettings . get Sleep Time () ĊĠĠĠĠĠĠĠ Ġval Ġsize Ġ= Ġbodies . size ĊĠĠĠĠĠĠĠ Ġval Ġj Size Ġ= Ġjoint s . size ĊĠĠĠĠĠĠĠ Ġval Ġdt : ĠDouble Ġ= Ġstep . dt ĊĠĠĠĠĠĠĠ Ġvar Ġinv M : ĠDouble ĊĠĠĠĠĠĠĠ Ġvar Ġinv I : ĠDouble ĊĊĠĠĠĠĠĠĠ Ġ// Ġintegrate Ġthe Ġve locities ĊĠĠĠĠĠĠĠ Ġfor Ġ( i Ġin Ġ 0 Ġuntil Ġsize ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġval Ġbody : ĠBody Ġ= Ġbodies [ i ] ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ// Ġcheck Ġif Ġthe Ġbody Ġhas Ġinfinite Ġmass Ġand Ġinfinite Ġin ertia ĊĠĠĠĠĠĠĠĠĠĠĠ Ġif Ġ(! body . is Dynamic ()) Ġcontinue ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ// Ġaccum ulate Ġthe Ġforces Ġand Ġtor ques ĊĠĠĠĠĠĠĠĠĠĠĠ Ġbody . accum ulate ( dt ) ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ// Ġget Ġthe Ġmass Ġproperties ĊĠĠĠĠĠĠĠĠĠĠĠ Ġinv M Ġ= Ġbody . mass !!. inverse Mass ĊĠĠĠĠĠĠĠĠĠĠĠ Ġinv I Ġ= Ġbody . mass !!. inverse In ertia ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ// Ġintegrate Ġforce Ġand Ġtor que Ġto Ġmodify Ġthe Ġvelocity Ġand ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ// Ġangular Ġvelocity Ġ( sy mp let ic Ġe uler ) ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ// Ġv 1 Ġ= Ġv 0 Ġ+ Ġ( f Ġ/ Ġm ) Ġ+ Ġg ) Ġ* Ġdt ĊĠĠĠĠĠĠĠĠĠĠĠ Ġif Ġ( inv M Ġ> ĠE psilon . E ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ// Ġonly Ġperform Ġthis Ġstep Ġif Ġthe Ġbody Ġdoes Ġnot Ġhave ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ// Ġa Ġfixed Ġlinear Ġvelocity ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġbody . velocity . x Ġ+= Ġ( body . force !!. x Ġ* Ġinv M Ġ+ Ġgravity . x Ġ* Ġbody . gravity Scale ) Ġ* Ġdt ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġbody . velocity . y Ġ+= Ġ( body . force !!. y Ġ* Ġinv M Ġ+ Ġgravity . y Ġ* Ġbody . gravity Scale ) Ġ* Ġdt ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ// Ġav 1 Ġ= Ġav 0 Ġ+ Ġ( t Ġ/ ĠI ) Ġ* Ġdt ĊĠĠĠĠĠĠĠĠĠĠĠ Ġif Ġ( inv I Ġ> ĠE psilon . E ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ// Ġonly Ġperform Ġthis Ġstep Ġif Ġthe Ġbody Ġdoes Ġnot Ġhave ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ// Ġa Ġfixed Ġangular Ġvelocity ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġbody . angular Velocity Ġ+= Ġdt Ġ* Ġinv I Ġ* Ġbody . tor que ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĊĠĠĠĠĠĠĠĠĠĠĠ Ġ// Ġapply Ġlinear Ġd amping ĊĠĠĠĠĠĠĠĠĠĠĠ Ġif Ġ( body . linear D amping Ġ!== Ġ 0 . 0 ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ// ĠBecause ĠDEFAULT _ LINEAR _ DA MP ING Ġis Ġ 0 . 0 Ġapply Ġlinear Ġd amping Ġonly Ġif Ġneeded ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġvar Ġlinear : ĠDouble Ġ= Ġ 1 . 0 Ġ- Ġdt Ġ* Ġbody . linear D amping ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġlinear Ġ= ĠInterval . clamp ( linear , Ġ 0 . 0 , Ġ 1 . 0 ) ĊĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ// Ġinline Ġbody . velocity . multiply ( linear ); ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġbody . velocity . x Ġ*= Ġlinear ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġbody . velocity . y Ġ*= Ġlinear ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĊĠĠĠĠĠĠĠĠĠĠĠ Ġ// Ġapply Ġangular Ġd amping ĊĠĠĠĠĠĠĠĠĠĠĠ Ġvar Ġangular : ĠDouble Ġ= Ġ 1 . 0 Ġ- Ġdt Ġ* Ġbody . angular D amping ĊĠĠĠĠĠĠĠĠĠĠĠ Ġangular Ġ= ĠInterval . clamp ( angular , Ġ 0 . 0 , Ġ 1 . 0 ) ĊĠĠĠĠĠĠĠĠĠĠĠ Ġbody . angular Velocity Ġ*= Ġangular ĊĠĠĠĠĠĠĠ Ġ} ĊĊĠĠĠĠĠĠĠ Ġ// Ġinitialize Ġthe Ġsolver ĊĠĠĠĠĠĠĠ Ġsolver . initialize ( contact Constraints , Ġstep , Ġsettings ) ĊĊĠĠĠĠĠĠĠ Ġ// Ġinitialize Ġjoint Ġconstraints ĊĠĠĠĠĠĠĠ Ġfor Ġ( i Ġin Ġ 0 Ġuntil Ġj Size ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġval Ġjoint Ġ= Ġjoint s [ i ] ĊĠĠĠĠĠĠĠĠĠĠĠ Ġjoint . initialize Constraints ( step , Ġsettings ) ĊĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠ Ġif Ġ(! contact Constraints . isEmpty () Ġ|| Ġ! joints . isEmpty ()) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ// Ġsolve Ġthe Ġvelocity Ġconstraints Ġif Ġneeded ĊĠĠĠĠĠĠĠĠĠĠĠ Ġfor Ġ( i Ġin Ġ 0 Ġuntil Ġvelocity Solver Iterations ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ// Ġsolve Ġthe Ġjoint Ġvelocity Ġconstraints ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġfor Ġ( j Ġin Ġ 0 Ġuntil Ġj Size ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġval Ġjoint Ġ= Ġjoint s [ j ] ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġjoint . solve Velocity Constraints ( step , Ġsettings ) ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġsolver . solve Velocity Contr aints ( contact Constraints , Ġstep , Ġsettings ) ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠ Ġ} ĊĊĠĠĠĠĠĠĠ Ġ// Ġthe Ġmax Ġsettings ĊĠĠĠĠĠĠĠ Ġval Ġmax Translation Ġ= Ġsettings . get Maximum Translation () ĊĠĠĠĠĠĠĠ Ġval Ġmax Rotation Ġ= Ġsettings . get Maximum Rotation () ĊĠĠĠĠĠĠĠ Ġval Ġmax Translation Sq rd Ġ= Ġsettings . maximum Translation Squared ĊĊĠĠĠĠĠĠĠ Ġ// Ġintegrate Ġthe Ġpositions ĊĠĠĠĠĠĠĠ Ġfor Ġ( i Ġin Ġ 0 Ġuntil Ġsize ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġval Ġbody : ĠBody Ġ= Ġbodies [ i ] ĊĠĠĠĠĠĠĠĠĠĠĠ Ġif Ġ( body . is Static ()) Ġcontinue ĊĊĠĠĠĠĠĠĠĠĠĠĠ Ġ// Ġcompute Ġthe Ġtranslation Ġand Ġrotation Ġfor Ġthis Ġtime Ġstep ĊĠĠĠĠĠĠĠĠĠĠĠ Ġvar Ġtranslation X : ĠDouble Ġ= Ġbody . velocity . x Ġ* Ġdt ĊĠĠĠĠĠĠĠĠĠĠĠ Ġvar Ġtranslation Y : ĠDouble Ġ= Ġbody . velocity . y Ġ* Ġdt ĊĠĠĠĠĠĠĠĠĠĠĠ Ġval Ġtranslation Magnitude Squared Ġ= Ġtranslation X Ġ* Ġtranslation X Ġ+ Ġtranslation Y Ġ* Ġtranslation Y ĊĊĠĠĠĠĠĠĠĠĠĠĠ Ġ// Ġmake Ġsure Ġthe Ġtranslation Ġis Ġnot Ġover Ġthe Ġmaximum ĊĠĠĠĠĠĠĠĠĠĠĠ Ġif Ġ( translation Magnitude Squared Ġ> Ġmax Translation Sq rd ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġval Ġtranslation Magnitude : ĠDouble Ġ= Ġsqrt ( translation Magnitude Squared ) ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġval Ġratio Ġ= Ġmax Translation Ġ/ Ġtranslation Magnitude ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġbody . velocity . multiply ( ratio ) ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġtranslation X Ġ*= Ġratio ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġtranslation Y Ġ*= Ġratio ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠĠĠĠĠ Ġvar Ġrotation : ĠDouble Ġ= Ġbody . angular Velocity Ġ* Ġdt ĊĊĠĠĠĠĠĠĠĠĠĠĠ Ġ// Ġmake Ġsure Ġthe Ġrotation Ġis Ġnot Ġover Ġthe Ġmaximum ĊĠĠĠĠĠĠĠĠĠĠĠ Ġif Ġ( rotation Ġ> Ġmax Rotation ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġval Ġratio : ĠDouble Ġ= Ġmax Rotation Ġ/ Ġabs ( rotation ) ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġbody . angular Velocity Ġ*= Ġratio ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġrotation Ġ*= Ġratio ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĊĠĠĠĠĠĠĠĠĠĠĠ Ġ// Ġre compute Ġthe Ġtranslation / rotation Ġin Ġcase Ġwe Ġhit Ġthe Ġmaximum s ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ// Ġinline Ġbody . translate ( body . velocity . product ( dt )); ĊĠĠĠĠĠĠĠĠĠĠĠ Ġbody . translate ( translation X , Ġtranslation Y ) ĊĠĠĠĠĠĠĠĠĠĠĠ Ġbody . rotate About Center ( rotation ) ĊĠĠĠĠĠĠĠ Ġ} ĊĊĠĠĠĠĠĠĠ Ġ// Ġsolve Ġthe Ġposition Ġconstraints ĊĠĠĠĠĠĠĠ Ġvar Ġposition Constraints S olved Ġ= Ġfalse ĊĠĠĠĠĠĠĠ Ġfor Ġ( i Ġin Ġ 0 Ġuntil Ġposition Solver Iterations ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġval Ġcontacts S olved Ġ= Ġsolver . solve Position Contr aints ( contact Constraints , Ġstep , Ġsettings ) ĊĊĠĠĠĠĠĠĠĠĠĠĠ Ġ// Ġsolve Ġthe Ġjoint Ġposition Ġconstraints ĊĠĠĠĠĠĠĠĠĠĠĠ Ġvar Ġjoint s S olved Ġ= Ġtrue ĊĠĠĠĠĠĠĠĠĠĠĠ Ġfor Ġ( j Ġin Ġ 0 Ġuntil Ġj Size ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġval Ġjoint Ġ= Ġjoint s [ j ] ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġval Ġjoint S olved Ġ= Ġjoint . solve Position Constraints ( step , Ġsettings ) ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġjoint s S olved Ġ= Ġjoint s S olved Ġ&& Ġjoint S olved ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠĠĠĠĠ Ġif Ġ( contacts S olved Ġ&& Ġjoint s S olved ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġposition Constraints S olved Ġ= Ġtrue ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġbreak ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠ Ġ} ĊĊĠĠĠĠĠĠĠ Ġ// Ġsee Ġif Ġsleep Ġis Ġenabled ĊĠĠĠĠĠĠĠ Ġif Ġ( settings . is Auto Sleep ing Enabled ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġvar Ġmin Sleep Time Ġ= ĠDouble . MAX _ VALUE ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ// Ġcheck Ġfor Ġsleep - able Ġbodies ĊĠĠĠĠĠĠĠĠĠĠĠ Ġfor Ġ( i Ġin Ġ 0 Ġuntil Ġsize ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġval Ġbody : ĠBody Ġ= Ġbodies [ i ] ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ// Ġjust Ġskip Ġstatic Ġbodies ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġif Ġ( body . is Static ()) Ġcontinue ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ// Ġsee Ġif Ġthe Ġbody Ġis Ġallowed Ġto Ġsleep ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġif Ġ( body . is Auto Sleep ing Enabled ()) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ// Ġcheck Ġthe Ġlinear Ġand Ġangular Ġvelocity ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġif Ġ( body . velocity . magnitude Squared Ġ> Ġsleep Linear Velocity Squared Ġ|| Ġbody . angular Velocity Ġ> Ġsleep Angular Velocity ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ// Ġif Ġeither Ġthe Ġlinear Ġor Ġangular Ġvelocity Ġis Ġabove Ġthe ĠĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ// Ġthreshold Ġthen Ġreset Ġthe Ġsleep Ġtime ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġbody . sleep Time Ġ= Ġ 0 . 0 ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġmin Sleep Time Ġ= Ġ 0 . 0 ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ} Ġelse Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ// Ġthen Ġincrement Ġthe Ġsleep Ġtime ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġbody . sleep Time Ġ+= Ġstep . dt ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġmin Sleep Time Ġ= Ġmin ( min Sleep Time , Ġbody . sleep Time ) ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ} Ġelse Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġbody . sleep Time Ġ= Ġ 0 . 0 ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġmin Sleep Time Ġ= Ġ 0 . 0 ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĊĠĠĠĠĠĠĠĠĠĠĠ Ġ// Ġcheck Ġthe Ġmin Ġsleep Ġtime ĊĠĠĠĠĠĠĠĠĠĠĠ Ġif Ġ( min Sleep Time Ġ>= Ġsleep Time Ġ&& Ġposition Constraints S olved ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġfor Ġ( i Ġin Ġ 0 Ġuntil Ġsize ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġval Ġbody : ĠBody Ġ= Ġbodies [ i ] ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġbody . set As leep ( true ) ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠ Ġ} ĊĠĠĠ Ġ} ĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* ĠFull Ġconstructor . ĊĠĠĠĠ Ġ* Ġ@ param Ġinitial Capacity Ġthe Ġinitial Ġcapacity Ġof Ġthe Ġis land ĊĠĠĠĠ Ġ* Ġ@ throws ĠNullPointerException Ġif Ġinitial Capacity Ġis Ġnull ĊĠĠĠĠ Ġ* Ġ@ since Ġ 3 . 2 . 0 ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* ĠDefault Ġconstructor . ĊĠĠĠĠ Ġ* ĊĠĠĠĠ Ġ* ĊĠĠĠĠ Ġ* ĠUses Ġa Ġdefault Ġ[ Capacity ] Ġfor Ġthe Ġinitial Ġcapacity . ĊĠĠĠĠ Ġ* Ġ@ since Ġ 3 . 2 . 0 ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġinit Ġ{ ĊĠĠĠĠĠĠĠ Ġ// Ġcheck Ġfor Ġnull Ġcapacity ĊĠĠĠĠĠĠĠ Ġif Ġ( initial Capacity Ġ== Ġnull ) Ġthrow ĠNullPointerException ( message (" dynamics . null Capacity ")) ĊĠĠĠĠĠĠĠ Ġbodies Ġ= ĠArrayList ( initial Capacity . body Count ) ĊĠĠĠĠĠĠĠ Ġjoint s Ġ= ĠArrayList ( initial Capacity . joint Count ) ĊĠĠĠĠĠĠĠ Ġ// Ġestimated Ġthe Ġnumber Ġof Ġcontacts ĊĠĠĠĠĠĠĠ Ġval Ġe Size Ġ= Ġget Est imated Collision Pairs ( initial Capacity . body Count ) ĊĠĠĠĠĠĠĠ Ġcontact Constraints Ġ= ĠArrayList ( e Size ) ĊĠĠĠ Ġ} Ċ }