/// <summary>
/// 单圈Lambert方程求解(转移角度:[0,2*pi))
/// <para>Gm,r,v,tof单位要一致</para>
/// <para>根据初始位置速度r1,v1确定转移轨道的方向,进而确定转移轨道的角度theta!</para>
/// <para>程序内部调用采用R.H.Gooding 方法的子程序VLamb</para>
/// </summary>
/// <param name="Gm">引力常数</param>
/// <param name="r1">初始位置矢量</param>
/// <param name="v1">初始速度矢量</param>
/// <param name="r2">末态位置矢量</param>
/// <param name="v2">末态速度矢量</param>
/// <param name="tof">转移时间</param>
/// <param name="dv1">初始速度增量</param>
/// <param name="dv2">末态速度增量</param>
public static void Lambert_RhGooding(double Gm, Cartesian r1, Cartesian v1, Cartesian r2, Cartesian v2, double tof, out Cartesian dv1, out Cartesian dv2)
{
double cos_theta = r1.Dot(r2) / r1.Magnitude / r2.Magnitude;
// 由初始位置速度矢量计算角动量方向,并计算转移轨道的的法向方向
UnitCartesian h1 = r1.Cross(v1).Normalize();
Cartesian h12 = r1.Cross(r2);
// 若始末位置在同一直线上，则令转移轨道的角动量方向同初始角动量方向相同
if (h12.Magnitude / r1.Magnitude / r2.Magnitude < 1e-10)
{
h12 = h1;
}
else
{
h12 = h12.Normalize();
}
// 根据初始角动量方向和始末位置的叉乘方向，计算转移轨道的转移角度theta，使其在[0,2*pi]范围内
// 也即确定转移轨道的角动量与初始角动量方向一致
double test_dir = h1.Dot(h12);
double theta = Math.Acos(cos_theta);
if (test_dir < 0)
{
theta = 2.0 * Math.PI - theta;
h12 = -h12;
}
// 求解Lambert方程
int n;
double vr11, vt11, vr12, vt12, vr21, vt21, vr22, vt22;
VLAMB(Gm, r1.Magnitude, r2.Magnitude, theta, tof, out n, out vr11, out vt11, out vr12, out vt12, out vr21, out vt21, out vr22, out vt22);
if (n != 1)
{
throw new Exception("单圈Lambert方程求解出错，解个数应为1!");
}
// 确定始末位置的单位速度方向(垂直于位置矢径)
UnitCartesian vi_ = h12.Cross(r1).Normalize();
UnitCartesian vf_ = h12.Cross(r2).Normalize();
// 解
dv1 = vt11 * vi_ + vr11 * r1.Normalize() - v1; //
dv2 = vt12 * vf_ + vr12 * r2.Normalize() - v2; //
dv1 = vt11 * vi_ + vr11 * r1.Normalize();
dv2 = vt12 * vf_ + vr12 * r2.Normalize();
}
/// <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));
}
public void TestCrossProduct()
{
double angle = Math.PI / 4.0;
double cos = Math.Cos(angle / 2.0);
double sin = Math.Sin(angle / 2.0);
double a = cos * cos - sin * sin / 3.0;
double b = 2.0 * (sin * sin + sin * cos * Math.Sqrt(3.0)) / 3.0;
double c = 2.0 * (sin * sin - sin * cos * Math.Sqrt(3.0)) / 3.0;
// The three vectors here are the orthonormal set obtained by rotating
// the x-axis, y-axis, and z-axis through an angle of 45 degrees about
// the (1,1,1) vector.
Cartesian first = new Cartesian(a, b, c);
Cartesian second = new Cartesian(c, a, b);
Cartesian third = new Cartesian(b, c, a);
Cartesian result = first.Cross(second);
Assert.AreEqual(third.X, result.X, Constants.Epsilon14);
Assert.AreEqual(third.Y, result.Y, Constants.Epsilon14);
Assert.AreEqual(third.Z, result.Z, Constants.Epsilon14);
}
public void TestCrossProduct()
{
double angle = Math.PI / 4.0;
double cos = Math.Cos(angle / 2.0);
double sin = Math.Sin(angle / 2.0);
double a = cos * cos - sin * sin / 3.0;
double b = 2.0 * (sin * sin + sin * cos * Math.Sqrt(3.0)) / 3.0;
double c = 2.0 * (sin * sin - sin * cos * Math.Sqrt(3.0)) / 3.0;
// The three vectors here are the orthonormal set obtained by rotating
// the x-axis, y-axis, and z-axis through an angle of 45 degrees about
// the (1,1,1) vector.
Cartesian first = new Cartesian(a, b, c);
Cartesian second = new Cartesian(c, a, b);
Cartesian third = new Cartesian(b, c, a);
Cartesian result = first.Cross(second);
Assert.AreEqual(third.X, result.X, Constants.Epsilon14);
Assert.AreEqual(third.Y, result.Y, Constants.Epsilon14);
Assert.AreEqual(third.Z, result.Z, Constants.Epsilon14);
}
