We Keep Coding

How to calculate endPoint using initial point, distance and azimuth?

I need to find the endpoint using the start point, distance and azimuth. Here are the values of each one:
latitude: 37.624942,
longitude": -7.896333,
azimute: 233.0
distance : 20.0
Here is my function:
private List<double> findEndPoint(string latitudeStart, string longitudeStart, string azimute,double distancia)
{
List<double> endPoint = new List<double>();
double latitudeStartDouble = Convert.ToDouble(latitudeStart, System.Globalization.CultureInfo.InvariantCulture);
double longitudeStartDouble = Convert.ToDouble(longitudeStart, System.Globalization.CultureInfo.InvariantCulture);
double azimuteDouble = Convert.ToDouble(azimute, System.Globalization.CultureInfo.InvariantCulture);
double azimuteRadians = ConvertToRadians(azimuteDouble);
double R = 6371.0; // Raio da Terra em km
double latitudeEnd = Math.Asin(Math.Sin(latitudeStartDouble) * Math.Cos(distancia / R) +
Math.Cos(latitudeStartDouble) * Math.Sin(distancia / R) * Math.Cos(azimuteRadians));
endPoint.Add(latitudeEnd);
double longitudeEnd = longitudeStartDouble + Math.Atan2(
Math.Sin(azimuteRadians) * Math.Sin(distancia / R) * Math.Cos(latitudeStartDouble),
Math.Cos(distancia / R) - Math.Sin(latitudeStartDouble) * Math.Sin(latitudeEnd));
endPoint.Add(longitudeEnd);
return endPoint;
}
It returns:
latitude: -0.0760588400705975
longitude:-7.8988473639987093
The latitude must be wrong, but I don't know why is it giving me that value.

You forgot to add latitudeStartDouble. You just calculate the delta. So latitudeEnd = latitudeStartDouble + ...etc...

I need to be working with radians and then converting it back to degrees:
private List<double> findEndPoint(string latitudeStart, string longitudeStart, string azimute,double distancia)
{
List<double> endPoint = new List<double>();
double latitudeStartDouble = Convert.ToDouble(latitudeStart, System.Globalization.CultureInfo.InvariantCulture);
latitudeStartDouble = ConvertToRadians(latitudeStartDouble);
double longitudeStartDouble = Convert.ToDouble(longitudeStart, System.Globalization.CultureInfo.InvariantCulture);
longitudeStartDouble = ConvertToRadians(longitudeStartDouble);
double azimuteDouble = Convert.ToDouble(azimute, System.Globalization.CultureInfo.InvariantCulture);
double azimuteRadians = ConvertToRadians(azimuteDouble);
double R = 6371; // Raio da Terra em km
double latitudeEnd = Math.Asin(Math.Sin(latitudeStartDouble) * Math.Cos(distancia / R) +
Math.Cos(latitudeStartDouble) * Math.Sin(distancia / R) * Math.Cos(azimuteRadians));
latitudeEnd = ConvertToDegrees(latitudeEnd);
endPoint.Add(latitudeEnd);
double longitudeEnd = longitudeStartDouble + Math.Atan2(Math.Sin(azimuteRadians) * Math.Sin(distancia / R) * Math.Cos(latitudeStartDouble),
Math.Cos(distancia / R) - Math.Sin(latitudeStartDouble) * Math.Sin(latitudeEnd));
longitudeEnd = ConvertToDegrees(longitudeEnd);
endPoint.Add(longitudeEnd);
return endPoint;
}

Solving equation to find center point of circle from 3 points

