private static Quaternion _look(Vector3 from, Vector3 to)
{
if (from.equals(to))
{
return(identity);
}
return(angleAxis(from.angle(to), from.cross(to)));
}
private void _update(Vector3 position, Vector3 speed, double time)
{
Vector3 c = speed.cross(position);
inclination = c.angle(mainBody.rotation.up);
Vector3 an = c.cross(Vector3.up);
if (an.equals(Vector3.zero))
{
LAN = 0;
an = Vector3.right;
}
else
{
LAN = an.angleInPlane(Vector3.right, mainBody.rotation.up) + universe.inverseRotation / 180 * Math.PI;
}
parameter = c.magnitudeSquare / mainBody.gravParameter;
double h = speed.magnitudeSquare - 2 * mainBody.gravParameter / position.magnitude;
eccentricity = (1 + h * parameter / mainBody.gravParameter).sqrt();
double vRadial = speed.dot(position.normalize);
double vNormal = speed.exclude(position).magnitude;
if (Math.Abs(eccentricity) > 1e-3)
{
double taSin = vRadial / eccentricity * (parameter / mainBody.gravParameter).sqrt();
double taCos = 1 / eccentricity * (vNormal * (parameter / mainBody.gravParameter).sqrt() - 1);
trueAnomaly = Math.Atan2(taSin, taCos);
}
else
{
trueAnomaly = position.angleInPlane(an, c);
}
if (trueAnomaly < 0)
{
trueAnomaly += 2 * Math.PI;
}
argumentOfPeriapsis = position.angleInPlane(an, c);
if (argumentOfPeriapsis < 0)
{
argumentOfPeriapsis += 2 * Math.PI;
}
argumentOfPeriapsis -= trueAnomaly;
timeAtPeriapsis = time - timeFromPeriapsis;
}
public double angleInPlane(Vector3 vector, Vector3 normal)
{
Vector3 v1 = exclude(normal);
Vector3 v2 = vector.exclude(normal);
if (v1.equals(zero))
{
return(0);
}
if (v2.equals(zero))
{
return(0);
}
Vector3 rotationNormal = v1.cross(v2);
if (rotationNormal.equals(zero))
{
return(Math.Sign(v1.dot(v2)) > 0 ? 0 : Math.PI);
}
return(v1.angle(v2) * Math.Sign(v1.cross(v2).dot(normal)));
}
private static Quaternion _look(Vector3 from, Vector3 to)
{
if(from.equals(to))
return identity;
return angleAxis(from.angle(to), from.cross(to));
}
