Develop Reference

Finding the true anomaly from state vectors

I'm attempting to convert from state vectors (position and speed) into Kepler elements, however I'm running into problems where a negative velocity or position will give me wrong results when trying to calculate true anomaly.
Here are the different ways I'm trying to calculate the True Anomaly:
/// <summary>
/// https://en.wikipedia.org/wiki/True_anomaly#From_state_vectors
/// </summary>
public static double TrueAnomaly(Vector4 eccentVector, Vector4 position, Vector4 velocity)
{
var dotEccPos = Vector4.Dot(eccentVector, position);
var talen = eccentVector.Length() * position.Length();
talen = dotEccPos / talen;
talen = GMath.Clamp(talen, -1, 1);
var trueAnomoly = Math.Acos(talen);
if (Vector4.Dot(position, velocity) < 0)
trueAnomoly = Math.PI * 2 - trueAnomoly;
return trueAnomoly;
}
//sgp = standard gravitational parameter
public static double TrueAnomaly(double sgp, Vector4 position, Vector4 velocity)
{
var H = Vector4.Cross(position, velocity).Length();
var R = position.Length();
var q = Vector4.Dot(position, velocity); // dot product of r*v
var TAx = H * H / (R * sgp) - 1;
var TAy = H * q / (R * sgp);
var TA = Math.Atan2(TAy, TAx);
return TA;
}
public static double TrueAnomalyFromEccentricAnomaly(double eccentricity, double eccentricAnomaly)
{
var x = Math.Sqrt(1 - Math.Pow(eccentricity, 2)) * Math.Sin(eccentricAnomaly);
var y = Math.Cos(eccentricAnomaly) - eccentricity;
return Math.Atan2(x, y);
}
public static double TrueAnomalyFromEccentricAnomaly2(double eccentricity, double eccentricAnomaly)
{
var x = Math.Cos(eccentricAnomaly) - eccentricity;
var y = 1 - eccentricity * Math.Cos(eccentricAnomaly);
return Math.Acos(x / y);
}
Edit: another way of doing it which Spectre pointed out:
public static double TrueAnomaly(Vector4 position, double loP)
{
return Math.Atan2(position.Y, position.X) - loP;
}
Positions are all relative to the parent body.
These functions all agree if position.x, position.y and velocity.y are all positive.
How do I fix these so that I get a consistent results when position and velocity are negitive?
Just to clarify: My angles appear to be sort of correct, just pointing in the wrong quadrant depending on the position and or velocity vectors.
Yeah so I was wrong, the above all do return the correct values after all.
So I found an edge case where most of the above calculations fail.
Given position and velocity:
pos = new Vector4() { X = -0.208994076275941, Y = 0.955838328099748 };
vel = new Vector4() { X = -2.1678187689294E-07, Y = -7.93096769486992E-08 };
I get some odd results, ie ~ -31.1 degrees, when I think it should return ` 31.1 (non negative). one of them returns ~ 328.8.
However testing with this position and velocity the results apear to be ok:
pos = new Vector4() { X = -0.25, Y = 0.25 };
vel = new Vector4() { X = Distance.KmToAU(-25), Y = Distance.KmToAU(-25) };
See my answer for extra code on how I'm testing and the math I'm using for some of the other variables.
I'm going around in circles on this one. this is a result of a bug in my existing code that shows up under some conditions but not others.
I guess the real question now is WHY am I getting different results with position/velocity above that don't match to my expectations or each other?

Assuming 2D case... I am doing this differently:
compute radius of semi axises and rotation
so you need to remember whole orbit and find 2 most distant points on it that is major axis a. The minor axis b usually is 90 deg from major axis but to be sure just fins 2 perpendicularly most distant points on your orbit to major axis. So now you got both semi axises. The initial rotation is computed from the major axis by atan2.
compute true anomaly E
so if center is x0,y0 (intersection of a,b or center point of both) initial rotation is ang0 (angle of a) and your point on orbit is x,y then:
E = atan2(y-y0,x-x0) - ang0
However in order to match Newton/D'Alembert physics to Kepler orbital parameters you need to boost the integration precision like I did here:
Is it possible to make realistic n-body solar system simulation in matter of size and mass?
see the [Edit3] Improving Newton D'ALembert integration precision even more in there.
For more info and equations see:
Solving Kepler's equation
[Edit1] so you want to compute V I see it like this:
As you got your coordinates relative to parent you can assume they are already in focal point centered so no need for x0,y0 anymore. Of coarse if you want high precision and have more than 2 bodies (focal mass + object + proximity object(s) like moons) then the parent mass will no longer be in focal point of orbit but close to it ... and to remedy you need to use real focal point position so x0,y0 again... So how to do it:
compute center point (cx,cy) and a,b semi axises
so its the same as in previous text.
compute focal point (x0,y0) in orbit axis aligned coordinates
simple:
x0 = cx + sqrt( a^2 + b^2 );
y0 = cy;
initial angle ang0 of a
let xa,ya be the intersection of orbit and major axis a on the side with bigger speeds (near parent object focus). Then:
ang0 = atan2( ya-cy , xa-cx );
and finally the V fore any of yours x,y
V = atan2( y-y0 , x-x0 ) - ang0;

Ok so on further testing it appears my original calcs do all return the correct values, however when I was looking at the outputs I was not taking the LoP into account and basically not recognizing that 180 is essentially the same angle as -180.
(I was also looking at the output in radians and just didn't see what should have been obvious)
Long story short, I have a bug I thought was in this area of the code and got lost in the weeds.
Seems I was wrong above. see OP for edge case.
Here's some code I used to test these,
I used variations of the following inputs:
pos = new Vector4() { X = 0.25, Y = 0.25 };
vel = new Vector4() { X = Distance.KmToAU(-25), Y = Distance.KmToAU(25) };
And tested them with the following
double parentMass = 1.989e30;
double objMass = 2.2e+15;
double sgp = GameConstants.Science.GravitationalConstant * (parentMass + objMass) / 3.347928976e33;
Vector4 ev = OrbitMath.EccentricityVector(sgp, pos, vel);
double e = ev.Length();
double specificOrbitalEnergy = Math.Pow(vel.Length(), 2) * 0.5 - sgp / pos.Length();
double a = -sgp / (2 * specificOrbitalEnergy);
double ae = e * a;
double aop = Math.Atan2(ev.Y, ev.X);
double eccentricAnomaly = OrbitMath.GetEccentricAnomalyFromStateVectors(pos, a, ae, aop);
double aopD = Angle.ToDegrees(aop);
double directAngle = Math.Atan2(pos.Y, pos.X);
var θ1 = OrbitMath.TrueAnomaly(sgp, pos, vel);
var θ2 = OrbitMath.TrueAnomaly(ev, pos, vel);
var θ3 = OrbitMath.TrueAnomalyFromEccentricAnomaly(e, eccentricAnomaly);
var θ4 = OrbitMath.TrueAnomalyFromEccentricAnomaly2(e, eccentricAnomaly);
var θ5 = OrbitMath.TrueAnomaly(pos, aop);
double angleΔ = 0.0000001; //this is the "acceptable" amount of error, really only the TrueAnomalyFromEccentricAnomaly() calcs needed this.
Assert.AreEqual(0, Angle.DifferenceBetweenRadians(directAngle, aop - θ1), angleΔ);
Assert.AreEqual(0, Angle.DifferenceBetweenRadians(directAngle, aop - θ2), angleΔ);
Assert.AreEqual(0, Angle.DifferenceBetweenRadians(directAngle, aop - θ3), angleΔ);
Assert.AreEqual(0, Angle.DifferenceBetweenRadians(directAngle, aop - θ4), angleΔ);
Assert.AreEqual(0, Angle.DifferenceBetweenRadians(directAngle, aop - θ5), angleΔ);
and the following to compare the angles:
public static double DifferenceBetweenRadians(double a1, double a2)
{
return Math.PI - Math.Abs(Math.Abs(a1 - a2) - Math.PI);
}
And eccentricity Vector found thus:
public static Vector4 EccentricityVector(double sgp, Vector4 position, Vector4 velocity)
{
Vector4 angularMomentum = Vector4.Cross(position, velocity);
Vector4 foo1 = Vector4.Cross(velocity, angularMomentum) / sgp;
var foo2 = position / position.Length();
return foo1 - foo2;
}
And EccentricAnomaly:
public static double GetEccentricAnomalyFromStateVectors(Vector4 position, double a, double linierEccentricity, double aop)
{
var x = (position.X * Math.Cos(-aop)) - (position.Y * Math.Sin(-aop));
x = linierEccentricity + x;
double foo = GMath.Clamp(x / a, -1, 1); //because sometimes we were getting a floating point error that resulted in numbers infinatly smaller than -1
return Math.Acos(foo);
}
Thanks to Futurogogist and Spektre for their help.

I am assuming you are working in two dimensions?
Two dimensional vectors of position p and velocity v. The constant K is the the product of the gravitational constant and the mass of the gravity generating body. Calculate the eccentricity vector
eccVector = (dot(v, v)*p - dot(v, p)*v) / K - p / sqrt(dot(p, p));
eccentricity = sqrt(dot(eccVector, eccVector));
eccVector = eccVector / eccentricity;
b = { - eccVector.y, eccVector.x}; //unit vector perpendicular to eccVector
r = sqrt(dot(p, p));
cos_TA = dot(p, eccVector) / r; \\ cosine of true anomaly
sin_TA = dot(p, b) / r; \\ sine of true anomaly
if (sin_TA >= 0) {
trueAnomaly = arccos(cos_TA);
}
else if (sin_TA < 0){
trueAnomaly = 2*pi - arccos(cos_TA);
}

How to choose random float in a square weighted against arbitrary zones?

I have a square, which goes from -1 to 1 in x and y.
Choosing a random point in this square is pretty easy:
Random r = new Random();
float x = (float)Math.Round(r.NextDouble() * 2 - 1, 4);
float y = (float)Math.Round(r.NextDouble() * 2 - 1, 4);
This gives me any point, with equal probability, in my square.
It woud also be pretty easy to just remove a section of the square from the possibilities
Random r = new Random();
float x = (float)Math.Round(r.NextDouble() * 1.5 - 1, 4);
float y = (float)Math.Round(r.NextDouble() * 2 - 1, 4);
But what I'm really struggling to do, is to weight the random towards a certain zone. Specifically, I would like the section highlighted here to be more likely, and everything else (except the red section, which is still off-limits) should have a probability lower depending on the distance from the highligthed line. The furthest point should have 0 chance, and the rest an existing chance which is higher when closer to the line, with points exactly on my line (since I round them to a specific decimal, there are points with are on the line) having the best odds.
Sorry for the ugly pictures. This is the best i could do in paint to show my thoughts.
The "most likely" area is an empty diamond (just the that with the vertices (-1, 0), (0, -0.5), (1, 0), (0, 0.5), with of course the red area override the weighting because it's off limits. The red area is anything with x > 0.5
Does anyone knows how to do this? I'm working in C# but honestly an algorithm in any non-esoteric language would do the trick. I'm completely lost as to how to proceed.
A commenter noted that adding the off-limits zone to the algorithm is an added difficulty with no real use.
You can assume that I'll take care of the off-limit section by myself after running the weighting algorithm. Since it's just 25% of the area, most of the times it wouldn't even make a difference performance-wise if I just made this:
while (x > 0.5)
{
runAlgorithmAgain();
}
So you can safely ignore that part for answers.

Ok, here my thoughts on this matter. I would like to propose algorithm which, with some rejections, might solve your problem. Note, due to need of acceptance-rejection, it might be slower than you expected it to be.
We sample in single quadrant (say, lower left one), then use reflection to put point into any other quadrant, and then reject red zone points.
Basically, sampling in quadrant is two-step process. First, we sample first position on the border line. As soon as we got position on the line, we sample from distribution which is bell-like shape (Gaussian or Laplace for example), and move point in the orthogonal to the border line direction.
Code compiles, but completely untested, so please check everything startign with the numbers
using System;
namespace diamond
{
class Program
{
public const double SQRT_5 = 2.2360679774997896964091736687313;
public static double gaussian((double mu, double sigma) N, Random rng) {
var phi = 2.0 * Math.PI * rng.NextDouble();
var r = Math.Sqrt( -2.0 * Math.Log(1.0 - rng.NextDouble()) );
return N.mu + N.sigma * r * Math.Sin(phi);
}
public static double laplace((double mu, double sigma) L, Random rng) {
var v = - L.sigma * Math.Log(1.0 - rng.NextDouble());
return L.mu + ((rng.NextDouble() < 0.5) ? v : -v );
}
public static double sample_length(double lmax, Random rng) {
return lmax * rng.NextDouble();
}
public static (double, double) move_point((double x, double y) pos, (double wx, double wy) dir, double l) {
return (pos.x + dir.wx * l, pos.y + dir.wy * l);
}
public static (double, double) sample_in_quadrant((double x0, double y0) pos, (double wx, double wy) dir, double lmax, double sigma, Random rng) {
while (true) {
var l = sample_length(lmax, rng);
(double x, double y) = move_point(pos, dir, l);
var dort = (dir.wy, -dir.wx); // orthogonal to the line direction
var s = gaussian((0.0, sigma), rng); // could be laplace instead of gaussian
(x, y) = move_point((x, y), dort, s);
if (x >= -1.0 && x <= 0.0 && y >= 0.0 && y <= 1.0) // acceptance/rejection
return (x, y);
}
}
public static (double, double) sample_in_plane((double x, double y) pos, (double wx, double wy) dir, double lmax, double sigma, Random rng) {
(double x, double y) = sample_in_quadrant(pos, dir, lmax, sigma, rng);
if (rng.NextDouble() < 0.25)
return (x, y);
if (rng.NextDouble() < 0.5) // reflection over X
return (x, -y);
if (rng.NextDouble() < 0.75) // reflection over Y
return (-x, y);
return (-x, -y); // reflection over X&Y
}
static void Main(string[] args) {
var rng = new Random(32345);
var L = 0.5 * SQRT_5 + 0.5 / SQRT_5; // sampling length, BIGGER THAN JUST A SEGMENT IN THE QUADRANT
(double x0, double y0) pos = (-1.0, 0.0); // initial position
(double wx, double wy) dir = (2.0 / SQRT_5, 1.0 / SQRT_5); // directional cosines, wx*wx + wy*wy = 1
double sigma = 0.2; // that's a value to play with
// last rejection stage
(double x, double y) pt;
while(true) {
pt = sample_in_plane(pos, dir, L, sigma, rng);
if (pt.x < 0.5) // reject points in the red area, accept otherwise
break;
}
Console.WriteLine(String.Format("{0} {1}", pt.x, pt.y));
}
}
}

Reflect 2d points with matrices over a reflectionline

Given a arbitrary reflection line how can you reflect a set of points with matrices? I have tried the following but i cannot get it to work:
Translate the system so that P1 of the reflectionline is in the
origin
Rotate the system so that the reflectionline is parallel to the Y
axis
Perform a Y axis reflection
Undo the rotation
Undo the translation
Iam trying to write a method to do this for me in C# basically i give it the 2 points of the line and i get the matrix back.

No rotations are needed since there is a formula for reflecting about any line through the origin. Let (a,b) and (c,d) be any two points on the reflection line. Let's say the point you want to reflect is (x,y).
Translate the coordinates so that (a,b) becomes the origin. Then (x,y) becomes (x-a,y-b). This step is just vector subtraction.
Reflect. This is where you need the matrix. You will multiply the matrix by the translated vector from step 1. Your result will be another vector (a 2-by-1 matrix).
Translate the coordinates back to the original system. This is the inverse of step 1, i.e., this is vector addition that just undoes the vector subtraction from step 1. In this step you're just adding (a,b) to your result from step 2.
The matrix for step 2 is:
H(θ) = [cos(2θ) sin(2θ)]
[sin(2θ) -cos(2θ)]
In the matrix, θ is the angle that the (translated) reflection line makes with the positive x-axis. As long as a and c are not equal, you can find θ by evaluating:
θ = arctangent( (d-b) / (c-a) )
You will get a value strictly between -π/2 and π/2. If a = c, that is, if the reflection line is vertical, then just take θ = π/2. Although if the reflection line is vertical (or horizontal, in which case θ = 0) then you can just use well-known reflection matrices for reflecting over the y- or x-axis.
This pretty much lays out the whole process of finding and using the matrix. It sounds like all you're asking for is finding the reflection matrix. I don't know C# well enough to use it in an answer, but here's pseudocode:
// (a,b) and (c,d) are any two distinct points on the reflection line
getMatrix(double a, double b, double c, double d)
double x11, x12, x21, x22; // Elements of the reflection matrix
if a == c // If the reflection line is vertical
x11 = -1; x12 = 0; x21 = 0; x22 = 1;
else if b == d // If the reflection line is horizontal
x11 = 1; x12 = 0; x21 = 0; x22 = -1;
else
double θ = arctangent( (d-b) / (c-a) );
x11 = cos(2 * θ);
x12 = sin(2 * θ);
x21 = x12; // sin(2 * θ) again
x22 = -x11; // -cos(2 * θ)
end if
return Matrix(x11, x12, x21, x22);
/* The above line returns a matrix with the following entries:
[ x11 x12 ]
[ x21 x22 ]
*/
And here's example pseudocode for using the above pseudocode:
// Reflect (x,y) over the line given by the points (a,b) and (c,d)
reflectPoint(double x, double y, double a, double b, double c, double d)
Matrix reflector = getMatrix(a, b, c, d);
Vector v1 = new Vector(x-a, x-b); // This is Step 1
Vector v2 = new Vector(a, b); // This is so we can do Step 3 later
return reflector * v1 + v2; // v1 already has Step 1 done
// reflector * v1 is Step 2
// + v2 is Step 3
There are more efficient ways of doing the above (like checking if one of the given points (a,b) and (c,d) is already the origin, for example) but the above should still work.

After fixing the typo i made here is what my code looks like:
private Transformer2D Reflect(Vector2D p1, Vector2D p2)
{
var translationMatrix = new TranslateTransformation2D(new Vector2D(0, 0) - p1);
var inverseTranslationMatrix = new TranslateTransformation2D(p1);
var translatedP2 = translationMatrix.Transform(p2); //What p2 would be if p1 of the line was translated to the origin.
var angleWithYaxis = new Vector2D(0, 1).AngleBetweenTwoVectors(translatedP2);
var rotationMatrix = new RotationTransformation2D(-angleWithYaxis * RhinoMath.Deg2Rad);
var inverseRotationMatrix = new RotationTransformation2D(angleWithYaxis * RhinoMath.Deg2Rad);
var reflectionMatrix = new ScaleTransformation2D(-1, 1);
return new Transformer2D(translationMatrix, rotationMatrix, reflectionMatrix, inverseRotationMatrix, inverseTranslationMatrix);
}
The classes iam using just abstract the matrices:
public class TranslateTransformation2D : MatrixTransformation2DBase
{
public TranslateTransformation2D(Vector2D translation)
{
TransformationMatrix = Matrix3x3.CreateTranslationMatrix(translation.X, translation.Y);
}
public TranslateTransformation2D(float x, float y)
{
TransformationMatrix = Matrix3x3.CreateTranslationMatrix(x, y);
}
}
This is how the Matrix3x3 class looks like:
public class Matrix3x3 : Matrix
{
public Matrix3x3(double m11, double m12, double m13,
double m21, double m22, double m23,
double m31, double m32, double m33)
{
Rows = 3;
Cols = 3;
Mat = new double[Rows * Cols];
Mat[0] = m11;
Mat[1] = m12;
Mat[2] = m13;
Mat[3] = m21;
Mat[4] = m22;
Mat[5] = m23;
Mat[6] = m31;
Mat[7] = m32;
Mat[8] = m33;
}
public static Matrix3x3 CreateTranslationMatrix(double x, double y)
{
return new Matrix3x3(1, 0, x,
0, 1, y,
0, 0, 1);
}
public static Matrix3x3 CreateScaleMatrix(double x, double y)
{
return new Matrix3x3(x, 0, 0,
0, y, 0,
0, 0, 1);
}
public static Matrix3x3 CreateIdentityMatrix()
{
return new Matrix3x3(1, 0, 0,
0, 1, 0,
0, 0, 1);
}
public static Matrix3x3 CreateRotationMatrix(double radians)
{
var cos = Math.Cos(radians);
var sin = Math.Sin(radians);
return new Matrix3x3(cos, -sin, 0,
sin, cos, 0,
0, 0, 1);
}
The Transformer2D class is special here since it simply combines all the transformations into one matrix so we only have to apply this matrix to get all our transformations:
public class Transformer2D : MatrixTransformation2DBase
{
public Transformer2D(params IMatrixTransformation2D[] transformations)
{
for (int i = transformations.Length - 1; i >= 0; i--)
{
var matrixTransformation2D = transformations[i];
if (TransformationMatrix != null)
{
TransformationMatrix = TransformationMatrix * matrixTransformation2D.TransformationMatrix;
}
else
{
TransformationMatrix = matrixTransformation2D.TransformationMatrix;
}
}
}
}
The MatrixTransformation2DBase class
public abstract class MatrixTransformation2DBase : IMatrixTransformation2D
{
public Matrix3x3 TransformationMatrix { get; protected set; }
public Vector2D Transform(Vector2D vector2Din)
{
return vector2Din*TransformationMatrix;
}
}
I could probably make it faster in a few places but the idea is that i dont have to worry anymore about the matrices themselves unless i want some new type of transformation.
For those wondering what matrix class i use internally its this one: https://github.com/darkdragon-001/LightweightMatrixCSharp
All i did was write some conveinence around this.

how can I use a matrix when I need a vector?

I have to make an iterpolation fo f(x,y) so I have a double[,].
For the bicubic spline interpolation I uses alglib.
public static int Main(string[] args)
{
//
// We use bilinear spline to interpolate f(x,y)=x^2+2*y^2 sampled
// at (x,y) from [0.0, 0.5, 1.0] X [0.0, 1.0].
//
double[] x = new double[]{0.0,0.5,1.0};
double[] y = new double[]{0.0,1.0};
double[] f = new double[]{0.00,0.25,1.00,2.00,2.25,3.00};
double vx = 0.25;
double vy = 0.50;
double v;
double dx;
double dy;
double dxy;
alglib.spline2dinterpolant s;
// build spline
alglib.spline2dbuildbicubicv(x, 3, y, 2, f, 1, out s);
// calculate S(0.25,0.50)
v = alglib.spline2dcalc(s, vx, vy);
As you see in the arguments of alglib.spline2dbuildbicubicv f is a double[]. I need to apply it with may f double[,].
How can I solve this?

C# convert magnitude/phase to real/imaginary

If I'm given the Polar coordinates of a Fourier transform and I want to go back to the Cartesian (Real/Imaginary) coordiates, how would I go about doing that?
I'm able to get the Polar numbers from the Cartesian coordiates with the following code:
private double GetPhase(double real, double imaginary)
{
return Math.Atan2(imaginary, real);
}
private double GetMagnitude(double real, double imaginary)
{
return Math.Sqrt((real * real) + (imaginary * imaginary));
}
But how do I go back?

Isn't that just:
(pseudocode)
x = cos(angle) * magnitude
y = sin(angle) * magnitude
(use negative sin if you're using computer's inverted coordinate system)
?

To add to #BlueMonkMN's answer:
private double GetX (double angle, double magnitude)
{
return Math.Cos(angle) * magnitude;
}
private double GetY (double angle, double magnitude)
{
return Math.Sin(angle) * magnitude;
}