gistfile1.cs

    public static Int32 CalcHeading(Single velDown, Single velNorth, Single velEast, ref Single pheading)
    {
      Int32 ret = 0;

      Double r = 0.0;
      Double dvNorth = 0.0;
      Double dvEast = 0.0;
      Double rsquare = 0.0;

      Single heading = 0.0f;

      dvNorth = (Double)velNorth;
      dvEast = (Double)velEast;

      rsquare = (dvNorth * dvNorth) + (dvEast * dvEast);

      r = Math.Sqrt(rsquare);

      if (r <= 0.0)
      {
        heading = 0.0f;
      }
      else
      {
        if (dvNorth >= 0 && dvEast >= 0)
        {
          heading = (Single)Math.Asin((Double)(dvEast / r));
          heading *= (Single)(180.0 / Math.PI);
        }

        else if (dvNorth >= 0 && dvEast <= 0)
        {
          heading = (Single)Math.Asin((Double)(dvEast / r));
          heading *= (Single)(180.0 / Math.PI);
          heading += 360.0f;
        }

        else if (dvNorth <= 0 && dvEast >= 0)
        {
          heading = (Single)Math.Acos((Double)(dvEast / r));
          heading *= (Single)(180.0 / Math.PI);
          heading += 90.0f;
        }

        else if (dvNorth <= 0 && dvEast <= 0)
        {
          dvEast *= -1.0;
          heading = (Single)Math.Acos((Double)(dvEast / r));
          heading *= (Single)(180.0 / Math.PI);
          heading *= -1.0f;
          heading -= 90.0f;
        }
      }
      pheading = heading;
      return ret;
    }

    private static Single CalcHeading_Test(Single velDown, Single velNorth, Single velEast)
    {
      Double vNorth = (Double)velNorth;
      Double vEast = (Double)velEast;
      Double rSquare = (vNorth * vNorth) + (vEast * vEast);
      Double r = Math.Sqrt(rSquare);
      Single heading = 0.0f;

      if (r <= 0.0)
        heading = 0.0f;
      else
      {
        if (vNorth >= 0 && vEast >= 0)
        {
          heading = (Single)Math.Asin((Double)(vEast / r));
          heading *= (Single)(180.0 / Math.PI);
          heading += 180.0f;
        }
        else if (vNorth >= 0 && vEast <= 0)
        {
          heading = (Single)Math.Asin((Double)(vEast / r));
          heading *= (Single)(180.0 / Math.PI);
          heading += 360.0f;
        }
        else if (vNorth <= 0 && vEast >= 0)
        {
          heading = (Single)Math.Acos((Double)(vEast / r));
          heading *= (Single)(180.0 / Math.PI);
          heading += 270.0f;
        }
        else if (vNorth <= 0 && vEast <= 0)
        {
          vEast *= -1.0;
          heading = (Single)Math.Acos((Double)(vEast / r));
          heading *= (Single)(180.0 / Math.PI);
          heading *= -1.0f;
          heading += 90.0f;
        }
      }
      return heading;
    }