Robot.Leg.SetXYZ C# (CSharp) Method

        public void SetXYZ(double newX, double newZ, double newY)
        {
            X = newX;
            Z = newZ;
            Y = newY;

            double x = CT;
            double y = Y;
            JointAngeles angeles = IK.CalculateIK(Coxa.Length, Femur.Length, Tibia.Length, x, y);
            Femur.Angle = angeles.FemurAngle;
            Tibia.Angle = angeles.TibiaAngle;

            Coxa.Angle = IK.CalculateIKOneJoint(X, Z);
        }

        private void CalculateNextStep(Leg leg, double direction, double distance, double yDistance, FromPoint fromPoint)
        {
            double newX;
            double newZ;
            double newY;

            Leg.CalculateXandZ(direction, distance, leg.Side, fromPoint.FromX, fromPoint.FromZ, out newX, out newZ);
            newY = fromPoint.FromY - yDistance;
            leg.SetXYZ(newX, newZ, newY);
        }