/// <summary>
/// LVLH坐标系到轨道坐标系的转换矩阵
/// <para>轨道系: X->水平向前;Y->天顶R方向;
/// </summary>
/// <returns></returns>
public static Matrix3By3 LVLH2GuiDao()
{
ElementaryRotation rotz = new ElementaryRotation(AxisIndicator.Third, 0.5 * Math.PI);
ElementaryRotation rotx = new ElementaryRotation(AxisIndicator.First, Math.PI);
return(rotx.Multiply(rotz));
}
/// <summary>
/// 惯性系到飞行器LVLH坐标系的转换矩阵
/// </summary>
/// <param name="r">惯性系下的位置矢量</param>
/// <param name="V">惯性系下的速度矢量</param>
/// <returns></returns>
public static Matrix3By3 Initial2LVLH(Cartesian r, Cartesian v)
{
UnitCartesian h = new UnitCartesian(r.Cross(v));
Cartesian uz = new Cartesian(0, 0, 1);
//升交点赤径所在单位向量坐标
UnitCartesian uraan = new UnitCartesian(uz.Cross(h));
//轨道倾角
double inc = Math.Acos(h.Z);
//升交点赤径
double raan = Math.Atan2(uraan.Y, uraan.X);
//w+f
double u = Math.Acos(r.Dot(uraan) / r.Magnitude);
if (r.Z < 0)
{
u = -u;
}
ElementaryRotation rot1 = new ElementaryRotation(AxisIndicator.Third, raan);
ElementaryRotation rot2 = new ElementaryRotation(AxisIndicator.First, inc);
ElementaryRotation rot3 = new ElementaryRotation(AxisIndicator.Third, u);
return(rot3.Multiply(rot2).Multiply(rot1));
}
/// <summary>
/// LVLH坐标系到VNC坐标系的转换矩阵
/// </summary>
/// <param name="r">惯性系下的位置矢量</param>
/// <param name="V">惯性系下的速度矢量</param>
/// <returns></returns>
public static Matrix3By3 LVLH2VNC(Cartesian r, Cartesian v)
{
//飞行路径角（垂直）
double vtFPA = Math.Acos(r.Dot(v) / r.Magnitude / v.Magnitude);
ElementaryRotation rotz = new ElementaryRotation(AxisIndicator.Third, vtFPA);
ElementaryRotation rotx = new ElementaryRotation(AxisIndicator.First, 0.5 * Math.PI);
return(rotx.Multiply(rotz));
}
static void Main(string[] args)
{
// 球坐标转为笛卡尔坐标
var spherical0 = new Spherical(Math.PI / 4, Math.PI / 8, 100.0);
var cartesian0 = new Cartesian(spherical0);
Console.WriteLine("球坐标：{0}；笛卡尔坐标：{1}", spherical0, cartesian0);
// 笛卡尔坐标归一化
UnitCartesian unitCartesian1 = cartesian0.Normalize();
Console.WriteLine("单位矢量笛卡尔坐标：{0}", unitCartesian1);
// 地图坐标转为笛卡尔坐标
var cartographic2 = new Cartographic(Trig.DegreesToRadians(120), Trig.DegreesToRadians(30), 100);
EarthCentralBody earth = CentralBodiesFacet.GetFromContext().Earth;
Cartesian cartesian2 = earth.Shape.CartographicToCartesian(cartographic2);
Console.WriteLine("地图坐标：{0}；笛卡尔坐标：{1}", cartographic2, cartesian2);
// 笛卡尔坐标转为地图坐标
Cartographic cartographic3 = earth.Shape.CartesianToCartographic(cartesian2);
Console.WriteLine("笛卡尔坐标：{0}；地图坐标：{1}", cartesian2, cartographic3);
// 新坐标系绕原坐标系z轴旋转90度，原向量(1,0,0)
var vector4 = new Cartesian(1, 0, 0);
var rotation4 = new ElementaryRotation(AxisIndicator.Third, Trig.DegreesToRadians(90));
Cartesian newVector4 = new Matrix3By3(rotation4).Multiply(vector4);
Console.WriteLine("旋转前：{0}；旋转后：{1}", vector4, newVector4);
// 欧拉旋转
var vector5 = new Cartesian(1, 0, 0);
double angle = Trig.DegreesToRadians(90);
var euler = new EulerSequence(angle, angle, angle, EulerSequenceIndicator.Euler321);
Cartesian newVector5 = new Matrix3By3(euler).Multiply(vector5);
Console.WriteLine("旋转前：{0}；旋转后：{1}", vector5, newVector5);
// 偏航俯仰翻滚旋转
var vector6 = new Cartesian(1, 0, 0);
double angle6 = Trig.DegreesToRadians(90);
var ypr = new YawPitchRoll(angle, angle, angle, YawPitchRollIndicator.YPR);
Cartesian newVector6 = new Matrix3By3(ypr).Multiply(vector6);
Console.WriteLine("旋转前：{0}；旋转后：{1}", vector6, newVector6);
Console.Read();
}
