Develop Reference

C# Vector3 Rotation with Quaternion / Matrix4x4

I've got a sensor that delivers object data in a coordinate system according to ISO 8855-2011.
My sensor is positioned at [0, 0, 0] but can be "misaligned". Therefore I want to compensate for roll, pitch and yaw.
In my App I perform the necessary rotations with the System.Numerics Lib and more specifically with a vector3 as the data point ([x, y, z]) and tried either a Matrix4x4 or a Quaternion as the rotation matrix.
Without having evaluated this final result a rotation in three steps seems to work quite well (at first glance looks like to be expected in my visualization). To perform the rotation I start by specifying three Quaternions for each axis and perform the rotation one after the other.
Vector3 vecMeas = new Vector3(x, y, z);
Quaternion rotRoll = Quaternion.CreateFromAxisAngle(new Vector3(1.0f, 0.0f, 0.0f), rollAngle);
Quaternion rotPitch = Quaternion.CreateFromAxisAngle(new Vector3(0.0f, 1.0f, 0.0f), pitchAngle);
Quaternion rotYaw = Quaternion.CreateFromAxisAngle(new Vector3(0.0f, 0.0f, 1.0f), yawAngle);
var vecRolled = Vector3.Transform(vecMeas, rotRoll);
var vecPitched = Vector3.Transform(vecRolled, rotPitch);
var vecResult = Vector3.Transform(vecPitched, rotYaw);
While this seems to work, it looks clumsy to me as the class Matrix4x4 and Quaternion offers a method called "CreateFromYawPitchRoll". I tried every combination of inputs for this method (if I remember correctly 6 combinations for the order of pitch, roll, yaw combined with 48 combinations of inputs for the order and sign of [x, y, z]), but was not able to produce the same as my rotation in 3 steps.
Example (one of ~288 combinations):
Vector3 vecMeas = new Vector3(x, y, z);
Quaternion rot = Quaternion.CreateFromYawPitchRoll(pitch, roll, yaw);
var vecResult = Vector3.Transform(vecMeas, rot);
The above shown "one step" solutions always looks off.
Unfortunately I was not able to find a a lot of information about the handedness (or the absence thereof) of the expected inputs for Matrix4x4 / Quaternion or the order in which the rotation of "CreateFromYawPitchRoll" is being performed. Do you have any idea, how I could refactor my code to be more elegant (aka maybe a one step solution) or how to properly use CreateFromYawPitchRoll? A multiplication of the rotation elements did not deliver the expected results (rotRoll * rotPitch * rotYaw).
Thanks in advance!
Update:
Sorry, I think I figured it out.
In my case I have to do the following:
Quaternion rot = Quaternion.CreateFromYawPitchRoll(yawAngle, pitchAngle, rollAngle);
var vecRot = Vector3.Transform(new Vector3(Y, Z, X), rot);
The result will be:
var vecRes = new Vector3(vecRot.Z, vecRot.X, vecRot.Y);

normalizing euler angles rotation vector

I need to display the rotation in Euler angles of an object's certain axis.
I am aware that retrieving the rotation of an object in Euler angles gives inconsistent results, some of which can be solved by simply using modulo 360 on the result. however one permutation that unity sometimes does when assigning a vector with the value of "transform.localRotation.eulerAngles" is instead of retrieving the Vector3 "V", it retrieves "(180, 180, 180) - V".
to my understanding, "(180, 180, 180) - V" does not result in the same real world rotation as V, unlike "(180, 180, 180) + V" which does leave the actual rotation unaffected.
what is the explanation for the phenomenon, and what is the best way of normalizing an Euler angles rotation vector assuming I know the desired and feasible value of one of its axes? (for example, to normalize it such that all of it's values are mod 360 and it's Z axis equals 0 assuming it does have a representation in which Z = 0)