I'm looking for a high precision solution to find the center point of a circle from 3 data points on a canvas (x,y). I found this example in the attached screenshot above, now I'm using the Math.NET package to solve the equation and I'm comparing the results against this online tool: https://planetcalc.com/8116/.
However, when I calculate the radius its completely off and often a negative number???
using MathNet.Numerics.LinearAlgebra.Double.Solvers;
using MathNet.Numerics.LinearAlgebra.Double;
using System;
namespace ConsoleAppTestBed
{
class Program
{
static void Main(string[] args)
{
var dataPoints = new double[,]
{
{ 5, 80 },
{ 20, 100 },
{ 40, 140 }
};
var fitter = new CircleFitter();
var result = fitter.Fit(dataPoints);
var x = -result[0];
var y = -result[1];
var c = result[2];
Console.WriteLine("Center Point:");
Console.WriteLine(x);
Console.WriteLine(y);
Console.WriteLine(c);
//// (x^2 + y^2 - c^2)
var radius = Math.Pow(x, 2) + Math.Pow(y, 2) - Math.Pow(c, 2);
//// sqrt((x^2 + y^2 - c^2))
radius = Math.Sqrt(radius);
Console.WriteLine("Radius:");
Console.WriteLine(radius);
Console.ReadLine();
}
public class CircleFitter
{
public double[] Fit(double[,] v)
{
var xy1 = new double[] { v[0,0], v[0,1] };
var xy2= new double[] { v[1, 0], v[1, 1] };
var xy3 = new double[] { v[2, 0], v[2, 1] };
// Create Left Side Matrix of Equation
var a = CreateLeftSide_(xy1);
var b = CreateLeftSide_(xy2);
var c = CreateLeftSide_(xy3);
var matrixA = DenseMatrix.OfArray(new[,]
{
{ a[0], a[1], a[2] },
{ b[0], b[1], b[2] },
{ c[0], c[1], c[2] }
});
// Create Right Side Vector of Equation
var d = CreateRightSide_(xy1);
var e = CreateRightSide_(xy2);
var f = CreateRightSide_(xy3);
double[] vector = { d, e, f };
var vectorB = Vector<double>.Build.Dense(vector);
// Solve Equation
var r = matrixA.Solve(vectorB);
var result = r.ToArray();
return result;
}
//2x, 2y, 1
public double[] CreateLeftSide_(double[] d)
{
return new double[] { (2 * d[0]), (2 * d[1]) , 1};
}
// -(x^2 + y^2)
public double CreateRightSide_(double[] d)
{
return -(Math.Pow(d[0], 2) + Math.Pow(d[1], 2));
}
}
}
}
Any ideas?
Thanks in advance.

The solution to your problem is here: The NumberDecimalDigits property
Code:
using System;
using System.Globalization;
namespace ConsoleApp1
{
class Program
{
static void Main()
{
double x1 = 1, y1 = 1;
double x2 = 2, y2 = 4;
double x3 = 5, y3 = -3;
findCircle(x1, y1, x2, y2, x3, y3);
Console.ReadKey();
}
static void findCircle(double x1, double y1,
double x2, double y2,
double x3, double y3)
{
NumberFormatInfo setPrecision = new NumberFormatInfo();
setPrecision.NumberDecimalDigits = 3; // 3 digits after the double point
double x12 = x1 - x2;
double x13 = x1 - x3;
double y12 = y1 - y2;
double y13 = y1 - y3;
double y31 = y3 - y1;
double y21 = y2 - y1;
double x31 = x3 - x1;
double x21 = x2 - x1;
double sx13 = (double)(Math.Pow(x1, 2) -
Math.Pow(x3, 2));
double sy13 = (double)(Math.Pow(y1, 2) -
Math.Pow(y3, 2));
double sx21 = (double)(Math.Pow(x2, 2) -
Math.Pow(x1, 2));
double sy21 = (double)(Math.Pow(y2, 2) -
Math.Pow(y1, 2));
double f = ((sx13) * (x12)
+ (sy13) * (x12)
+ (sx21) * (x13)
+ (sy21) * (x13))
/ (2 * ((y31) * (x12) - (y21) * (x13)));
double g = ((sx13) * (y12)
+ (sy13) * (y12)
+ (sx21) * (y13)
+ (sy21) * (y13))
/ (2 * ((x31) * (y12) - (x21) * (y13)));
double c = -(double)Math.Pow(x1, 2) - (double)Math.Pow(y1, 2) -
2 * g * x1 - 2 * f * y1;
double h = -g;
double k = -f;
double sqr_of_r = h * h + k * k - c;
// r is the radius
double r = Math.Round(Math.Sqrt(sqr_of_r), 5);
Console.WriteLine("Center of a circle: x = " + h.ToString("N", setPrecision) +
", y = " + k.ToString("N", setPrecision));
Console.WriteLine("Radius: " + r.ToString("N", setPrecision));
}
}
}

