mathengine.cs

来自「功能：基于windows mobile 的地图查看器。使用vs2005开发」· CS 代码 · 共 168 行

using System;
using Microsoft.WindowsMobile.DirectX;
using Microsoft.WindowsMobile.DirectX.Direct3D;
using System.IO;

namespace WorldWindow
{
	/// <summary>
	/// 
	/// </summary>
	public sealed class MathEngine
	{
		private MathEngine()
		{
		}

		public static Vector3 SphericalToCartesian(
			double latitude,
			double longitude,
			double radius
			)
		{
			//latitude += 90.0f;
			latitude *= System.Math.PI / 180.0f;
			longitude *= System.Math.PI /180.0f;

			double radCosLat = radius * Math.Cos(latitude);

			return new Vector3(
				(float)(radCosLat * Math.Cos(longitude)),
				(float)(radCosLat * Math.Sin(longitude)),
				(float)(radius * Math.Sin(latitude)) );
		}

		public static Vector3 SphericalToCartesian(
			Angle latitude,
			Angle longitude,
			double radius )
		{
			double latRadians = latitude.Radians;
			double lonRadians = longitude.Radians;

			double radCosLat = radius * Math.Cos(latRadians);

			return new Vector3(
				(float)(radCosLat * Math.Cos(lonRadians)),
				(float)(radCosLat * Math.Sin(lonRadians)),
				(float)(radius * Math.Sin(latRadians)));
		}

		public static double DegreesToRadians(double degrees)
		{
			return Math.PI * degrees / 180.0f;
		}

		public static double RadiansToDegrees(double radians)
		{
			return  radians * 180.0 / Math.PI;
		}

		public static double SphericalDistanceDegrees(double latA, double lonA, double latB, double lonB)
		{
			double radLatA = MathEngine.DegreesToRadians(latA);
			double radLatB = MathEngine.DegreesToRadians(latB);
			double radLonA = MathEngine.DegreesToRadians(lonA);
			double radLonB = MathEngine.DegreesToRadians(lonB);

			return MathEngine.RadiansToDegrees(Math.Acos(Math.Cos(radLatA)*Math.Cos(radLatB)*Math.Cos(radLonA-radLonB)+Math.Sin(radLatA)*Math.Sin(radLatB)));
		}

		public static Angle SphericalDistance(Angle latA, Angle lonA, Angle latB, Angle lonB)
		{
			double radLatA = latA.Radians;
			double radLatB = latB.Radians;
			double radLonA = lonA.Radians;
			double radLonB = lonB.Radians;
			return Angle.FromRadians(
				Math.Acos(Math.Cos(radLatA)*Math.Cos(radLatB)*Math.Cos(radLonA-radLonB)+
				Math.Sin(radLatA)*Math.Sin(radLatB)));
		}

		public static Quaternion EulerToQuaternion(double yaw, double pitch, double roll)
		{
			double cy = Math.Cos(yaw * 0.5);
			double cp = Math.Cos(pitch * 0.5);
			double cr = Math.Cos(roll * 0.5);
			double sy = Math.Sin(yaw * 0.5);
			double sp = Math.Sin(pitch * 0.5);
			double sr = Math.Sin(roll * 0.5);

			double qw = cy*cp*cr + sy*sp*sr;
			double qx = sy*cp*cr - cy*sp*sr;
			double qy = cy*sp*cr + sy*cp*sr;
			double qz = cy*cp*sr - sy*sp*cr;

			return new Quaternion((float)qx, (float)qy, (float)qz, (float)qw);
		}

		public static Vector3 QuaternionToEuler(Quaternion q)
		{
			double q0 = q.W;
			double q1 = q.X;
			double q2 = q.Y;
			double q3 = q.Z;

			double x = Math.Atan2( 2 * (q2*q3 + q0*q1), (q0*q0 - q1*q1 - q2*q2 + q3*q3));
			double y = Math.Asin( -2 * (q1*q3 - q0*q2));
			double z = Math.Atan2( 2 * (q1*q2 + q0*q3), (q0*q0 + q1*q1 - q2*q2 - q3*q3));

			return new Vector3((float)x, (float)y, (float)z);

		}

		public static Vector3 CartesianToSpherical(float x, float y, float z)
		{
//			double r = Math.Sqrt((double)(x * x + y * y));
			double rho = Math.Sqrt((double)(x * x + y * y + z * z));
//			double theta = Math.Atan2((double)y, (double)x);

			float longitude = (float)Math.Atan2(y,x);
			float latitude = (float)(Math.Asin(z / rho));
			
			return new Vector3((float)rho, latitude, longitude);
		}

		/// <param name="latitude">Latitude (decimal degrees)</param>
		/// <param name="tileSize">Tile size  (decimal degrees)</param>
		public static int GetRowFromLatitude(double latitude, double tileSize)
		{
			return (int)System.Math.Floor((System.Math.Abs(-90.0 - latitude)%180)/tileSize);
		}

		public static int GetRowFromLatitude(Angle latitude, double tileSize)
		{
			return GetRowFromLatitude(latitude.Degrees, tileSize);
		}

		/// <param name="longitude">Longitude (decimal degrees)</param>
		/// <param name="tileSize">Tile size  (decimal degrees)</param>
		public static int GetColFromLongitude(double longitude, double tileSize)
		{
			return (int)System.Math.Floor((System.Math.Abs(-180.0 - longitude)%360)/tileSize);
		}

		public static int GetColFromLongitude(Angle longitude, double tileSize)
		{
			return GetColFromLongitude(longitude.Degrees, tileSize);
		}

		public static Quaternion GetWorldQuaternion(double latitude, double longitude)
		{
			return Quaternion.RotationYawPitchRoll((float)-MathEngine.DegreesToRadians(latitude),0.0f, (float)-MathEngine.DegreesToRadians(longitude));
		}

		public static Quaternion GetViewQuaternion(float eyeTilt, float eyeDirection)
		{
			Quaternion q1 = Quaternion.RotationAxis(new Vector3(0,0,-1), eyeDirection * (float)Math.PI / 180.0f);
			Quaternion q2 = Quaternion.RotationAxis(new Vector3(1,0,0), eyeTilt * (float)Math.PI / 180.0f);
			return Quaternion.Multiply(q1, q2);
		}

		//public static float DistancePlaneToPoint(Plane p, Vector3 v)
		//{
		//	return p.A * v.X + p.B * v.Y + p.C + v.Z + p.D;
		//}
	}
}