package Ġcom . team 2 5 4 . lib . spline Ċ Ċ import Ġcom . team 2 5 4 . lib . geometry . Pose 2 d Ċ import Ġcom . team 2 5 4 . lib . geometry . Rotation 2 d Ċ import Ġcom . team 2 5 4 . lib . geometry . Translation 2 d Ċ import Ġkotlin . jvm . Jvm Static Ċ Ċ class ĠQu int ic H erm ite Spline Ġ: ĠSpl ine Ġ{ ĊĠĠĠ Ġprivate Ġvar Ġx 0 : ĠDouble ĊĠĠĠ Ġprivate Ġvar Ġx 1 : ĠDouble ĊĠĠĠ Ġprivate Ġvar Ġdx 0 : ĠDouble ĊĠĠĠ Ġprivate Ġvar Ġdx 1 : ĠDouble ĊĠĠĠ Ġprivate Ġvar Ġd dx 0 : ĠDouble ĊĠĠĠ Ġprivate Ġvar Ġd dx 1 : ĠDouble ĊĠĠĠ Ġprivate Ġvar Ġy 0 : ĠDouble ĊĠĠĠ Ġprivate Ġvar Ġy 1 : ĠDouble ĊĠĠĠ Ġprivate Ġvar Ġdy 0 : ĠDouble ĊĠĠĠ Ġprivate Ġvar Ġdy 1 : ĠDouble ĊĠĠĠ Ġprivate Ġvar Ġd dy 0 : ĠDouble ĊĠĠĠ Ġprivate Ġvar Ġd dy 1 : ĠDouble ĊĠĠĠ Ġprivate Ġvar Ġax Ġ= Ġ 0 . 0 ĊĠĠĠ Ġprivate Ġvar Ġbx Ġ= Ġ 0 . 0 ĊĠĠĠ Ġprivate Ġvar Ġcx Ġ= Ġ 0 . 0 ĊĠĠĠ Ġprivate Ġvar Ġdx Ġ= Ġ 0 . 0 ĊĠĠĠ Ġprivate Ġvar Ġex Ġ= Ġ 0 . 0 ĊĠĠĠ Ġprivate Ġvar Ġfx Ġ= Ġ 0 . 0 ĊĠĠĠ Ġprivate Ġvar Ġay Ġ= Ġ 0 . 0 ĊĠĠĠ Ġprivate Ġvar Ġby Ġ= Ġ 0 . 0 ĊĠĠĠ Ġprivate Ġvar Ġcy Ġ= Ġ 0 . 0 ĊĠĠĠ Ġprivate Ġvar Ġdy Ġ= Ġ 0 . 0 ĊĠĠĠ Ġprivate Ġvar Ġe y Ġ= Ġ 0 . 0 ĊĠĠĠ Ġprivate Ġvar Ġfy Ġ= Ġ 0 . 0 ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* Ġ@ param Ġp 0 ĠThe Ġstarting Ġpose Ġof Ġthe Ġspline ĊĠĠĠĠ Ġ* Ġ@ param Ġp 1 ĠThe Ġending Ġpose Ġof Ġthe Ġspline ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġconstructor ( p 0 : ĠP ose 2 d , Ġp 1 : ĠP ose 2 d ) Ġ{ ĊĠĠĠĠĠĠĠ Ġval Ġscale Ġ= Ġ 1 . 2 Ġ* Ġp 0 . translation . distance ( p 1 . translation ) ĊĠĠĠĠĠĠĠ Ġx 0 Ġ= Ġp 0 . translation . x () ĊĠĠĠĠĠĠĠ Ġx 1 Ġ= Ġp 1 . translation . x () ĊĠĠĠĠĠĠĠ Ġdx 0 Ġ= Ġp 0 . rotation . cos () Ġ* Ġscale ĊĠĠĠĠĠĠĠ Ġdx 1 Ġ= Ġp 1 . rotation . cos () Ġ* Ġscale ĊĠĠĠĠĠĠĠ Ġd dx 0 Ġ= Ġ 0 . 0 ĊĠĠĠĠĠĠĠ Ġd dx 1 Ġ= Ġ 0 . 0 ĊĠĠĠĠĠĠĠ Ġy 0 Ġ= Ġp 0 . translation . y () ĊĠĠĠĠĠĠĠ Ġy 1 Ġ= Ġp 1 . translation . y () ĊĠĠĠĠĠĠĠ Ġdy 0 Ġ= Ġp 0 . rotation . sin () Ġ* Ġscale ĊĠĠĠĠĠĠĠ Ġdy 1 Ġ= Ġp 1 . rotation . sin () Ġ* Ġscale ĊĠĠĠĠĠĠĠ Ġd dy 0 Ġ= Ġ 0 . 0 ĊĠĠĠĠĠĠĠ Ġd dy 1 Ġ= Ġ 0 . 0 ĊĠĠĠĠĠĠĠ Ġcompute Coefficient s () ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* ĠUsed Ġby Ġthe Ġcur v ature Ġoptimization Ġfunction ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġprivate Ġconstructor ( ĊĠĠĠĠĠĠĠ Ġx 0 : ĠDouble , Ġx 1 : ĠDouble , Ġdx 0 : ĠDouble , Ġdx 1 : ĠDouble , Ġd dx 0 : ĠDouble , Ġd dx 1 : ĠDouble , ĊĠĠĠĠĠĠĠ Ġy 0 : ĠDouble , Ġy 1 : ĠDouble , Ġdy 0 : ĠDouble , Ġdy 1 : ĠDouble , Ġd dy 0 : ĠDouble , Ġd dy 1 : ĠDouble ĊĠĠĠ Ġ) Ġ{ ĊĠĠĠĠĠĠĠ Ġthis . x 0 Ġ= Ġx 0 ĊĠĠĠĠĠĠĠ Ġthis . x 1 Ġ= Ġx 1 ĊĠĠĠĠĠĠĠ Ġthis . dx 0 Ġ= Ġdx 0 ĊĠĠĠĠĠĠĠ Ġthis . dx 1 Ġ= Ġdx 1 ĊĠĠĠĠĠĠĠ Ġthis . dd x 0 Ġ= Ġd dx 0 ĊĠĠĠĠĠĠĠ Ġthis . dd x 1 Ġ= Ġd dx 1 ĊĠĠĠĠĠĠĠ Ġthis . y 0 Ġ= Ġy 0 ĊĠĠĠĠĠĠĠ Ġthis . y 1 Ġ= Ġy 1 ĊĠĠĠĠĠĠĠ Ġthis . dy 0 Ġ= Ġdy 0 ĊĠĠĠĠĠĠĠ Ġthis . dy 1 Ġ= Ġdy 1 ĊĠĠĠĠĠĠĠ Ġthis . ddy 0 Ġ= Ġd dy 0 ĊĠĠĠĠĠĠĠ Ġthis . ddy 1 Ġ= Ġd dy 1 ĊĠĠĠĠĠĠĠ Ġcompute Coefficient s () ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* ĠRe - arrange s Ġthe Ġspline Ġinto Ġan Ġat ^ 5 Ġ+ Ġbt ^ 4 Ġ+ Ġ... Ġ+ Ġf Ġform Ġfor Ġsimpler Ġcomput ations ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġprivate Ġfun Ġcompute Coefficient s () Ġ{ ĊĠĠĠĠĠĠĠ Ġax Ġ= Ġ- 6 Ġ* Ġx 0 Ġ- Ġ 3 Ġ* Ġdx 0 Ġ- Ġ 0 . 5 Ġ* Ġd dx 0 Ġ+ Ġ 0 . 5 Ġ* Ġd dx 1 Ġ- Ġ 3 Ġ* Ġdx 1 Ġ+ Ġ 6 Ġ* Ġx 1 ĊĠĠĠĠĠĠĠ Ġbx Ġ= Ġ 1 5 Ġ* Ġx 0 Ġ+ Ġ 8 Ġ* Ġdx 0 Ġ+ Ġ 1 . 5 Ġ* Ġd dx 0 Ġ- Ġd dx 1 Ġ+ Ġ 7 Ġ* Ġdx 1 Ġ- Ġ 1 5 Ġ* Ġx 1 ĊĠĠĠĠĠĠĠ Ġcx Ġ= Ġ- 1 0 Ġ* Ġx 0 Ġ- Ġ 6 Ġ* Ġdx 0 Ġ- Ġ 1 . 5 Ġ* Ġd dx 0 Ġ+ Ġ 0 . 5 Ġ* Ġd dx 1 Ġ- Ġ 4 Ġ* Ġdx 1 Ġ+ Ġ 1 0 Ġ* Ġx 1 ĊĠĠĠĠĠĠĠ Ġdx Ġ= Ġ 0 . 5 Ġ* Ġd dx 0 ĊĠĠĠĠĠĠĠ Ġex Ġ= Ġdx 0 ĊĠĠĠĠĠĠĠ Ġfx Ġ= Ġx 0 ĊĠĠĠĠĠĠĠ Ġay Ġ= Ġ- 6 Ġ* Ġy 0 Ġ- Ġ 3 Ġ* Ġdy 0 Ġ- Ġ 0 . 5 Ġ* Ġd dy 0 Ġ+ Ġ 0 . 5 Ġ* Ġd dy 1 Ġ- Ġ 3 Ġ* Ġdy 1 Ġ+ Ġ 6 Ġ* Ġy 1 ĊĠĠĠĠĠĠĠ Ġby Ġ= Ġ 1 5 Ġ* Ġy 0 Ġ+ Ġ 8 Ġ* Ġdy 0 Ġ+ Ġ 1 . 5 Ġ* Ġd dy 0 Ġ- Ġd dy 1 Ġ+ Ġ 7 Ġ* Ġdy 1 Ġ- Ġ 1 5 Ġ* Ġy 1 ĊĠĠĠĠĠĠĠ Ġcy Ġ= Ġ- 1 0 Ġ* Ġy 0 Ġ- Ġ 6 Ġ* Ġdy 0 Ġ- Ġ 1 . 5 Ġ* Ġd dy 0 Ġ+ Ġ 0 . 5 Ġ* Ġd dy 1 Ġ- Ġ 4 Ġ* Ġdy 1 Ġ+ Ġ 1 0 Ġ* Ġy 1 ĊĠĠĠĠĠĠĠ Ġdy Ġ= Ġ 0 . 5 Ġ* Ġd dy 0 ĊĠĠĠĠĠĠĠ Ġe y Ġ= Ġdy 0 ĊĠĠĠĠĠĠĠ Ġfy Ġ= Ġy 0 ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġval Ġstart Pose : ĠP ose 2 d ĊĠĠĠĠĠĠĠ Ġget () Ġ= ĠP ose 2 d ( ĊĠĠĠĠĠĠĠĠĠĠĠ ĠTranslation 2 d ( x 0 , Ġy 0 ), ĊĠĠĠĠĠĠĠĠĠĠĠ ĠRotation 2 d ( dx 0 , Ġdy 0 , Ġtrue ) ĊĠĠĠĠĠĠĠ Ġ) ĊĠĠĠ Ġval Ġend Pose : ĠP ose 2 d ĊĠĠĠĠĠĠĠ Ġget () Ġ= ĠP ose 2 d ( ĊĠĠĠĠĠĠĠĠĠĠĠ ĠTranslation 2 d ( x 1 , Ġy 1 ), ĊĠĠĠĠĠĠĠĠĠĠĠ ĠRotation 2 d ( dx 1 , Ġdy 1 , Ġtrue ) ĊĠĠĠĠĠĠĠ Ġ) ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* Ġ@ param Ġt Ġranges Ġfrom Ġ 0 Ġto Ġ 1 ĊĠĠĠĠ Ġ* Ġ@ return Ġthe Ġpoint Ġon Ġthe Ġspline Ġfor Ġthat Ġt Ġvalue ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġoverride Ġfun Ġget Point ( t : ĠDouble ): ĠTranslation 2 d Ġ{ ĊĠĠĠĠĠĠĠ Ġval Ġx Ġ= Ġax Ġ* Ġt Ġ* Ġt Ġ* Ġt Ġ* Ġt Ġ* Ġt Ġ+ Ġbx Ġ* Ġt Ġ* Ġt Ġ* Ġt Ġ* Ġt Ġ+ Ġcx Ġ* Ġt Ġ* Ġt Ġ* Ġt Ġ+ Ġdx Ġ* Ġt Ġ* Ġt Ġ+ Ġex Ġ* Ġt Ġ+ Ġfx ĊĠĠĠĠĠĠĠ Ġval Ġy Ġ= Ġay Ġ* Ġt Ġ* Ġt Ġ* Ġt Ġ* Ġt Ġ* Ġt Ġ+ Ġby Ġ* Ġt Ġ* Ġt Ġ* Ġt Ġ* Ġt Ġ+ Ġcy Ġ* Ġt Ġ* Ġt Ġ* Ġt Ġ+ Ġdy Ġ* Ġt Ġ* Ġt Ġ+ Ġe y Ġ* Ġt Ġ+ Ġfy ĊĠĠĠĠĠĠĠ Ġreturn ĠTranslation 2 d ( x , Ġy ) ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġprivate Ġfun Ġdx ( t : ĠDouble ): ĠDouble Ġ{ ĊĠĠĠĠĠĠĠ Ġreturn Ġ 5 Ġ* Ġax Ġ* Ġt Ġ* Ġt Ġ* Ġt Ġ* Ġt Ġ+ Ġ 4 Ġ* Ġbx Ġ* Ġt Ġ* Ġt Ġ* Ġt Ġ+ Ġ 3 Ġ* Ġcx Ġ* Ġt Ġ* Ġt Ġ+ Ġ 2 Ġ* Ġdx Ġ* Ġt Ġ+ Ġex ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġprivate Ġfun Ġdy ( t : ĠDouble ): ĠDouble Ġ{ ĊĠĠĠĠĠĠĠ Ġreturn Ġ 5 Ġ* Ġay Ġ* Ġt Ġ* Ġt Ġ* Ġt Ġ* Ġt Ġ+ Ġ 4 Ġ* Ġby Ġ* Ġt Ġ* Ġt Ġ* Ġt Ġ+ Ġ 3 Ġ* Ġcy Ġ* Ġt Ġ* Ġt Ġ+ Ġ 2 Ġ* Ġdy Ġ* Ġt Ġ+ Ġe y ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġprivate Ġfun Ġd dx ( t : ĠDouble ): ĠDouble Ġ{ ĊĠĠĠĠĠĠĠ Ġreturn Ġ 2 0 Ġ* Ġax Ġ* Ġt Ġ* Ġt Ġ* Ġt Ġ+ Ġ 1 2 Ġ* Ġbx Ġ* Ġt Ġ* Ġt Ġ+ Ġ 6 Ġ* Ġcx Ġ* Ġt Ġ+ Ġ 2 Ġ* Ġdx ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġprivate Ġfun Ġd dy ( t : ĠDouble ): ĠDouble Ġ{ ĊĠĠĠĠĠĠĠ Ġreturn Ġ 2 0 Ġ* Ġay Ġ* Ġt Ġ* Ġt Ġ* Ġt Ġ+ Ġ 1 2 Ġ* Ġby Ġ* Ġt Ġ* Ġt Ġ+ Ġ 6 Ġ* Ġcy Ġ* Ġt Ġ+ Ġ 2 Ġ* Ġdy ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġprivate Ġfun Ġd dd x ( t : ĠDouble ): ĠDouble Ġ{ ĊĠĠĠĠĠĠĠ Ġreturn Ġ 6 0 Ġ* Ġax Ġ* Ġt Ġ* Ġt Ġ+ Ġ 2 4 Ġ* Ġbx Ġ* Ġt Ġ+ Ġ 6 Ġ* Ġcx ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġprivate Ġfun Ġd ddy ( t : ĠDouble ): ĠDouble Ġ{ ĊĠĠĠĠĠĠĠ Ġreturn Ġ 6 0 Ġ* Ġay Ġ* Ġt Ġ* Ġt Ġ+ Ġ 2 4 Ġ* Ġby Ġ* Ġt Ġ+ Ġ 6 Ġ* Ġcy ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġoverride Ġfun Ġget Velocity ( t : ĠDouble ): ĠDouble Ġ{ ĊĠĠĠĠĠĠĠ Ġreturn Ġkotlin . math . h ypot ( dx ( t ), Ġdy ( t )) ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġoverride Ġfun Ġget Cur v ature ( t : ĠDouble ): ĠDouble Ġ{ ĊĠĠĠĠĠĠĠ Ġreturn Ġ( dx ( t ) Ġ* Ġd dy ( t ) Ġ- Ġd dx ( t ) Ġ* Ġdy ( t )) Ġ/ Ġ(( dx ( t ) Ġ* Ġdx ( t ) Ġ+ Ġdy ( t ) Ġ* Ġdy ( t )) Ġ* Ġkotlin . math . sqrt ( ĊĠĠĠĠĠĠĠĠĠĠĠ Ġdx ( t ) Ġ* Ġdx ( t ) Ġ+ Ġdy ( ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġt ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ) Ġ* Ġdy ( t ) ĊĠĠĠĠĠĠĠ Ġ)) ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġoverride Ġfun Ġget D Cur v ature ( t : ĠDouble ): ĠDouble Ġ{ ĊĠĠĠĠĠĠĠ Ġval Ġdx 2 dy 2 Ġ= Ġdx ( t ) Ġ* Ġdx ( t ) Ġ+ Ġdy ( t ) Ġ* Ġdy ( t ) ĊĠĠĠĠĠĠĠ Ġval Ġnum Ġ= ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ( dx ( t ) Ġ* Ġd ddy ( t ) Ġ- Ġd dd x ( t ) Ġ* Ġdy ( t )) Ġ* Ġdx 2 dy 2 Ġ- Ġ 3 Ġ* Ġ( dx ( t ) Ġ* Ġd dy ( t ) Ġ- Ġd dx ( t ) Ġ* Ġdy ( t )) Ġ* Ġ( dx ( t ) Ġ* Ġd dx ( t ) Ġ+ Ġdy ( ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġt ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ) Ġ* Ġd dy ( t )) ĊĠĠĠĠĠĠĠ Ġreturn Ġnum Ġ/ Ġ( dx 2 dy 2 Ġ* Ġdx 2 dy 2 Ġ* Ġkotlin . math . sqrt ( dx 2 dy 2 )) ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġprivate Ġfun Ġd Cur v ature 2 ( t : ĠDouble ): ĠDouble Ġ{ ĊĠĠĠĠĠĠĠ Ġval Ġdx 2 dy 2 Ġ= Ġdx ( t ) Ġ* Ġdx ( t ) Ġ+ Ġdy ( t ) Ġ* Ġdy ( t ) ĊĠĠĠĠĠĠĠ Ġval Ġnum Ġ= ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ( dx ( t ) Ġ* Ġd ddy ( t ) Ġ- Ġd dd x ( t ) Ġ* Ġdy ( t )) Ġ* Ġdx 2 dy 2 Ġ- Ġ 3 Ġ* Ġ( dx ( t ) Ġ* Ġd dy ( t ) Ġ- Ġd dx ( t ) Ġ* Ġdy ( t )) Ġ* Ġ( dx ( t ) Ġ* Ġd dx ( t ) Ġ+ Ġdy ( ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġt ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ) Ġ* Ġd dy ( t )) ĊĠĠĠĠĠĠĠ Ġreturn Ġnum Ġ* Ġnum Ġ/ Ġ( dx 2 dy 2 Ġ* Ġdx 2 dy 2 Ġ* Ġdx 2 dy 2 Ġ* Ġdx 2 dy 2 Ġ* Ġdx 2 dy 2 ) ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġoverride Ġfun Ġget Heading ( t : ĠDouble ): ĠRotation 2 d Ġ{ ĊĠĠĠĠĠĠĠ Ġreturn ĠRotation 2 d ( dx ( t ), Ġdy ( t ), Ġtrue ) ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* Ġ@ return Ġintegral Ġof Ġd Cur v ature ^ 2 Ġover Ġthe Ġlength Ġof Ġthe Ġspline ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġprivate Ġfun Ġsum D Cur v ature 2 (): ĠDouble Ġ{ ĊĠĠĠĠĠĠĠ Ġval Ġdt Ġ= Ġ 1 . 0 Ġ/ Ġk Samples ĊĠĠĠĠĠĠĠ Ġvar Ġsum Ġ= Ġ 0 . 0 ĊĠĠĠĠĠĠĠ Ġvar Ġt Ġ= Ġ 0 . 0 ĊĠĠĠĠĠĠĠ Ġwhile Ġ( t Ġ< Ġ 1 . 0 ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġsum Ġ+= Ġdt Ġ* Ġd Cur v ature 2 ( t ) ĊĠĠĠĠĠĠĠĠĠĠĠ Ġt Ġ+= Ġdt ĊĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠ Ġreturn Ġsum ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġ/** ĊĠĠĠĠ Ġ* ĠMakes Ġoptimization Ġcode Ġa Ġlittle Ġmore Ġreadable ĊĠĠĠĠ Ġ*/ ĊĠĠĠ Ġprivate Ġclass ĠControl Point Ġ{ ĊĠĠĠĠĠĠĠ Ġvar Ġd dx Ġ= Ġ 0 . 0 ĊĠĠĠĠĠĠĠ Ġvar Ġd dy Ġ= Ġ 0 . 0 ĊĠĠĠ Ġ} ĊĊĠĠĠ Ġcompanion Ġobject Ġ{ ĊĠĠĠĠĠĠĠ Ġprivate Ġconst Ġval Ġk Epsilon Ġ= Ġ 1 e - 5 ĊĠĠĠĠĠĠĠ Ġprivate Ġconst Ġval Ġk Step Size Ġ= Ġ 1 . 0 ĊĠĠĠĠĠĠĠ Ġprivate Ġconst Ġval Ġk Min Delta Ġ= Ġ 0 . 0 0 1 ĊĠĠĠĠĠĠĠ Ġprivate Ġconst Ġval Ġk Samples Ġ= Ġ 1 0 0 ĊĠĠĠĠĠĠĠ Ġprivate Ġconst Ġval Ġk Max Iterations Ġ= Ġ 1 0 0 ĊĊĠĠĠĠĠĠĠ Ġ/** ĊĠĠĠĠĠĠĠĠ Ġ* Ġ@ return Ġintegral Ġof Ġd Cur v ature ^ 2 Ġover Ġthe Ġlength Ġof Ġmultiple Ġspl ines ĊĠĠĠĠĠĠĠĠ Ġ*/ ĊĠĠĠĠĠĠĠ Ġ@ Jvm Static ĊĠĠĠĠĠĠĠ Ġfun Ġsum D Cur v ature 2 ( sp lines : ĠList < Qu int ic H erm ite Spline >): ĠDouble Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġvar Ġsum Ġ= Ġ 0 . 0 ĊĠĠĠĠĠĠĠĠĠĠĠ Ġfor Ġ( s Ġin Ġspl ines ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġsum Ġ+= Ġs . sum D Cur v ature 2 () ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠĠĠĠĠ Ġreturn Ġsum ĊĠĠĠĠĠĠĠ Ġ} ĊĊĠĠĠĠĠĠĠ Ġ/** ĊĠĠĠĠĠĠĠĠ Ġ* ĠFinds Ġthe Ġoptimal Ġsecond Ġderivative Ġvalues Ġfor Ġa Ġset Ġof Ġspl ines Ġto Ġreduce Ġthe Ġsum Ġof Ġthe Ġchange Ġin Ġcur v ature ĊĠĠĠĠĠĠĠĠ Ġ* Ġsquared Ġover Ġthe Ġpath ĊĠĠĠĠĠĠĠĠ Ġ* ĊĠĠĠĠĠĠĠĠ Ġ* Ġ@ param Ġspl ines Ġthe Ġlist Ġof Ġspl ines Ġto Ġoptimize ĊĠĠĠĠĠĠĠĠ Ġ* Ġ@ return Ġthe Ġfinal Ġsum D Cur v ature 2 ĊĠĠĠĠĠĠĠĠ Ġ*/ ĊĠĠĠĠĠĠĠ Ġ@ Jvm Static ĊĠĠĠĠĠĠĠ Ġfun Ġoptimize Spline ( sp lines : ĠMutable List < Qu int ic H erm ite Spline >): ĠDouble Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġvar Ġcount Ġ= Ġ 0 ĊĠĠĠĠĠĠĠĠĠĠĠ Ġvar Ġprev Ġ= Ġsum D Cur v ature 2 ( sp lines ) ĊĠĠĠĠĠĠĠĠĠĠĠ Ġwhile Ġ( count Ġ< Ġk Max Iterations ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġrun Optimization Iteration ( sp lines ) ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġval Ġcurrent Ġ= Ġsum D Cur v ature 2 ( sp lines ) ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġif Ġ( prev Ġ- Ġcurrent Ġ< Ġk Min Delta ) Ġreturn Ġcurrent ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġprev Ġ= Ġcurrent ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġcount ++ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠĠĠĠĠ Ġreturn Ġprev ĊĠĠĠĠĠĠĠ Ġ} ĊĊĠĠĠĠĠĠĠ Ġ/** ĊĠĠĠĠĠĠĠĠ Ġ* ĠRuns Ġa Ġsingle Ġoptimization Ġiteration ĊĠĠĠĠĠĠĠĠ Ġ*/ ĊĠĠĠĠĠĠĠ Ġprivate Ġfun Ġrun Optimization Iteration ( sp lines : ĠMutable List < Qu int ic H erm ite Spline >) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ// can 't Ġoptimize Ġanything Ġwith Ġless Ġthan Ġ 2 Ġspl ines ĊĠĠĠĠĠĠĠĠĠĠĠ Ġif Ġ( sp lines . size Ġ<= Ġ 1 ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġreturn ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠĠĠĠĠ Ġval Ġcontrol Points Ġ= ĠarrayOf Null s < ControlPoint >( sp lines . size Ġ- Ġ 1 ) ĊĠĠĠĠĠĠĠĠĠĠĠ Ġvar Ġmagnitude Ġ= Ġ 0 . 0 ĊĠĠĠĠĠĠĠĠĠĠĠ Ġfor Ġ( i Ġin Ġ 0 Ġuntil Ġspl ines . size Ġ- Ġ 1 ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ// don 't Ġtry Ġto Ġoptimize Ġcol ine ar Ġpoints ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġif Ġ( sp lines [ i ]. start Pose . is Col ine ar ( sp lines [ i Ġ+ Ġ 1 ]. start Pose ) Ġ|| Ġspl ines [ i ]. end Pose . is Col ine ar ( sp lines [ i Ġ+ Ġ 1 ]. end Pose )) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġcontinue ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġval Ġoriginal Ġ= Ġsum D Cur v ature 2 ( sp lines ) ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġval Ġtemp : ĠQu int ic H erm ite Spline Ġ= Ġspl ines [ i ] ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġval Ġtemp 1 : ĠQu int ic H erm ite Spline Ġ= Ġspl ines [ i Ġ+ Ġ 1 ] ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġcontrol Points [ i ] Ġ= ĠControl Point () Ġ// holds Ġthe Ġgradient Ġat Ġa Ġcontrol Ġpoint ĊĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ// calculate Ġpartial Ġderivatives Ġof Ġsum D Cur v ature 2 ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġspl ines [ i ] Ġ= ĠQu int ic H erm ite Spline ( ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġtemp . x 0 , ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġtemp . x 1 , ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġtemp . dx 0 , ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġtemp . dx 1 , ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġtemp . dd x 0 , ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġtemp . dd x 1 Ġ+ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġk Epsilon , ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġtemp . y 0 , ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġtemp . y 1 , ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġtemp . dy 0 , ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġtemp . dy 1 , ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġtemp . ddy 0 , ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġtemp . ddy 1 ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ) ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġspl ines [ i Ġ+ Ġ 1 ] Ġ= ĠQu int ic H erm ite Spline ( ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġtemp 1 . x 0 , ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġtemp 1 . x 1 , ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġtemp 1 . dx 0 , ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġtemp 1 . dx 1 , ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġtemp 1 . dd x 0 Ġ+ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġk Epsilon , ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġtemp 1 . dd x 1 , ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġtemp 1 . y 0 , ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġtemp 1 . y 1 , ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġtemp 1 . dy 0 , ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġtemp 1 . dy 1 , ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġtemp 1 . ddy 0 , ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġtemp 1 . ddy 1 ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ) ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġcontrol Points [ i ] !!. dd x Ġ= Ġ( sum D Cur v ature 2 ( sp lines ) Ġ- Ġoriginal ) Ġ/ Ġk Epsilon ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġspl ines [ i ] Ġ= ĠQu int ic H erm ite Spline ( ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġtemp . x 0 , ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġtemp . x 1 , ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġtemp . dx 0 , ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġtemp . dx 1 , ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġtemp . dd x 0 , ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġtemp . dd x 1 , ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġtemp . y 0 , ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġtemp . y 1 , ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġtemp . dy 0 , ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġtemp . dy 1 , ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġtemp . ddy 0 , ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġtemp . ddy 1 Ġ+ Ġk Epsilon ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ) ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġspl ines [ i Ġ+ Ġ 1 ] Ġ= ĠQu int ic H erm ite Spline ( ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġtemp 1 . x 0 , ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġtemp 1 . x 1 , ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġtemp 1 . dx 0 , ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġtemp 1 . dx 1 , ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġtemp 1 . dd x 0 , ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġtemp 1 . dd x 1 , ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġtemp 1 . y 0 , ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġtemp 1 . y 1 , ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġtemp 1 . dy 0 , ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġtemp 1 . dy 1 , ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġtemp 1 . ddy 0 Ġ+ Ġk Epsilon , ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġtemp 1 . ddy 1 ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ) ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġcontrol Points [ i ] !!. ddy Ġ= Ġ( sum D Cur v ature 2 ( sp lines ) Ġ- Ġoriginal ) Ġ/ Ġk Epsilon ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġspl ines [ i ] Ġ= Ġtemp ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġspl ines [ i Ġ+ Ġ 1 ] Ġ= Ġtemp 1 ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġmagnitude Ġ+= Ġcontrol Points [ i ] !!. dd x Ġ* Ġcontrol Points [ i ] !!. dd x Ġ+ Ġcontrol Points [ i ] !!. ddy Ġ* Ġcontrol Points [ i ] !!. ddy ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠĠĠĠĠ Ġmagnitude Ġ= Ġkotlin . math . sqrt ( magnitude ) ĊĊĠĠĠĠĠĠĠĠĠĠĠ Ġ// minimize Ġalong Ġthe Ġdirection Ġof Ġthe Ġgradient ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ// first Ġcalculate Ġ 3 Ġpoints Ġalong Ġthe Ġdirection Ġof Ġthe Ġgradient ĊĠĠĠĠĠĠĠĠĠĠĠ Ġval Ġp 1 : ĠTranslation 2 d ĊĠĠĠĠĠĠĠĠĠĠĠ Ġval Ġp 2 : ĠTranslation 2 d ĊĠĠĠĠĠĠĠĠĠĠĠ Ġval Ġp 3 : ĠTranslation 2 d ĊĠĠĠĠĠĠĠĠĠĠĠ Ġp 2 Ġ= ĠTranslation 2 d ( 0 . 0 , Ġsum D Cur v ature 2 ( sp lines )) Ġ// middle Ġpoint Ġis Ġat Ġthe Ġcurrent Ġlocation ĊĠĠĠĠĠĠĠĠĠĠĠ Ġfor Ġ( i Ġin Ġ 0 Ġuntil Ġspl ines . size Ġ- Ġ 1 ) Ġ{ Ġ// first Ġpoint Ġis Ġoffset Ġfrom Ġthe Ġmiddle Ġlocation Ġby Ġ- step Size ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġif Ġ( sp lines [ i ]. start Pose . is Col ine ar ( sp lines [ i Ġ+ Ġ 1 ]. start Pose ) Ġ|| Ġspl ines [ i ]. end Pose . is Col ine ar ( sp lines [ i Ġ+ Ġ 1 ]. end Pose )) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġcontinue ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ// normalize Ġto Ġstep Ġsize ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġcontrol Points [ i ] !!. dd x Ġ*= Ġk Step Size Ġ/ Ġmagnitude ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġcontrol Points [ i ] !!. ddy Ġ*= Ġk Step Size Ġ/ Ġmagnitude ĊĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ// move Ġopposite Ġthe Ġgradient Ġby Ġstep Ġsize Ġamount ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġspl ines [ i ]. dd x 1 Ġ-= Ġcontrol Points [ i ] !!. dd x ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġspl ines [ i ]. ddy 1 Ġ-= Ġcontrol Points [ i ] !!. ddy ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġspl ines [ i Ġ+ Ġ 1 ]. dd x 0 Ġ-= Ġcontrol Points [ i ] !!. dd x ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġspl ines [ i Ġ+ Ġ 1 ]. ddy 0 Ġ-= Ġcontrol Points [ i ] !!. ddy ĊĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ// re compute Ġthe Ġspline 's Ġcoefficients Ġto Ġaccount Ġfor Ġnew Ġsecond Ġderivatives ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġspl ines [ i ]. compute Coefficient s () ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġspl ines [ i Ġ+ Ġ 1 ]. compute Coefficient s () ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠĠĠĠĠ Ġp 1 Ġ= ĠTranslation 2 d (- k Step Size , Ġsum D Cur v ature 2 ( sp lines )) ĊĠĠĠĠĠĠĠĠĠĠĠ Ġfor Ġ( i Ġin Ġ 0 Ġuntil Ġspl ines . size Ġ- Ġ 1 ) Ġ{ Ġ// last Ġpoint Ġis Ġoffset Ġfrom Ġthe Ġmiddle Ġlocation Ġby Ġ+ step Size ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġif Ġ( sp lines [ i ]. start Pose . is Col ine ar ( sp lines [ i Ġ+ Ġ 1 ]. start Pose ) Ġ|| Ġspl ines [ i ]. end Pose . is Col ine ar ( sp lines [ i Ġ+ Ġ 1 ]. end Pose )) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġcontinue ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ// move Ġalong Ġthe Ġgradient Ġby Ġ 2 Ġtimes Ġthe Ġstep Ġsize Ġamount Ġ( to Ġreturn Ġto Ġoriginal Ġlocation Ġand Ġmove Ġby Ġ 1 ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ// Ġstep ) ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġspl ines [ i ]. dd x 1 Ġ+= Ġ 2 Ġ* Ġcontrol Points [ i ] !!. dd x ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġspl ines [ i ]. ddy 1 Ġ+= Ġ 2 Ġ* Ġcontrol Points [ i ] !!. ddy ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġspl ines [ i Ġ+ Ġ 1 ]. dd x 0 Ġ+= Ġ 2 Ġ* Ġcontrol Points [ i ] !!. dd x ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġspl ines [ i Ġ+ Ġ 1 ]. ddy 0 Ġ+= Ġ 2 Ġ* Ġcontrol Points [ i ] !!. ddy ĊĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ// re compute Ġthe Ġspline 's Ġcoefficients Ġto Ġaccount Ġfor Ġnew Ġsecond Ġderivatives ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġspl ines [ i ]. compute Coefficient s () ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġspl ines [ i Ġ+ Ġ 1 ]. compute Coefficient s () ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠĠĠĠĠ Ġp 3 Ġ= ĠTranslation 2 d ( k Step Size , Ġsum D Cur v ature 2 ( sp lines )) ĊĠĠĠĠĠĠĠĠĠĠĠ Ġval Ġstep Size Ġ= Ġfit Par ab ola ( p 1 , Ġp 2 , Ġp 3 ) Ġ// approx imate Ġstep Ġsize Ġto Ġminimize Ġsum D Cur v ature 2 Ġalong Ġthe Ġgradient ĊĠĠĠĠĠĠĠĠĠĠĠ Ġfor Ġ( i Ġin Ġ 0 Ġuntil Ġspl ines . size Ġ- Ġ 1 ) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġif Ġ( sp lines [ i ]. start Pose . is Col ine ar ( sp lines [ i Ġ+ Ġ 1 ]. start Pose ) Ġ|| Ġspl ines [ i ]. end Pose . is Col ine ar ( sp lines [ i Ġ+ Ġ 1 ]. end Pose )) Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġcontinue ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ// move Ġby Ġthe Ġstep Ġsize Ġcalculated Ġby Ġthe Ġpar ab ola Ġfit Ġ(+ 1 Ġto Ġoffset Ġfor Ġthe Ġfinal Ġtransformation Ġto Ġfind ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ// Ġp 3 ) ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġcontrol Points [ i ] !!. dd x Ġ*= Ġ 1 Ġ+ Ġstep Size Ġ/ Ġk Step Size ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġcontrol Points [ i ] !!. ddy Ġ*= Ġ 1 Ġ+ Ġstep Size Ġ/ Ġk Step Size ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġspl ines [ i ]. dd x 1 Ġ+= Ġcontrol Points [ i ] !!. dd x ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġspl ines [ i ]. ddy 1 Ġ+= Ġcontrol Points [ i ] !!. ddy ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġspl ines [ i Ġ+ Ġ 1 ]. dd x 0 Ġ+= Ġcontrol Points [ i ] !!. dd x ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġspl ines [ i Ġ+ Ġ 1 ]. ddy 0 Ġ+= Ġcontrol Points [ i ] !!. ddy ĊĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ// re compute Ġthe Ġspline 's Ġcoefficients Ġto Ġaccount Ġfor Ġnew Ġsecond Ġderivatives ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġspl ines [ i ]. compute Coefficient s () ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġspl ines [ i Ġ+ Ġ 1 ]. compute Coefficient s () ĊĠĠĠĠĠĠĠĠĠĠĠ Ġ} ĊĠĠĠĠĠĠĠ Ġ} ĊĊĠĠĠĠĠĠĠ Ġ/** ĊĠĠĠĠĠĠĠĠ Ġ* Ġfits Ġa Ġpar ab ola Ġto Ġ 3 Ġpoints ĊĠĠĠĠĠĠĠĠ Ġ* ĊĠĠĠĠĠĠĠĠ Ġ* Ġ@ return Ġthe Ġx Ġcoordinate Ġof Ġthe Ġvertex Ġof Ġthe Ġpar ab ola ĊĠĠĠĠĠĠĠĠ Ġ*/ ĊĠĠĠĠĠĠĠ Ġprivate Ġfun Ġfit Par ab ola ( p 1 : ĠTranslation 2 d , Ġp 2 : ĠTranslation 2 d , Ġp 3 : ĠTranslation 2 d ): ĠDouble Ġ{ ĊĠĠĠĠĠĠĠĠĠĠĠ Ġval ĠA Ġ= Ġp 3 . x () Ġ* Ġ( p 2 . y () Ġ- Ġp 1 . y ()) Ġ+ Ġp 2 . x () Ġ* Ġ( p 1 . y () Ġ- Ġp 3 . y ()) Ġ+ Ġp 1 . x () Ġ* Ġ( p 3 . y () Ġ- Ġp 2 . y ()) ĊĠĠĠĠĠĠĠĠĠĠĠ Ġval ĠB Ġ= Ġp 3 . x () Ġ* Ġp 3 . x () Ġ* Ġ( p 1 . y () Ġ- Ġp 2 . y ()) Ġ+ Ġp 2 . x () Ġ* Ġp 2 . x () Ġ* Ġ( p 3 . y () Ġ- Ġp 1 . y ()) Ġ+ Ġp 1 . x () Ġ* Ġp 1 . x () Ġ* ĊĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠĠ Ġ( p 2 . y () Ġ- Ġp 3 . y ()) ĊĠĠĠĠĠĠĠĠĠĠĠ Ġreturn Ġ- B Ġ/ Ġ( 2 Ġ* ĠA ) ĊĠĠĠĠĠĠĠ Ġ} ĊĠĠĠ Ġ} Ċ }