I have just converted William Li's answer to Swift 5 for
those who like to cmd+C and cmd+V like me :)
func calculateCircle(){
let x1:Float = 0
let y1:Float = 0
let x2:Float = 0.5
let y2:Float = 0.5
let x3:Float = 1
let y3:Float = 0
let x12 = x1 - x2
let x13 = x1 - x3
let y12 = y1 - y2
let y13 = y1 - y3
let y31 = y3 - y1
let y21 = y2 - y1
let x31 = x3 - x1
let x21 = x2 - x1
let sx13 = pow(x1, 2) - pow(x3, 2)
let sy13 = pow(y1, 2) - pow(y3, 2)
let sx21 = pow(x2, 2) - pow(x1, 2)
let sy21 = pow(y2, 2) - pow(y1, 2)
let f = ((sx13) * (x12)
+ (sy13) * (x12)
+ (sx21) * (x13)
+ (sy21) * (x13))
/ (2 * ((y31) * (x12) - (y21) * (x13)))
let g = ((sx13) * (y12)
+ (sy13) * (y12)
+ (sx21) * (y13)
+ (sy21) * (y13))
/ (2 * ((x31) * (y12) - (x21) * (y13)))
let c = -pow(x1, 2) - pow(y1, 2) - 2 * g * x1 - 2 * f * y1
let h = -g
let k = -f
let r = sqrt(h * h + k * k - c)
print("center x = \(h)")
print("center y = \(k)")
print("r = \(r)")
}

Updated Answer
The equation for radius is incorrect; it should be (not c squared):
which is why you get incorrect values for the radius.
The original answer was incorrect, but it is still 'interesting'.
(Incorrect) Original Answer
The cause of the problematic calculation is not solely due to the precision of the numbers, but more because problem is ill-conditioned. If you look at the three points and where they are located on the circle, you'll see that they are bunched on a small segment of the circumference. When the points are so close to each other, it is a tough ask to find the circle's centre and radius accurately.
So the intermediate calculations, which will have small rounding errors, result in hugely exaggerated errors.
You can see the ill-conditioned nature of the problem by adding the ConditionNumber() method.
// Solve Equation
Console.WriteLine(matrixA.ConditionNumber()); // <<< Returns 5800 -> Big!
var r = matrixA.Solve(vectorB); // Existing code
A large result indicates an ill-conditioned problem. In this case a value of 5800 is returned, which is large. You might get better results using Gaussian Elimination with partial pivoting, but it still does not address the fact that the basic problem is ill-conditioned, which is why you get wildly incorrect answers.

What is wrong in this math equation in C#?

I have a math equation and I want use it in my program
but I got an error. The result from this equation is non number ..
the math equation i:
note: n is the number in the textbox
((298/2)^2*ACOS(((298/2)-n)/(298/2))-((298/2)-n)*((2*(298/2)*n-n^2))^(0.5))*1213/1000
in button code
double a = Math.Pow((298.00 / 2), 2);
double b = (Math.Acos(((298.00 / 2) - Convert.ToDouble(textBox1.Text)) / (298.00 / 2)));
double c = ((298.00 / 2) - Convert.ToDouble(textBox1.Text));
double d = (2 * (298.00 / 2) * Convert.ToDouble(textBox1.Text));
double f = Math.Pow(Convert.ToDouble(textBox1.Text), 2);
double r = ((a * b) - (c * d) )- f;
double result = Math.Pow(r, (0.5));
double h = result * 1213.00 / 1000;
textBox1.Text = Convert.ToString(h );
}
If anyone knows what is wrong here, tell me please.
The error is in variable r because the result from r is less that 0!

Lets rewrite given math expression in more convenient way:
( (298/2)^2 * ACOS(((298/2)-n)/(298/2)) - ((298/2)-n) * ( (2*(298/2)*n - n^2 ) ) ^ (0.5) ) * 1213/1000
--------- ------------------------- ----------- ------------ ---
a b c d f
--------------------------------
r = (d - f) ^ 0.5
------------------------------------------------------------------------------------------------------
(a * b - c * r) * 1213/1000
Now it is easier to see that you have an error in formula:
double r = ((a * b) - (c * d) )- f;
r must be calculated using the next formula:
double r = Math.Sqrt(d - f); // It is better to use Math.Sqrt(x) instead of Math.Pow(x, 0.5).
Now knowing where the error is we can fix it:
double n = Convert.ToDouble(textBox1.Text);
double a = Math.Pow(298.00 / 2, 2);
double b = Math.Acos( (298.00 / 2 - n) / (298.00 / 2) );
double c = 298.00 / 2 - n;
double d = 2 * 298.00 / 2 * n;
double f = Math.Pow(n, 2);
double r = Math.Sqrt(d - f);
double result = a * b - c * r;
double h = result * 1213.00 / 1000;
textBox1.Text = Convert.ToString(h);
But for values of n > 298 you will still get r = NaN because for such values of n expression d - f is negative.