I don't know about the first part of the question (it is different enough to be its own question imo) but I can answer your second one.
So, you have these inputs :
Quaternion desiredRotation;
float knownZ;
And you're trying to find Vector3 eulers where eulers.z is approximately knownZ and Quaternion.Euler(eulers) == desiredRotation.
Here's the procedure I would use:
First, determine the up direction rotated by desiredRotation and the up and right direction rotated by a roll of knownZ:
Vector3 upDirEnd = desiredRotation * Vector3.up;
Quaternion rollRotation = Quaternion.Euler(0,0,knownZ);
Vector3 upDirAfterRoll = rollRotation * Vector3.up;
Vector3 rightDirAfterRoll = rollRotation * Vector3.right;
We know the local up direction after desiredRotation is applied and that the only thing that can adjust the up direction after the roll knownZ is applied is the rotation done by the euler pitch component. So, if we can calculate the angle from upDirAfterRoll to upDirEnd as measured around the rightDirAfterRoll axis...
float determinedX = Vector3.SignedAngle(upDirAfterRoll, upDirEnd, rightDirAfterRoll);
// Normalizing determinedX
determinedX = (determinedX + 360f) % 360f;
...we can determine the x component of eulers!
Then, we do the same with the yaw component of eulers to make the new forward direction line up with the end forward direction:
Vector3 forwardDirEnd = desiredRotation * Vector3.forward;
Quaternion rollAndPitchRotation = Quaternion.Euler(determinedX, 0, knownZ);
Vector3 forwardDirAfterRollAndPitch = rollAndPitchRotation * Vector3.forward;
Vector3 upDirAfterRollAndPitch = upDirEnd; // unnecessary but here for clarity
float determinedY = Vector3.SignedAngle(forwardDirAfterRollAndPitch, forwardDirEnd, upDirAfterRollAndPitch );
// Normalizing determinedY
determinedY = (determinedY + 360f) % 360f;
Vector3 eulers = new Vector3(determinedX, determinedY, knownZ);
To ensure that the given quaternion can be made with the given component, you could check if the axes given to SignedAngle actually can rotate the input vector to the target vector, or you can just compare the calculated eulers and the given quaternion:
Quaternion fromEuler = Quaternion.Euler(eulerAngles);
if (fromEuler==desiredRotation)
{
// use eulerAngles here
}
else
{
// component and quaternion incompatible
}
Hopefully that helps.

I'm not quite sure I understand your question correctly, but the euler angles just represent the angles of 3 rotations applied around the 3 axis in a specific order, right? So why would you normalize it by adding 180 everywhere? You should bring each angle individually into the range 0-360 by modulo-ing them.
Your question seems to imply that you can obtain any orientation by only rotating around two axis instead of three... is that what you are trying to achieve?
Using quaternions could possibly help you, in fact an orientation can be defined with just 4 scalar values: an axis and an angle

3D relative angle sum calculation

I have a 3D object with rotation r1 in a quaternion form.
I rotate it with local euler angles:
transform.Rotate(new Vector3(0f, 15f, 0f), relativeTo: Space.Self); // right
transform.Rotate(new Vector3(-10f, -5f, 0f), relativeTo: Space.Self); // left up
transform.Rotate(new Vector3(0f, 0f, 90f), relativeTo: Space.Self); // 90 clockwise
Now I have rotation r2. How can I retrieve the local Y rotation sum 15-5+0=10 if I don't know what angles have been applied? It may be impossible to get exactly that value (10) but you've got my idea. May be I can just get Y diff in the local r2 space?

One possible solution I found:
(r2 * Quaternion.Inverse(r1)).eulerAngles.Y

I am still convinced that transform matrices will be much better approach for you
As mentioned in previous question Euler angles are not the best for your purpose and only mess thing up for you but anyway what about this:
P0=(0,0,0)
P1=(1,0,0) // or (0,0,1) y=0 !!!
A0=r2_localtoglobal(P0)
A1=r2_localtoglobal(P1)
B0=r2r1_localtoglobal(P0)
B1=r2r1_localtoglobal(P1)
A=A1-A0 // local r2 X axis direction in GCS (without r1)
B=B1-B0 // local r2r1 X axis direction in GCS (with r1)
angle=-acos((A.B)/(|A|.|B|)) // angle between A,B (but inverted because you wanted local angle)
I assume r1 is ship and r2 is radar
[Edit1] after read of your edit from linked question is finally clear what you want
P0=(0,0,0)
P1=(1,0,0) // or (0,0,1) y=0 !!!
A0=r1_globaltolocal(P0)
A1=r1_globaltolocal(P1)
A=A1-A0
angle=atanxy(A.x,A.z)
where r1 is your ship transformation
radar transformation is irelevant to background image
atanxy is atan2(y,x) = atan(y/x) but with sign decomposition so it works on whole < 0,2PI > interval
atan2,atanxy:
const double pi=M_PI;
const double pi2=2.0*M_PI;
double atanxy(double x,double y) // atan2 return < 0 , 2.0*M_PI >
{
int sx,sy;
double a;
const double _zero=1.0e-30;
sx=0; if (x<-_zero) sx=-1; if (x>+_zero) sx=+1;
sy=0; if (y<-_zero) sy=-1; if (y>+_zero) sy=+1;
if ((sy==0)&&(sx==0)) return 0;
if ((sx==0)&&(sy> 0)) return 0.5*pi;
if ((sx==0)&&(sy< 0)) return 1.5*pi;
if ((sy==0)&&(sx> 0)) return 0;
if ((sy==0)&&(sx< 0)) return pi;
a=y/x; if (a<0) a=-a;
a=atan(a);
if ((x>0)&&(y>0)) a=a;
if ((x<0)&&(y>0)) a=pi-a;
if ((x<0)&&(y<0)) a=pi+a;
if ((x>0)&&(y<0)) a=pi2-a;
return a;
}

