MissionPlanner.HIL.Vector3.length C# (CSharp) Method

        public double length()
        {
            return Math.Sqrt(x*x + y*y + z*z);
        }

        public void normalize()
        {
            //  '''re-normalise a rotation matrix'''
            Vector3 error = self.a * self.b;
            Vector3 t0    = self.a - (self.b * (0.5 * error));
            Vector3 t1    = self.b - (self.a * (0.5 * error));
            Vector3 t2    = t0 % t1;

            self.a = t0 * (1.0 / t0.length());
            self.b = t1 * (1.0 / t1.length());
            self.c = t2 * (1.0 / t2.length());
        }