algo_init implemented and add geogebra file for ilustration.
+formationControl 1 dan 2 model +formationControl 2 +param sx/sy fncSpeedRef