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);
Related
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
I'm working on an Android app which extracts a Quaternion relative to a rotation, then sends its axis/angle representation to C# in order to operate a 3D rotation of a vector.
I need this to be done outside Android (in my C# application), but for testing I'm doing it on both Android and C# to be sure the results are the same.
The result is not the same, and I noted that the Quaternion in C# is different from the original one on Android.
I'm assuming this is because C# and Android use a different reference coordinate system.
Here's my code:
On Android. Quaternion and Vector3 imported from libgdx
Quaternion q = (Quaternion) bundle.getSerializable("QUATERNION");
Vector3 axis = new Vector3();
q.getAxisAngle(axis);
float angle = q.getAngle();
The transformation I do afterwards is the following:
Vector3 vi = new Vector3(input[0],input[1],input[2]);
Vector3 output = vi.rotate(axis,angle);
On C#. Quaternion and Vector3 are imported from System.Numerics
public static float[] TranslateQuaternion(Vector3 axis, float angle, float x, float y, float z) {
Vector3 input = new Vector3(x,y,z);
Quaternion q = Quaternion.CreateFromAxisAngle(axis,angle);
Console.WriteLine("Quaternion: " + q.ToString());
Matrix4x4 m = System.Numerics.Matrix4x4.CreateFromAxisAngle(axis,angle);
Vector3 output = Vector3.Transform(input,m);
return new float[3]{output.X,output.Y,output.Z};
}
My Vector3 transformation leads to a different result than the one I get on Android, and I think the cause is due to a different Quaternion.
My starting Quaternion from Android is:
x=0.00 y=-0.67 z=0.01 w=0.74
//The axis I pass to C#:
x = -0.00 y=-1.00 z=0.01 angle=83.74
While my C# Quaternion is this:
x=0.00 y=0.86 z=-0,01 w=-0,52
How can I get around this? Is there some mapping I can do to have matching coordinates systems? Is there a C# way to perform the rotation of Vector3 from axis/angle directly as I've done in Android?
I'm getting quite crazy about that...
Thanks in advance guys!
I want to convert a point in world coordinates (P_w) to camera coordinates (P_c).
To do so I first have to translate P_w by the negative camera position (in world coordinates C_pos) and after that rotate P_w - C_pos x degrees around the x-axis, y degrees around the y-axis and z degrees around the z-axis.
I wrote this function in Unity to do this:
static public Vector4 convertWorldToCameraCoordinatesByDegrees (Vector3 point, PhotoData photoData)
{
// translate the point by the negative camera-offset
//and convert to Vector4
Vector4 translatedPoint = point - photoData.cameraPosition;
// by default translatedPoint.w is 0
translatedPoint.w = 1.0f;
// create rotation matrices
Matrix4x4 rotationMatrixX = Matrix4x4.identity;
Matrix4x4 rotationMatrixY = Matrix4x4.identity;
Matrix4x4 rotationMatrixZ = Matrix4x4.identity;
// setup rotationMatrixX
rotationMatrixX.SetRow (1,
new Vector4 (0,
Mathf.Cos (photoData.rotation.x),
- Mathf.Sin (photoData.rotation.x),
0));
rotationMatrixX.SetRow (2,
new Vector4 (0,
Mathf.Sin (photoData.rotation.x),
Mathf.Cos (photoData.rotation.x),
0));
// setup rotationMatrixY
rotationMatrixY.SetRow (0,
new Vector4 (Mathf.Cos (photoData.rotation.y),
0,
Mathf.Sin (photoData.rotation.y),
0));
rotationMatrixY.SetRow (2,
new Vector4 (- Mathf.Sin (photoData.rotation.y),
0,
Mathf.Cos (photoData.rotation.y),
0));
// setup rotationMatrixZ
rotationMatrixZ.SetRow (0,
new Vector4 (Mathf.Cos (photoData.rotation.z),
- Mathf.Sin (photoData.rotation.z),
0,
0));
rotationMatrixZ.SetRow (1,
new Vector4 (Mathf.Sin (photoData.rotation.z),
Mathf.Cos (photoData.rotation.z),
0,
0));
// unity doc says z-x-y in this order
Matrix4x4 rotationMatrix = rotationMatrixY * rotationMatrixX * rotationMatrixZ;
Vector4 transformedPoint = rotationMatrix * translatedPoint;
return transformedPoint;
}
But the results are different from another function where build my roations matrix using the up, forward and right vector of my camera.
PhotoData is a class that has some information about the camera including the position and the rotation. PhotoData.rotation is a Vector3 which has the camera rotation around the x, the y, and the z axis as x-, y-, z-values.
Can someone tell me why this is not working?
// edit:
Some people have mentioned build-in functions, but I have to compute this with out them. I forgot to write it in the original post.
I wrote this algorithm first in c++ and wanted to test it in Unity since it's easier to verify it.
Looking at your code and think you are massively overcomplicating simple task, or I do not understand the question. You can definitely use Camera.WorldToScreenPoint
http://docs.unity3d.com/ScriptReference/Camera.WorldToScreenPoint.html
I have some problems with a rotating marble.
I've tried it with Matrix.CreateFromYawPitchRoll and Matrix.CreateRotation but there were some problems, I think it's due to the Gimbal lock effect.
So, I've tried using quaternions instead, but nothing changed.
When moving on only an axis it works fine, but when the rotation occurs on two different axes the marble still rotates on wrong axes.
Here's my code:
// declarations
Vector3 Position = Vector3.Zero;
Vector3 Rotation = Vector3.Zero;
Quaternion qRotation = Quaternion.Identity;
AbsoluteBoneTransforms = new Matrix[Model.Bones.Count];
Model.CopyAbsoluteBoneTransformsTo(AbsoluteBoneTransforms);
In the Update method:
Position += speed;
Rotation = speed * MathHelper.ToRadians(-1.5f);
Quaternion rot = Quaternion.CreateFromAxisAngle(Vector3.Right, Rotation.Z) *
Quaternion.CreateFromAxisAngle(Vector3.Backward, Rotation.X);
qRotation *= rot;
And in the Draw method:
effect.World = AbsoluteBoneTransforms[mesh.ParentBone.Index] *
Matrix.CreateFromQuaternion(qRotation) * Matrix.CreateTranslation(Position);
What's wrong? Is it wrong to use Quaternion.CreateFromAxisAngle on multiple axes?
EDIT
I've tried calculating directly the axis of rotation of my marble, instead of using combination of multiple axes:
angle += speed.Length() * angularVelocity;
qRotation = Quaternion.CreateFromAxisAngle(Vector3.Cross(speed, Vector3.Up), angle);
qRotation.Normalize();
angle is a float that keeps track of the current movement.
This solution doesn't seem to create Gimbal lock, but marble rotations aren't correct, it seems that the rotating speed is not constant, but became faster and slower over time, I can't understand why.
If I "concatenate" the quaternions I get every frame using
qRotation *= Quaternion.CreateFromAxisAngle(Vector3.Cross(speed, Vector3.Up), angle)
the Gimbal lock effect is still visible.
Here's how I've tackled that:
I'm assuming speed is a vector representing the direction the ball is rolling and whose magnitude represents the rate it is traveling in that direction.
Vector3 axis = Vector3.Cross(speed, Vector3.Up);
float angle = speed.Length();//factor by delta time if neccesary
Quaternion rotationThisFrame = Quaternion.CreateFromAxisAngle(axis, angle * (1/radiusOfBall));
then you can concatenate that to your qRotation. Also, you may need to normalize your quaternion after concatenation.
Update: The correct answer to this question/thread was reversing the order that quaternions concatenate in. With respect to XNA, matrices combine left to right, quaternions combine right to left.
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.