2D Trilateration using linear least squares in C#

I am doing 2D trilateration using the MathNet for the matrices and vectors. This i my code:
public static double[] trilaterate2DLinear(double[] pA, double[] pB, double[] pC, double rA, double rB, double rC) {
//Convert doubles to vectors for processing
Vector<double> vA = Vector<double>.Build.Dense(pA);
Vector<double> vB = Vector<double>.Build.Dense(pB);
Vector<double> vC = Vector<double>.Build.Dense(pC);
//Declare elements of b vector
//bBA = 1/2 * (rA^2 - rB^2 + dBA^2)
double[] b = {0, 0};
b[0] = 0.5 * (Math.Pow(rA, 2) - Math.Pow(rB, 2) + Math.Pow(getDistance(pB, pA), 2));
b[1] = 0.5 * (Math.Pow(rA, 2) - Math.Pow(rC, 2) + Math.Pow(getDistance(pC, pA), 2));
//Convert b array to vector form
Vector<double> vb = Vector<double>.Build.Dense(b);
//Build A array
//A = {x2 -x1, y2 - y1}
// {x3 - x1, y3 - y1}
double[,] A = { { pB[0] - pA[0], pB[1] - pA[1] }, { pC[0] - pA[0], pC[1] - pA[1] } };
//Convert A to Matrix form
Matrix<double> mA = Matrix<double>.Build.DenseOfArray(A);
//Declare Transpose of A matrix;
Matrix<double> mAT = mA.Transpose();
//Declare solution vector x to 0
Vector<double> x = Vector<double>.Build.Dense(2);
//Check if A*AT is non-singular (non 0 determinant)
if (mA.Multiply(mAT).Determinant() == 0)
{
//x = ((AT * A)^-1)*AT*b
x = (((mA.Multiply(mAT)).Inverse()).Multiply(mAT)).Multiply(vb);
}
else
{
//TODO case for A*AT to be singular
x = (((mA.Multiply(mAT)).Inverse()).Multiply(mAT)).Multiply(vb);
}
//final position is x + vA
//return as double so as not
return (x.Add(vA)).ToArray();
}
//Gets the Euclidean distance between two points
private static double getDistance(double[] p1, double[] p2)
{
//d^2 = (p1[0] - p2[0])^2 + (p1[1] - p2[1]);
double distSquared = Math.Pow((p1[0] - p2[0]),2) + Math.Pow((p1[1] - p2[1]),2);
return Math.Sqrt(distSquared);
}
pA, pB & pC are the coordinates of the the Beacons and rA, rB & rC are the distances from the each beacon to the user. Is there anything obvious I am doing wrong? Maybe the order of Matrix multiplications need to change but I am not familiar enough with the Linear Least Squares to be able to track the Matrices and tell.

