Develop Reference

C# How to convert Quaternions to euler angles (XYZ)

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;
}

How to draw an arched arrow?

i need to draw an arrow representing the rotation of a circle in WPF. The best i got so far is this one. But it seems that the arrowhead is not correctly aligned with the angle of the arrowarc.
private void InternalDrawArrowGeometry(StreamGeometryContext context)
{
var angleWidth = 45;
var startAngle = Rotation + 90;
var endAngle = Rotation + angleWidth + 90;
var xEnd = Radius * Math.Cos(startAngle * Math.PI / 180.0);
var yEnd = Radius * Math.Sin(startAngle * Math.PI / 180.0);
var xStart = Radius * Math.Cos(endAngle * Math.PI / 180.0);
var yStart = Radius * Math.Sin(endAngle * Math.PI / 180.0);
var b = angleWidth * Math.PI/180;
var pt1 = new Point(CentreX + xStart, CentreY - yStart);
var pt2 = new Point(CentreX + xEnd, CentreY - yEnd);
var len2 = 1;
const int angle = 45;
var pt3 = new Point(
pt2.X + (len2 / b) * ((pt1.X - pt2.X) * Math.Cos(angle) + (pt1.Y - pt2.Y) * Math.Sin(angle)),
pt2.Y + (len2 / b) * ((pt1.Y - pt2.Y) * Math.Cos(angle) - (pt1.X - pt2.X) * Math.Sin(angle)));
var pt4 = new Point(
pt2.X + (len2 / b) * ((pt1.X - pt2.X) * Math.Cos(angle) - (pt1.Y - pt2.Y) * Math.Sin(angle)),
pt2.Y + (len2 / b) * ((pt1.Y - pt2.Y) * Math.Cos(angle) + (pt1.X - pt2.X) * Math.Sin(angle)));
context.BeginFigure(pt1,
false, // Filled
false); // Closed
context.ArcTo(pt2,
new Size(Radius, Radius),
0.0, // rotationAngle
startAngle - endAngle > 180, // greater than 180 deg?
SweepDirection.Clockwise,
true, // isStroked
false);
context.LineTo(pt3, true, false);
context.LineTo(pt2, true, false);
context.LineTo(pt4, true, false);
}
Has anyone coded something like this correctly and give me the code or can tell me whats wrong with my code?

Calculate Coord from start point, endpoint, and distance

I have two points on a map and I know the distance between them. Now, I need to get a new point between them X meters away from start point. However, i cannot figure out, how to find the new coords.
var nextTrazadoPoint = new Coord {Lat = ...., Lng=...., Alt=...};
var previousTrazadoPoint = new Coord {Lat = ...., Lng=...., Alt...};
var fromCoords = new GeoCoordinate(nextTrazadoPoint.Lat, nextTrazadoPoint.Lng, nextTrazadoPoint.Alt);
var toCoords = new GeoCoordinate(previousTrazadoPoint .Lat, previousTrazadoPoint .Lng, previousTrazadoPoint .Alt);
var distance = fromCoords.GetDistanceTo(toCoords); //Let's say 1000 ¿meters?
Now I want to walk 200 meters from previousTrazadoPoint to nextTrazadoPoint
//Vector from previousTrazadoPoint to nextTrazadoPoint
var vectorDireccion = new Vector(
(double)(nextTrazadoPoint.Latitud - previousTrazadoPoint.Latitud),
(double)(nextTrazadoPoint.Longitud - previousTrazadoPoint.Longitud)
);
//Normalize
vectorDireccion.Normalize();
//meters from previousTrazadoPoint
var distanciaARecorrer = 200;
//New coords
var vectorDestino = distanciaARecorrer * vectorDireccion;
point.Latitud = (decimal)vectorDestino.X + previousTrazadoPoint.Latitud;
point.Longitud = (decimal)vectorDestino.Y + previousTrazadoPoint.Longitud;
However, when i draw the new point on Gmaps, it is not placed between both.
Any ideas?

Thanks to #HansKilian and #cletus (Calculate distance between 2 GPS coordinates), I could find the solution
private const double EARTH_RADIUS = 6378.1;
private static double ConvertToRadians(double angle)
{
return (Math.PI / 180) * angle;
}
private static double ConvertToDegree(double angle)
{
return angle * (180.0 / Math.PI);
}
private static double CalculateBearing(CoordsDto from, CoordsDto to)
{
var from_lat_rad = ConvertToRadians(from.Latitud);
var to_lat_rad = ConvertToRadians(to.Latitud);
var dif_lng_rad = ConvertToRadians(to.Longitud - from.Longitud);
double x = Math.Cos(from_lat_rad) * Math.Sin(to_lat_rad) - Math.Sin(from_lat_rad) * Math.Cos(to_lat_rad) * Math.Cos(dif_lng_rad);
double y = Math.Sin(dif_lng_rad) * Math.Cos(to_lat_rad);
// Math.Atan2 can return negative value, 0 <= output value < 2*PI expected
return (Math.Atan2(y, x) + Math.PI * 2) % (Math.PI * 2);
}
public static CoordsDto GetPointFarAway(CoordsDto from, CoordsDto to, double meterAwayFromStart)
{
var resp = new CoordsDto();
var bearing_rad = CalculateBearing(from, to);
var d = meterAwayFromStart * 0.001; //KM
var input_lat1_rad = ConvertToRadians(from.Latitud);
var input_lon1_rad = ConvertToRadians(from.Longitud);
var newPoint_lat_rad = Math.Asin(
Math.Sin(input_lat1_rad) * Math.Cos(d / EARTH_RADIUS) + Math.Cos(input_lat1_rad) * Math.Sin(d / EARTH_RADIUS) * Math.Cos(bearing_rad)
);
var newPoint_lon_rad = input_lon1_rad + Math.Atan2(
Math.Sin(bearing_rad) * Math.Sin(d / EARTH_RADIUS) * Math.Cos(input_lat1_rad),
Math.Cos(d / EARTH_RADIUS) - Math.Sin(input_lat1_rad) * Math.Sin(newPoint_lat_rad)
);
resp.Latitud = ConvertToDegree(newPoint_lat_rad);
resp.Longitud = ConvertToDegree(newPoint_lon_rad);
return resp;
}

