public static Matrix GetOrientation(float x, float y)
{
Matrix Transformation = Matrix.Identity;
Transformation.Up = PlanetEarth.GetNormal(x, y);
Transformation.Right = PlanetEarth.GetTangentX(x, y);
Transformation.Forward = -PlanetEarth.GetTangentY(x, y);
return(Transformation);
}
/// <summary>
/// 有两个Map的坐标计算采集车朝向
/// Calculate the orientation by the given 2 points in Map's coordinate,
/// which can be converted from long-lat by Map.GetMapCoord
/// </summary>
/// <param name="pa"></param>
/// <param name="pb"></param>
/// <returns></returns>
Quaternion GetOrientation(Point pa, Point pb)
{
float alng;
float alat;
float blng;
float blat;
// 先得到两个点的经纬度
// first convert the coord to long-lat
Map.GetCoord(pa.X, pa.Y, out alng, out alat);
Map.GetCoord(pb.X, pb.Y, out blng, out blat);
// 法向
// calculate the normal vector(up vector)
Vector3 n = PlanetEarth.GetNormal(alng, alat);
// 高度
// elevation
float altA = map.GetHeight(alng, alat);
Vector3 posA = PlanetEarth.GetPosition(alng, alat, altA + PlanetEarth.PlanetRadius);
float altB = map.GetHeight(blng, blat);
Vector3 posB = PlanetEarth.GetPosition(blng, blat, altB + PlanetEarth.PlanetRadius);
Vector3 dir = posB - posA;
dir.Normalize();
Vector3 bi = Vector3.Cross(n, dir);
bi.Normalize();
n = Vector3.Cross(dir, bi);
// 采集车旋转World矩阵由向量基构建
// constitute a final world matrix using these vectors
Matrix result = Matrix.Identity;
result.Right = bi;
result.Up = n;
result.Forward = -dir;
return(Quaternion.RotationMatrix(result));
}