Solved. The if statement condition and calculations inside the if statement where wrong.
Correction:
public static double[] trilaterate2DLinear(double[] pA, double[] pB, double[] pC, double rA, double rB, double rC) {
//Convert doubles to vectors for processing
Vector<double> vA = Vector<double>.Build.Dense(pA);
Vector<double> vB = Vector<double>.Build.Dense(pB);
Vector<double> vC = Vector<double>.Build.Dense(pC);
//Declare elements of b vector
//bBA = 1/2 * (rA^2 - rB^2 + dBA^2)
double[] b = {0, 0};
b[0] = 0.5 * (Math.Pow(rA, 2) - Math.Pow(rB, 2) + Math.Pow(getDistance(pB, pA), 2));
b[1] = 0.5 * (Math.Pow(rA, 2) - Math.Pow(rC, 2) + Math.Pow(getDistance(pC, pA), 2));
//Convert b array to vector form
Vector<double> vb = Vector<double>.Build.Dense(b);
//Build A array
//A = {x2 -x1, y2 - y1}
// {x3 - x1, y3 - y1}
double[,] A = { { pB[0] - pA[0], pB[1] - pA[1] }, { pC[0] - pA[0], pC[1] - pA[1] } };
//Convert A to Matrix form
Matrix<double> mA = Matrix<double>.Build.DenseOfArray(A);
//Declare Transpose of A matrix;
Matrix<double> mAT = mA.Transpose();
//Declare solution vector x to 0
Vector<double> x = Vector<double>.Build.Dense(2);
//Check if A*AT is non-singular (non 0 determinant)
double det = mA.Multiply(mAT).Determinant();
if (mA.Multiply(mAT).Determinant() > 0.1)
{
//x = ((AT * A)^-1)*AT*b
// x = (((mA.Multiply(mAT)).Inverse()).Multiply(mAT)).Multiply(vb);
x = (mA.Transpose() * mA).Inverse() * (mA.Transpose() * vb);
}
else
{
//TODO case for A*AT to be singular
x = (((mA.Multiply(mAT)).Inverse()).Multiply(mAT)).Multiply(vb);
}
//final position is x + vA
//return as double so as not
return (x.Add(vA)).ToArray();
}

You are not calculating the B vector correctly.
It should be:
//dBA = 0.5 * (rA^2 - rB^2 - length_vA^2 + length_vB^2)
b[0] = 0.5 * (Math.Pow(rA, 2) - Math.Pow(rB, 2) - Math.Pow(getDistance(pA,{0,0}), 2) + Math.Pow(getDistance(pB,{0,0}), 2));
b[1] = 0.5 * (Math.Pow(rA, 2) - Math.Pow(rC, 2) - Math.Pow(getDistance(pA,{0,0}), 2) + Math.Pow(getDistance(pC,{0,0}), 2));

How would i work out magnitude quickly for 3 values?

How can I use a Fast Magnitude calculation for 3 values (instead of using square root)? (+/- 3% is good enough)
public void RGBToComparison(Color32[] color)
{
DateTime start = DateTime.Now;
foreach (Color32 i in color)
{
var r = PivotRgb(i.r / 255.0);
var g = PivotRgb(i.g / 255.0);
var b = PivotRgb(i.b / 255.0);
var X = r * 0.4124 + g * 0.3576 + b * 0.1805;
var Y = r * 0.2126 + g * 0.7152 + b * 0.0722;
var Z = r * 0.0193 + g * 0.1192 + b * 0.9505;
var LB = PivotXyz(X / 95.047);
var AB = PivotXyz(Y / 100);
var BB = PivotXyz(Z / 108.883);
var L = Math.Max(0, 116 * AB - 16);
var A = 500 * (LB - AB);
var B = 200 * (AB - BB);
totalDifference += Math.Sqrt((L-LT)*(L-LT) + (A-AT)*(A-AT) + (B-BT)*(B-BT));
}
totalDifference = totalDifference / color.Length;
text.text = "Amount of Pixels: " + color.Length + " Time(MilliSeconds):" + DateTime.Now.Subtract(start).TotalMilliseconds + " Score (0 to 100)" + (totalDifference).ToString();
RandomOrNot();
}
private static double PivotRgb(double n)
{
return (n > 0.04045 ? Math.Pow((n + 0.055) / 1.055, 2.4) : n / 12.92) * 100.0;
}
private static double PivotXyz(double n)
{
return n > 0.008856 ? CubicRoot(n) : (903.3 * n + 16) / 116;
}
private static double CubicRoot(double n)
{
return Math.Pow(n, 1.0 / 3.0);
}
This is the important part: totalDifference += Math.Sqrt((L-LT)*(L-LT) + (A-AT)*(A-AT) + (B-BT)*(B-BT));
I know there are FastMagnitude calculations online, but all the ones online are for two values, not three. For example, could i use the difference between the values to get a precise answer? (By implementing the difference value into the equation, and if the difference percentage-wise is big, falling back onto square root?)
Adding up the values and iterating the square root every 4 pixels is a last resort that I could do. But firstly, I want to find out if it is possible to have a good FastMagnitude calculation for 3 values.
I know I can multi-thread and parllelize it, but I want to optimize my code before I do that.

If you just want to compare the values, why not leave the square root out and work with the length squared?
Or use the taylor series of the square root of 1+x and cut off early :)