package Ġf rc . robot . utils Ċ Ċ import Ġedu . wpi . first . math . Math Util Ċ import Ġkotlin . math . sqrt Ċ Ċ class ĠVector 3 Ġ: ĠInterpol able < Vector 3 > Ġ{ ĊĠ Ġvar Ġx : ĠDouble Ġ= Ġ 0 . 0 ĊĠ Ġvar Ġy : ĠDouble Ġ= Ġ 0 . 0 ĊĠ Ġvar Ġz : ĠDouble Ġ= Ġ 0 . 0 ĊĊĠ Ġval Ġmagnitude : ĠDouble ĊĠĠĠ Ġget () Ġ= Ġsqrt ( x Ġ* Ġx Ġ+ Ġy Ġ* Ġy Ġ+ Ġz Ġ* Ġz ) ĊĠ Ġval Ġquaternion : ĠQuaternion ĊĠĠĠ Ġget () Ġ= ĠQuaternion ( Vector 4 ( 0 . 0 , Ġthis )) ĊĊĠ Ġconstructor () ĊĊĠ Ġconstructor ( x : ĠDouble , Ġy : ĠDouble , Ġz : ĠDouble ) Ġ{ ĊĠĠĠ Ġthis . x Ġ= Ġx ĊĠĠĠ Ġthis . y Ġ= Ġy ĊĠĠĠ Ġthis . z Ġ= Ġz ĊĠ Ġ} ĊĊĠ Ġoperator Ġfun Ġplus ( other : ĠVector 3 ): ĠVector 3 Ġ{ ĊĠĠĠ Ġreturn ĠVector 3 ( x Ġ+ Ġother . x , Ġy Ġ+ Ġother . y , Ġz Ġ+ Ġother . z ) ĊĠ Ġ} ĊĊĠ Ġoperator Ġfun Ġminus ( other : ĠVector 3 ): ĠVector 3 Ġ{ ĊĠĠĠ Ġreturn ĠVector 3 ( x Ġ- Ġother . x , Ġy Ġ- Ġother . y , Ġz Ġ- Ġother . z ) ĊĠ Ġ} ĊĊĠ Ġoperator Ġfun Ġunary Minus (): ĠVector 3 Ġ{ ĊĠĠĠ Ġreturn ĠVector 3 (- x , Ġ- y , Ġ- z ) ĊĠ Ġ} ĊĊĠ Ġoperator Ġfun Ġtimes ( scalar : ĠDouble ): ĠVector 3 Ġ{ ĊĠĠĠ Ġreturn ĠVector 3 ( x Ġ* Ġscalar , Ġy Ġ+ Ġscalar , Ġz Ġ* Ġscalar ) ĊĠ Ġ} ĊĊĠ Ġoperator Ġfun Ġdiv ( scalar : ĠDouble ): ĠVector 3 Ġ{ ĊĠĠĠ Ġreturn ĠVector 3 ( x Ġ/ Ġscalar , Ġy Ġ/ Ġscalar , Ġz Ġ/ Ġscalar ) ĊĠ Ġ} ĊĊĠ Ġfun Ġdot ( other : ĠVector 3 ): ĠDouble Ġ{ ĊĠĠĠ Ġreturn Ġx Ġ* Ġother . x Ġ+ Ġy Ġ* Ġother . y Ġ+ Ġz Ġ* Ġother . z ĊĠ Ġ} ĊĊĠ Ġfun Ġcross ( other : ĠVector 3 ): ĠVector 3 Ġ{ ĊĠĠĠ Ġreturn ĠVector 3 ( y Ġ* Ġother . z Ġ- Ġz Ġ* Ġother . y , Ġz Ġ* Ġother . x Ġ- Ġx Ġ* Ġother . z , Ġx Ġ* Ġother . y Ġ- Ġy Ġ* Ġother . x ) ĊĠ Ġ} ĊĊĠ Ġfun Ġrotate ( angle : ĠDouble , Ġaxis : ĠVector 3 ): ĠVector 3 Ġ{ ĊĠĠĠ Ġval Ġp Ġ= Ġquaternion . normalized ĊĠĠĠ Ġval Ġq Ġ= ĠQuaternion ( angle , Ġaxis ) ĊĠĠĠ Ġreturn Ġ( q Ġ* Ġp Ġ* Ġq . inverse ). xyz ĊĠ Ġ} ĊĊĠ Ġfun Ġrotate ( q : ĠQuaternion ): ĠVector 3 Ġ{ ĊĠĠĠ Ġval Ġp Ġ= Ġquaternion . normalized ĊĠĠĠ Ġreturn Ġ( q Ġ* Ġp Ġ* Ġq . inverse ). xyz ĊĠ Ġ} ĊĊĠ Ġoverride Ġfun Ġinterpolate ( upper : ĠVector 3 , Ġt : ĠDouble ): ĠVector 3 Ġ{ ĊĠĠĠ Ġreturn ĠVector 3 ( ĊĠĠĠĠĠ ĠMath Util . interpolate ( x , Ġupper . x , Ġt ), ĊĠĠĠĠĠ ĠMath Util . interpolate ( y , Ġupper . y , Ġt ), ĊĠĠĠĠĠ ĠMath Util . interpolate ( z , Ġupper . z , Ġt ) ĊĠĠĠ Ġ) ĊĠ Ġ} Ċ }