Rotation matrix to vector with angle

Using HelixToolkit in my code i have a rotation matrix (3x3) like :
UX VX WX
UY VY WY
UZ VZ WZ
And i want to rotate a GeometryModel3D. I found the RotateTransform3D and i need a Vector3D with an angle :
// Create and apply a transformation that rotates the object.
RotateTransform3D myRotateTransform3D = new RotateTransform3D();
AxisAngleRotation3D myAxisAngleRotation3d = new AxisAngleRotation3D();
**myAxisAngleRotation3d.Axis = new Vector3D(0, 3, 0);
myAxisAngleRotation3d.Angle = 40;**
myRotateTransform3D.Rotation = myAxisAngleRotation3d;
// Add the rotation transform to a Transform3DGroup
Transform3DGroup myTransform3DGroup = new Transform3DGroup();
myTransform3DGroup.Children.Add(myRotateTransform3D);
//ajoute the transformation to the model
model3D.Transform = myTransform3DGroup;
How can i calculate the vector and the angle from my rotation matrix ?

If you already calculated the matrix, you can just use it with a general MatrixTransform3D. Just set the values of the matrix to the calculated ones.
If, however, you really want to calculate the rotation axis and angle, you have to solve a linear equation system. Take a look at the Wikipedia entry. But then, you would calculate far too much than you actually need.

How can I rotate this matrix around the center?

My XNA game uses Farseer Physics, which is a 2d physics engine with an optional renderer for physics engine data, to help you debug. Visual debug data is very useful, so I have it setup to be drawn according to my camera's state. This works perfectly, except for z axis rotation. See, I have a camera class that supports movement, zoom, and z axis rotation. My debug class uses the Farseer's debug renderer to create matrices that make the debug data be drawn according to the camera, and it does it well, except for one thing.. the z axis rotation uses the top-left corner of the screen for (0, 0), while my camera rotates using the center of the viewport as (0, 0). Does anyone have any tips for me? If I can make the debug drawer rotate from the center, it would work perfectly with my camera.
public void Draw(Camera2D camera, GraphicsDevice graphicsDevice)
{
// Projection (location and zoom)
float width = (1f / camera.Zoom) * ConvertUnits.ToSimUnits(graphicsDevice.Viewport.Width / 2);
float height = (-1f / camera.Zoom) * ConvertUnits.ToSimUnits(graphicsDevice.Viewport.Height / 2);
//projection = Matrix.CreateOrthographic(width, height, 1f, 1000000f);
projection = Matrix.CreateOrthographicOffCenter(
-width,
width,
-height,
height,
0f, 1000000f);
// View (translation and rotation)
float xTranslation = -1 * ConvertUnits.ToSimUnits(camera.Position.X);
float yTranslation = -1 * ConvertUnits.ToSimUnits(camera.Position.Y);
Vector3 translationVector = new Vector3(xTranslation, yTranslation, 0f);
view = Matrix.CreateRotationZ(camera.Rotation) * Matrix.Identity;
view.Translation = translationVector;
DebugViewXNA.RenderDebugData(ref projection, ref view);
}

One common approach to solving these sort of issues is to move the object in question to the 'centre', rotate and the move it back.
So in this case, I'd suggest applying a transformation that moves the camera "up and across" by half the screen dimensions, apply the rotation and then move it back.

In general, in order to perform rotation around point (x, y, z), the operation needs to be broken down into 3 conceptual parts:
T is a translation matrix that translates by (-x, -y, -z)
R is a rotation matrix that rotates around the relevant axis.
T^-1 is the matrix that translates back to (x, y, z)
The matrix you're after is the result of the multiplication of these 3, in reverse order:
M = T^-1 * R ^ T
The x,y,z you should use are your camera's position.