I have seen many questions to conversions between Euler angles and Quaternion, but I never found any working solution. Maybe you can help me why this is not returning the right values. I need the conversion between Quaternions(XYZ) to Euler angles and this is the code I am currently using:
public static Vector3 Q2E(Quaternion q) // Returns the XYZ in ZXY
{
Vector3 angles;
angles.X = (float)Math.Atan2(2 * (q.W * q.X + q.Y * q.Z), 1 - 2 * (q.X * q.X + q.Y * q.Y));
if (Math.Abs(2 * (q.W * q.Y - q.Z * q.X)) >= 1) angles.Y = (float)Math.CopySign(Math.PI / 2, 2 * (q.W * q.Y - q.Z * q.X));
else angles.Y = (float)Math.Asin(2 * (q.W * q.Y - q.Z * q.X));
angles.Z = (float)Math.Atan2(2 * (q.W * q.Z + q.X * q.Y), 1 - 2 * (q.Y * q.Y + q.Z * q.Z));
return new Vector3()
{
X = (float)(180 / Math.PI) * angles.X,
Y = (float)(180 / Math.PI) * angles.Y,
Z = (float)(180 / Math.PI) * angles.Z
};
}
Thx everyone.
Your title is from Euler angles to Quaternions but you sample code is 'supposed' to convert from Quaternion to Euler.
Is below what you are looking for?
public class Program
{
public static void Main(string[] args)
{
EulerAngles e = new();
e.roll = 0.14;
e.pitch = 1.21;
e.yaw = 2.1;
// convert the Euler angles to Quaternions:
Quaternion q = ToQuaternion(e.yaw,e.pitch,e.roll);
// convert the same Quaternion back to Euler angles:
EulerAngles n = ToEulerAngles(q);
// verify conversion
Console.WriteLine($"Q: {q.x} {q.y} {q.z} {q.w}");
Console.WriteLine($"E: {n.roll} {n.pitch} {n.yaw}");
}
public class Quaternion
{
public double w;
public double x;
public double y;
public double z;
}
public class EulerAngles
{
public double roll; // x
public double pitch; // y
public double yaw; // z
}
public static Quaternion ToQuaternion(double yaw, double pitch, double roll)
{
double cy = Math.Cos(yaw * 0.5);
double sy = Math.Sin(yaw * 0.5);
double cp = Math.Cos(pitch * 0.5);
double sp = Math.Sin(pitch * 0.5);
double cr = Math.Cos(roll * 0.5);
double sr = Math.Sin(roll * 0.5);
Quaternion q = new Quaternion();
q.w = cr * cp * cy + sr * sp * sy;
q.x = sr * cp * cy - cr * sp * sy;
q.y = cr * sp * cy + sr * cp * sy;
q.z = cr * cp * sy - sr * sp * cy;
return q;
}
public static EulerAngles ToEulerAngles(Quaternion q)
{
EulerAngles angles = new();
// roll (x-axis rotation)
double sinr_cosp = 2 * (q.w * q.x + q.y * q.z);
double cosr_cosp = 1 - 2 * (q.x * q.x + q.y * q.y);
angles.roll = Math.Atan2(sinr_cosp, cosr_cosp);
// pitch (y-axis rotation)
double sinp = 2 * (q.w * q.y - q.z * q.x);
if (Math.Abs(sinp) >= 1)
{
angles.pitch = Math.CopySign(Math.PI / 2, sinp);
}
else
{
angles.pitch = Math.Asin(sinp);
}
// yaw (z-axis rotation)
double siny_cosp = 2 * (q.w * q.z + q.x * q.y);
double cosy_cosp = 1 - 2 * (q.y * q.y + q.z * q.z);
angles.yaw = Math.Atan2(siny_cosp, cosy_cosp);
return angles;
}
}
UPDATE: Using built-in classes for Quaternion and Euler Angles (Vector3):
using System.Numerics;
public static void Main()
{
Vector3 v = new() { X = 0.14F, Y = 1.21F, Z = 2.1F };
Quaternion q = ToQuaternion(v);
Vector3 n = ToEulerAngles(q);
Console.WriteLine($"Q: {q.X} {q.Y} {q.Z} {q.W}");
Console.WriteLine($"E: {n.X} {n.Y} {n.Z}");
}
public static Quaternion ToQuaternion(Vector3 v)
{
float cy = (float)Math.Cos(v.Z * 0.5);
float sy = (float)Math.Sin(v.Z * 0.5);
float cp = (float)Math.Cos(v.Y * 0.5);
float sp = (float)Math.Sin(v.Y * 0.5);
float cr = (float)Math.Cos(v.X * 0.5);
float sr = (float)Math.Sin(v.X * 0.5);
return new Quaternion
{
W = (cr * cp * cy + sr * sp * sy),
X = (sr * cp * cy - cr * sp * sy),
Y = (cr * sp * cy + sr * cp * sy),
Z = (cr * cp * sy - sr * sp * cy)
};
}
public static Vector3 ToEulerAngles(Quaternion q)
{
Vector3 angles = new();
// roll / x
double sinr_cosp = 2 * (q.W * q.X + q.Y * q.Z);
double cosr_cosp = 1 - 2 * (q.X * q.X + q.Y * q.Y);
angles.X = (float)Math.Atan2(sinr_cosp, cosr_cosp);
// pitch / y
double sinp = 2 * (q.W * q.Y - q.Z * q.X);
if (Math.Abs(sinp) >= 1)
{
angles.Y = (float)Math.CopySign(Math.PI / 2, sinp);
}
else
{
angles.Y = (float)Math.Asin(sinp);
}
// yaw / z
double siny_cosp = 2 * (q.W * q.Z + q.X * q.Y);
double cosy_cosp = 1 - 2 * (q.Y * q.Y + q.Z * q.Z);
angles.Z = (float)Math.Atan2(siny_cosp, cosy_cosp);
return angles;
}
Related
I am currently creating a rubiks cube project. The cube solves, but now I'm trying to implement a 3d model of this cube.
At the moment the x axis and z axis rotations work correctly, but the y axis rotation seems to start of as a cube but as it rotates round becomes more of a trapezium as it rotates 180'.
I have this code:
Point3D final;
double x = rotation.x;
final.x = original.x;
final.y = original.y * Math.Cos(x) - original.z * Math.Sin(x);
final.z = original.y * Math.Sin(x) + original.z * Math.Cos(x);
original.x = final.x;
original.y = final.y;
original.z = final.z;
x = rotation.y;
final.x = original.z * Math.Sin(x) + original.x * Math.Cos(x);
final.y = original.y;
final.z = original.y * Math.Cos(x) - original.x * Math.Sin(x);
original.x = final.x;
original.y = final.y;
original.z = final.z;
x = rotation.z;
final.x = original.x * Math.Cos(x) - original.y * Math.Sin(x);
final.y = original.x * Math.Sin(x) + original.y * Math.Cos(x);
final.z = original.z;
typo. Change line for y-rotation to
final.z = original.z * Math.Cos(x) - original.x * Math.Sin(x);
You were using original.y instead of original.z, but for a y-rotation the value of y does not play into the rotation.
May I suggest you define the rotations in methods
public static class Rotations
{
public static Point3D RotateAboutX(this Point3D point, double angle)
{
return new Point3D(
point.X,
Math.Cos(angle) * point.Y- Math.Sin(angle) * point.Z,
Math.Sin(angle) * point.Y+ Math.Cos(angle) * point.Z);
}
public static Point3D RotateAboutY(this Point3D point, double angle)
{
return new Point3D(
Math.Cos(angle) * point.X + Math.Sin(angle) * point.Z,
point.Y,
-Math.Sin(angle) * point.X + Math.Cos(angle) * point.Z);
}
public static Point3D RotateAboutZ(this Point3D point, double angle)
{
return new Point3D(
Math.Cos(angle) * point.X - Math.Sin(angle) * point.Y,
Math.Sin(angle) * point.X + Math.Cos(angle) * point.Y,
point.Z);
}
}
and then used them as needed. For Example
Point3D final = original.RotateAboutX(rotation.x)
.RotateAboutY(rotation.y)
.RotateAboutZ(rotation.z);
or the remain true to the original code
Point3D final = original.RotateAboutX(rotation.x);
original = final;
final = original.RotateAboutY(rotation.y);
original = final;
final = original.RotateAboutZ(rotation.z);
I've written the following code to calculate the length of a cubic bezier curve. I got the idea from Calculate the arclength, curve length of a cubic bezier curve. Why is not working?. The problem is it always produces a length of zero.
public Vector2 SegmentAtPoint(int segmentIndex, float t)
{
t = Mathf.Clamp01(t);
float oneMinusT = 1f - t;
return
oneMinusT * oneMinusT * oneMinusT * points[segmentIndex * 3] +
3f * oneMinusT * oneMinusT * t * points[segmentIndex * 3 + 1] +
3f * oneMinusT * t * t * points[segmentIndex * 3 + 2] +
t * t * t * points[segmentIndex * 3 + 3];
}
public float SegmentLength(int segmentIndex) {
var steps = 10;
var t = 1 / steps;
var sumArc = 0.0f;
var j = 0.0f;
var a = new Vector2(0.0f, 0.0f);
var b = points[segmentIndex * 3];
var dX = 0.0f;
var dY = 0.0f;
var dS = 0.0f;
for (int i = 0; i < steps; j = j + t)
{
a = SegmentAtPoint(segmentIndex, j);
dX = a.x - b.x;
dY = a.y - b.y;
dS = Mathf.Sqrt((dX * dX) + (dY * dY));
sumArc = sumArc + dS;
b.x = a.x;
b.y = a.y;
i++;
}
return sumArc;
}
Code var t = 1 / steps; makes integer division, so result t is zero
Also note that j = j + t is executed after every loop, so at the first iteration j==0
This flaw causes such problem: both b and a are equal at the first iteration because j still remains =0. So you calculate segment lengths on intervals: 0-0, 0-0.1, 0.1-0.2...0.7-0.8,0.8-0.9 - ignoring 0.9-1.0 interval
I am creating a 2D physics engine, and I am having trouble with a certain type of collision between movable and immovable objects. What I mean by movable is that the (x,y) values can change, not that the frame of reference can or can't change.
For example, a ball hitting a wall would be something that is movable colliding with something immovable.
I believe that I need to use something like Normal Force in this situation, but I am no sure how that would be used for finding the outcome of the collision.
Here is the code that I have so far. This code is for collision between two moving entities, but I need to add a case for when one is not moving:
private static void UpdateEntities(PhysicsEntity a, PhysicsEntity b)
{
var collisionAngle = Math.Atan2(a.Position.Y - b.Position.Y, a.Position.X - b.Position.X);
var angleA = a.Velocity.Direction - collisionAngle;
var angleB = b.Velocity.Direction - collisionAngle;
var vAx = a.Velocity.Magnitude * Math.Cos(angleA);
var vAy = a.Velocity.Magnitude * Math.Sin(angleA);
var vBx = b.Velocity.Magnitude * Math.Cos(angleB);
var vBy = b.Velocity.Magnitude * Math.Sin(angleB);
var vfAx = ((vAx * (a.Mass - b.Mass) + 2 * b.Mass * vBx) / (a.Mass + b.Mass)) * a.Material.Elasticity;
var vfBx = ((vBx * (b.Mass - a.Mass) + 2 * a.Mass * vAx) / (a.Mass + b.Mass)) * b.Material.Elasticity;
var vfAy = vAy * a.Material.Elasticity;
var vfBy = vBy * b.Material.Elasticity;
var magA = Math.Sqrt(Math.Pow(vfAx, 2) + Math.Pow(vfAy, 2));
var magB = Math.Sqrt(Math.Pow(vfBx, 2) + Math.Pow(vfBy, 2));
var dirA = Math.Atan2(vfAy, vfAx) + collisionAngle;
var dirB = Math.Atan2(vfBy, vfBx) + collisionAngle;
a.Velocity.X = magA * Math.Cos(dirA);
a.Velocity.Y = magA * Math.Sin(dirA);
b.Velocity.X = magB * Math.Cos(dirB);
b.Velocity.Y = magB * Math.Sin(dirB);
}
I tried setting the velocity of the immovable object to the opposite of the movable object's velocity, but that caused things to phase into each other.
I was able to get help from a friend and we worked out this algorithm to have objects reflect off of immovable objects during a collision.
Modified Original Function:
private static void UpdateEntities(PhysicsEntity a, PhysicsEntity b)
{
var collisionAngle = Math.Atan2(a.Position.Y - b.Position.Y, a.Position.X - b.Position.X);
if (a.IsMoveable && b.IsMoveable)
{
var angleA = a.Velocity.Direction - collisionAngle;
var angleB = b.Velocity.Direction - collisionAngle;
var vAx = a.Velocity.Magnitude * Math.Cos(angleA);
var vAy = a.Velocity.Magnitude * Math.Sin(angleA);
var vBx = b.Velocity.Magnitude * Math.Cos(angleB);
var vBy = b.Velocity.Magnitude * Math.Sin(angleB);
var vfAx = ((vAx * (a.Mass - b.Mass) + 2 * b.Mass * vBx) / (a.Mass + b.Mass)) * a.Material.Elasticity;
var vfBx = ((vBx * (b.Mass - a.Mass) + 2 * a.Mass * vAx) / (a.Mass + b.Mass)) * b.Material.Elasticity;
var vfAy = vAy * a.Material.Elasticity;
var vfBy = vBy * b.Material.Elasticity;
var magA = Math.Sqrt(Math.Pow(vfAx, 2) + Math.Pow(vfAy, 2));
var magB = Math.Sqrt(Math.Pow(vfBx, 2) + Math.Pow(vfBy, 2));
var dirA = Math.Atan2(vfAy, vfAx) + collisionAngle;
var dirB = Math.Atan2(vfBy, vfBx) + collisionAngle;
a.Velocity.X = magA * Math.Cos(dirA);
a.Velocity.Y = magA * Math.Sin(dirA);
b.Velocity.X = magB * Math.Cos(dirB);
b.Velocity.Y = magB * Math.Sin(dirB);
}
else
{
var sign = Math.Sign(collisionAngle);
collisionAngle *= sign;
while (collisionAngle > Math.PI/2)
{
collisionAngle -= Math.PI / 2;
}
collisionAngle *= sign;
if (a.IsMoveable)
{
Reflection(ref a, b, collisionAngle);
}
else
{
Reflection(ref b, a, collisionAngle);
}
}
}
Reflection Function:
private static void Reflection(ref PhysicsEntity movable, PhysicsEntity immovable, double collisionAngle)
{
if (Math.Abs(collisionAngle - Math.PI / 2) < Universe.Epsilon)
{
// take the velocity vector, rotate it 180 degrees, scale it
movable.Velocity.X *= -1;
movable.Velocity.Y *= -1;
}
else if (Math.Abs(movable.Position.Y - immovable.Position.Y) < Universe.Epsilon ||
(movable.Position.X > movable.CollisionPoint.X ^ movable.Position.Y < movable.CollisionPoint.Y))
{
//take velocity vector, rotate CCW by 2*collisionAngle, scale it
var rotateAngle = 2 * collisionAngle;
var xPrime = movable.Velocity.X * Math.Cos(rotateAngle) - movable.Velocity.Y * Math.Sin(rotateAngle);
var yPrime = movable.Velocity.Y * Math.Cos(rotateAngle) - movable.Velocity.X * Math.Sin(rotateAngle);
movable.Velocity.X = xPrime;
movable.Velocity.Y = yPrime;
}
else
{
//take the vector, rotate it CCW by 360-2*collisionAngle, scale it
var rotateAngle = 2 * (Math.PI - collisionAngle);
var xPrime = movable.Velocity.X * Math.Cos(rotateAngle) - movable.Velocity.Y * Math.Sin(rotateAngle);
var yPrime = movable.Velocity.Y * Math.Cos(rotateAngle) - movable.Velocity.X * Math.Sin(rotateAngle);
movable.Velocity.X = xPrime;
movable.Velocity.Y = yPrime;
}
movable.Velocity.X *= movable.Material.Elasticity;
movable.Velocity.Y *= movable.Material.Elasticity;
}
following this article to calculate the point from a lat and long with a bearing and distance when run results in NaN?
article below :
A method is given for calculating the destination point for ellipsoid earth models using Vincenty's formula:
Convert the starting point latitude 'lat1' (in the range -90 to 90) to radians.
lat1 = lat1 * PI/180
Convert the starting point longitude 'lon1' (in the range -180 to 180) to radians.
lon1 = lon1 * PI/180
Convert the bearing 'brg' (in the range 0 to 360) to radians.
brg = brg * PI/180
Ellipsoid earth models
Note: the variable 'flat' below represents the earth's polar flattening used in various ellipsoid models. For the commonly used WGS-84, let flat = 298.257223563.
Given the distance s in meters, the semi-major axis 'a' in meters, the semi-minor axis 'b' in meters and the polar flattening 'flat'.
Calculate the destination point useing Vincenty's formula. Shortened variable names are used.
f = 1/flat
sb=sin(brg)
cb=cos(brg)
tu1=(1-f)*tan(lat1)
cu1=1/sqrt((1+tu1*tu1))
su1=tu1*cu1
s2=atan2(tu1, cb)
sa = cu1*sb
csa=1-sa*sa
us=csa*(a*a - b*b)/(b*b)
A=1+us/16384*(4096+us*(-768+us*(320-175*us)))
B = us/1024*(256+us*(-128+us*(74-47*us)))
s1=s/(b*A)
s1p = 2*PI
Loop through the following while condition is true.
while (abs(s1-s1p) > 1e-12)
cs1m=cos(2*s2+s1)
ss1=sin(s1)
cs1=cos(s1)
ds1=B*ss1*(cs1m+B/4*(cs1*(-1+2*cs1m*cs1m)- B/6*cs1m*(-3+4*ss1*ss1)*(-3+4*cs1m*cs1m)))
s1p=s1
s1=s/(b*A)+ds1
Continue calculation after the loop.
t=su1*ss1-cu1*cs1*cb
lat2=atan2(su1*cs1+cu1*ss1*cb, (1-f)*sqrt(sa*sa + t*t))
l2=atan2(ss1*sb, cu1*cs1-su1*ss1*cb)
c=f/16*csa*(4+f*(4-3*csa))
l=l2-(1-c)*f*sa* (s1+c*ss1*(cs1m+c*cs1*(-1+2*cs1m*cs1m)))
d=atan2(sa, -t)
finalBrg=d+2*PI
backBrg=d+PI
lon2 = lon1+l;
Convert lat2, lon2, finalBrg and backBrg to degrees
lat2 = lat2 * 180/PI
lon2 = lon2 * 180/PI
finalBrg = finalBrg * 180/PI
backBrg = backBrg * 180/PI
If lon2 is outside the range -180 to 180, add or subtract 360 to bring it back into that range.
If finalBrg or backBrg is outside the range 0 to 360, add or subtract 360 to bring them back into that range.
Note: the variables 'a', 'b' and 'flat' above have the following relationships:
b = a - (a/flat)
flat = a / (a - b)
so porting this formula to c# i ended up with:
double Latitude = 50.390202;
double Longitude = -3.9204310000000078;
double Bearing = 225;
double lat1 = Latitude * (Math.PI / 180.0);
double lon1 = Longitude * (Math.PI / 180.0);
double brg = Bearing * (Math.PI / 180.0);
double s = 1000;
double a = 6378137.0;
double b = 6356752.314245;
double f = 1 / 298.257223563;
double sb = Math.Sin(brg);
double cb = Math.Cos(brg);
double tu1 = (1-f) * Math.Tan(lat1);
double cu1 = 1 / Math.Sqrt((1+tu1*tu1));
double su1 = tu1 * cu1;
double s2 = Math.Atan2(tu1, cb);
double sa = cu1 * sb;
double csa = 1 - sa * sa;
double us = csa * (a * a - b * b) / (b * b);
double A = 1 + us / 16384 * (4096 + us * (- 768 + us * (320 - 175 * us)));
double B = us / 1024 * (256 + us * (-128 + us * (74 - 47 * us)));
double s1 = s / (b * A);
double s1p = 2 * Math.PI;
while (Math.Abs(s1-s1p) > 1e-12)
{
cs1m = Math.Cos(2 * s2 + s1);
ss1 = Math.Sin(s1);
cs1 = Math.Cos(s1);
double ds1 = B * ss1 * (cs1m + B / 4 * (cs1 * (-1 + 2 * cs1m * cs1m) - B / 6 * cs1m * (-3 + 4 * ss1 * ss1) * (-3 + 4 * cs1m * cs1m)));
s1p = s1;
s1 = s / (b * A) + ds1;
}
double t = su1 * ss1 - cu1 * cs1 * cb;
double lat2 = Math.Atan2(su1 * cs1 + cu1 * ss1 * cb, (1 - f) * Math.Sqrt(sa * sa + t * t));
double l2 = Math.Atan2(ss1 * sb, cu1 * cs1 - su1 * ss1 * cb);
double c = f / 16 * csa * (4 + f * (4 - 3 * csa));
double l = l2 - (1 - c) * f * sa * (s1 + c * ss1 * (cs1m + c * cs1 * (-1 + 2 * cs1m * cs1m)));
double d = Math.Atan2(sa, -t);
double finalBrg = d + 2 * Math.PI;
double backBrg = d + Math.PI;
double lon2 = lon1 + l;
lat2 = lat2 * 180 / Math.PI;
lon2 = lon2 * 180/ Math.PI;
finalBrg = finalBrg * 180/ Math.PI;
backBrg = backBrg * 180 / Math.PI;
if (lon2 < -180)
{
lon2 = lon2 + 360;
}
if (lon2 > 180)
{
lon2 = lon2 - 360;
}
if (finalBrg < 0)
{
finalBrg = finalBrg + 360;
}
if (finalBrg > 360)
{
finalBrg = finalBrg - 360;
}
Console.WriteLine("{0} {1}", lat2, lon2);
but when run result in NaN for both lat2 and lon2?
expected result should be:
Latitude: 50°23′02″N 50.38384479
Longitude: 3°55′49″W -3.93037298
Final bearing: 224°59′32″ 224.99234101
Back bearing: 44°59′32″ 44.99234101
does c# calc do something different of have i made a mistake in there that i cant see :(
Thanks
According to Wikipedia (https://en.wikipedia.org/wiki/Vincenty%27s_formulae) b = (1-f)a
so your 10th line of code should be :
double b = a * (1 - 1 / flat);
NOTE - I haven't checked for any other problems, but it does give values that are not NaN.
I work on a C# project where i have a point of a given lattitude and longitude.
I need to find the lattitude and longitude of a second point that is x degrees angled and y meteres away from the given point.
thank you,
Agisilaos
See this page for Destination point given distance and bearing from start point
Here's a WGS-84 (non-spherical earth) version adapted from http://www.movable-type.co.uk/scripts/latlong-vincenty-direct.html:
private const double wgs84_major = 6378.137;
private const double wgs84_minor = 6356.7523142;
private const double wgs84_flattening = 1D / 298.257223563;
public static bool PointFromDistance(double latitude, double longitude, double angleRadians, double distanceMetres, out double newLatitude, out double newLongitude)
{
double a = wgs84_major * 1000;
double b = wgs84_minor * 1000;
double f = wgs84_flattening;
double s = distanceMetres;
double sinAlpha1 = Math.Sin(angleRadians), cosAlpha1 = Math.Cos(angleRadians);
double tanU1 = (1 - f) * Math.Tan(latitude * Math.PI / 180D);
double cosU1 = 1 / Math.Sqrt((1 + tanU1 * tanU1)), sinU1 = tanU1 * cosU1;
double sigma1 = Math.Atan2(tanU1, cosAlpha1);
double sinAlpha = cosU1 * sinAlpha1;
double cosSqAlpha = 1 - sinAlpha * sinAlpha;
double uSq = cosSqAlpha * (a * a - b * b) / (b * b);
double A = 1 + uSq / 16384 * (4096 + uSq * (-768 + uSq * (320 - 175 * uSq)));
double B = uSq / 1024 * (256 + uSq * (-128 + uSq * (74 - 47 * uSq)));
double sigma = s / (b * A), sigmaP = 2 * Math.PI;
double cos2SigmaM = 0;
double sinSigma = 0;
double cosSigma = 0;
double deltaSigma = 0;
while (Math.Abs(sigma - sigmaP) > 1e-12)
{
cos2SigmaM = Math.Cos(2 * sigma1 + sigma);
sinSigma = Math.Sin(sigma);
cosSigma = Math.Cos(sigma);
deltaSigma = B * sinSigma * (cos2SigmaM + B / 4 * (cosSigma * (-1 + 2 * cos2SigmaM * cos2SigmaM) -
B / 6 * cos2SigmaM * (-3 + 4 * sinSigma * sinSigma) * (-3 + 4 * cos2SigmaM * cos2SigmaM)));
sigmaP = sigma;
sigma = s / (b * A) + deltaSigma;
}
double tmp = sinU1 * sinSigma - cosU1 * cosSigma * cosAlpha1;
double lat2 = Math.Atan2(sinU1 * cosSigma + cosU1 * sinSigma * cosAlpha1,
(1 - f) * Math.Sqrt(sinAlpha * sinAlpha + tmp * tmp));
double lambda = Math.Atan2(sinSigma * sinAlpha1, cosU1 * cosSigma - sinU1 * sinSigma * cosAlpha1);
double C = f / 16 * cosSqAlpha * (4 + f * (4 - 3 * cosSqAlpha));
double L = lambda - (1 - C) * f * sinAlpha *
(sigma + C * sinSigma * (cos2SigmaM + C * cosSigma * (-1 + 2 * cos2SigmaM * cos2SigmaM)));
double revAz = Math.Atan2(sinAlpha, -tmp); // final bearing
newLatitude = lat2 * 180D / Math.PI;
newLongitude = longitude + L * 180D / Math.PI;
return true;
}