I can't see anything wrong with your code. You haven't really provided enough information that I can take your code and compile it and see what's wrong, so I tried coding it up from scratch using the Vector2 class. I couldn't use Vector since that isn't available in .NET core and my sandbox project is a .NET core project.
This is what I got
var origin = new Vector2(100.0f, 100.0f);
var destination = new Vector2(0.0f, 400.0f);
var direction = destination - origin;
var movement = Vector2.Normalize(direction) * 200.0f;
var movementdestination = origin + movement;
Console.WriteLine($"X: {movementdestination.X}, Y: {movementdestination.Y}");
It prints
X: 36.75444, Y: 289.7367
which - as far as I can tell - is correct. I hope that helps.

Cubic bezier curve arc length is always zero

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

How to change Orbit View on Windows Community Toolkit to arrange at the top instead of middle left center

So, I'm working on Pie Control with Orbit View from Windows Community toolkit.
I tried to change the behavior of Orbit View to arrange item started at the top instead of at the left center.
Here is when Orbit View has only one item.
I tried tracking it down and found the code that Orbit View used to arrange item. But the problem is that I know nothing about math and didn't know where to change the value :/ So, here is the code.
protected override Size ArrangeOverride(Size finalSize)
{
var angle = 2 * Math.PI / Children.Count;
var minDistance = 80;
var maxDistance = Math.Max(minDistance, (Math.Min(finalSize.Width, finalSize.Height) - OrbitView.MaxItemSize) / 2);
var elementsProperties = new List<OrbitViewElementProperties>();
for (var i = 0; i < Children.Count; i++)
{
var element = Children.ElementAt(i);
OrbitViewDataItem orbitViewDataItem = null;
if (element is FrameworkElement)
{
orbitViewDataItem = ((FrameworkElement)element).DataContext as OrbitViewDataItem;
}
var d = orbitViewDataItem != null && orbitViewDataItem.Distance >= 0 ? orbitViewDataItem.Distance : 0.5;
d = Math.Min(d, 1d);
var distance = (d * (maxDistance - minDistance)) + minDistance;
var x = distance * Math.Cos((angle * i) + (angle / 2));
var y = distance * Math.Sin((angle * i) + (angle / 2));
var x_normalized = (finalSize.Width / 2) + x - (element.DesiredSize.Width / 2);
var y_normalized = (finalSize.Height / 2) - y - (element.DesiredSize.Height / 2);
var point = new Point(x_normalized, y_normalized);
element.Arrange(new Rect(point, element.DesiredSize));
var elementProperties = new OrbitViewElementProperties()
{
XYFromCenter = new Point(x, y),
DistanceFromCenter = distance,
Element = element
};
elementsProperties.Add(elementProperties);
if (ItemArranged != null)
{
var args = new OrbitViewPanelItemArrangedArgs()
{
ElementProperties = elementProperties,
ItemIndex = i
};
ItemArranged.Invoke(this, args);
}
}
ItemsArranged?.Invoke(this, new OrbitViewPanelItemsArrangedArgs() { Elements = elementsProperties });
return finalSize;
}
So, where do I change it to make it start putting the item at the top center (0 degree) instead of left center (270 degree)
Edit: I fork the project and remove all item to just a few control here: https://github.com/ray1997/WindowsCommunityToolkit/tree/ForJustR
The code I mention above is in here: https://github.com/ray1997/WindowsCommunityToolkit/blob/ForJustR/Microsoft.Toolkit.Uwp.UI.Controls/OrbitView/OrbitViewPanel.cs line 90

var angle = 2 * Math.PI / Children.Count;
This looks like the angle is expressed in radians.
var distance = (d * (maxDistance - minDistance)) + minDistance;
var x = distance * Math.Cos((angle * i) + (angle / 2));
var y = distance * Math.Sin((angle * i) + (angle / 2));
and here it is used to calculate the x and y values for the elements.
Now adding 90° or 2 * Math.PI / 4 Radians will move it by 90°.
So that gets you
// offset the first element by 90°
var customOffset = 2 * Math.PI / 4;
var distance = (d * (maxDistance - minDistance)) + minDistance;
var x = distance * Math.Cos((angle * i) + (angle / 2) - customOffset);
var y = distance * Math.Sin((angle * i) + (angle / 2) - customOffset);