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);
}