I need to convert an angle axis representation to a Quaternion using Eigen
For example I have the following angle axis representation of a pose from a UR robot (rotation part of the pose of the robot only)
(-2.237 -2.217 -0.030)
I want to convert this to a Quaternion using Eigen
I tried this code
Eigen::Quaterniond q;
q = AngleAxisd(-2.237, Vector3d::UnitX())
* AngleAxisd(-2.217, Vector3d::UnitY())
* AngleAxisd(-0.030, Vector3d::UnitZ());
but I end up with
qx: -3.9853142115731549e-01
qy: -4.0057160431638472e-01
qz: 7.9758941987327925e-01
qw: 2.1110711374071198e-01
When I use this online conversion tool I get something different
https://www.andre-gaschler.com/rotationconverter/
What am I doing wrong?
Thanks
Managed to get it sorted this way
Eigen::Vector3d rotation(roll, pitch, yaw);
double angle = rotation.norm();
Eigen::Vector3d axis = rotation.normalized();
Eigen::Quaterniond q(Eigen::AngleAxisd(angle, axis));
Related
I'm using this very simple code to rotate objects around their own origin
void Entity::rotate(unsigned short axis, float speed, float delta)
{
m_Orientation[axis] += delta * speed;
glm::quat quaternion = glm::quat(glm::vec3(m_Orientation[0], m_Orientation[1], m_Orientation[2]));
m_RotationMatrix = glm::mat4_cast(quaternion);
}
The issue I'm facing is that the rotation is only relative to object's direction on the X axis. Meaning that no matter which way the object is facing (what it's orientation is), if I rotate it around X it will always rotate around it's own X axis. That is what I would like to do, but on the other 2 axis as well.
But Y and Z always rotate around world X and Y axis, disregarding which way my object is facing.
My code is so small and simple I don't really see the problem? Is the X rotation being always correct the accident, or is it that the other two are wrong?
I suggest that you try this:
#include <glm/gtx/euler_angles.hpp>
/**
If you change speed to a vec3,
it do all the calculations at once
(you won't have to do it for each angle)
**/
void Entity::rotate(glm::vec3 speed, float delta)
{
rotation = speed * data
m_modelMatrix *= glm::eulerAngleXYZ(rotation.x, rotation.y, rotation.z);
}
The glm::eulerAngleXYZ function will take the angles and create a rotation matrix from it.
But be aware of gimbal lock.
If you do your multiplication of the matrix from right to left, it will rotate around the global axis. If you do it the other way, it will rotate around the local axis.
Implementing the answer from this thread I have this code that translates the deltayaw, deltaroll and deltapitch angles into one angle and rotates a node around it. The angles that are taken as the parameters are momentary changes of angles since giving the whole angles would ignore the changes in orientation.
public static void matrixRotate(Group n, double deltaroll, double deltapitch, double deltayaw){
double A11 = Math.cos(deltaroll)*Math.cos(deltayaw);
double A12 = Math.cos(deltapitch)*Math.sin(deltaroll)+Math.cos(deltaroll)*Math.sin(deltapitch)*Math.sin(deltayaw);
double A13 = Math.sin(deltaroll)*Math.sin(deltapitch)-Math.cos(deltaroll)*Math.cos(deltapitch)*Math.sin(deltayaw);
double A21 =-Math.cos(deltayaw)*Math.sin(deltaroll);
double A22 = Math.cos(deltaroll)*Math.cos(deltapitch)-Math.sin(deltaroll)*Math.sin(deltapitch)*Math.sin(deltayaw);
double A23 = Math.cos(deltaroll)*Math.sin(deltapitch)+Math.cos(deltapitch)*Math.sin(deltaroll)*Math.sin(deltayaw);
double A31 = Math.sin(deltayaw);
double A32 =-Math.cos(deltayaw)*Math.sin(deltapitch);
double A33 = Math.cos(deltapitch)*Math.cos(deltayaw);
double d = Math.acos((A11+A22+A33-1d)/2d);
if(d!=0d){
double den=2d*Math.sin(d);
Point3D p= new Point3D((A32-A23)/den,(A13-A31)/den,(A21-A12)/den);
Rotate r = new Rotate();
r.setAxis(p);
r.setAngle(Math.toDegrees(d));
n.getTransforms().add(r);
Transform all = n.getLocalToSceneTransform();
n.getTransforms().clear();
n.getTransforms().add(all);
}
}
(I'm using rotate because I need to always rotate the object around the origin, not the center)
Now this creates a problem as I'm no longer able to get the actual pitch, roll and yaw angles.
I used to keep track of them like this (which doesn't take into account the changing orientation):
roll +=deltaroll;
pitch += deltapitch;
yaw += deltayaw;
And later I've come up with this, which is a bit more accurate, but doesn't track the changes that occur if the angles are not directly modified(inserted after the n.getTransforms().add(all) in the main snippet):
roll+= Math.toDegrees(d)*((A32-A23)/den);
pitch += Math.toDegrees(d)*((A13-A31)/den);
yaw += Math.toDegrees(d)*((A21-A12)/den);
I've been searching around for solutions and found this answer which is supposed to give the angle from the final transform but I haven't been able to get it working for all angles.
double xx = n.getLocalToSceneTransform().getMxx();
double xy = n.getLocalToSceneTransform().getMxy();
double roll = Math.atan2(-xy, xx);
Again what I'm trying to get are the full angles (composited out of the transforms made from the delta angles in different orientations) relative to the scene's coodrdinate system. I'm really bad at this so all help would be great.
If you want to get the pitch, yaw and roll angles at any stage after several rotations, you can get them from the transformation matrix of the 3D model.
If you have a look at the transformation matrix after several rotations:
Transform T = model3D.getLocalToSceneTransform();
System.out.println(T);
you'll see something like this:
Transform [
0.9034731871219395, -0.4260296991535005, -0.04727468234587054, 1.4044414829046357
0.3743586809560477, 0.837958815679334, -0.39709016761704913, 0.5234811188037405
0.2087864414768669, 0.3410626315861443, 0.9165612381019399, -1.1277640590168572
]
If you want the angles, you just need to compare this matrix with this one from this answer:
As you have already stated, to get the roll angle you can use T.getMxx() and T.getMyx():
double roll = Math.atan2(-T.getMyx(),T.getMxx());
Now, for the pitch, you can use T.getMzy() and T.getMzz() in the same way:
double pitch = Math.atan2(-T.getMzy(),T.getMzz());
Finally, for the yaw, use T.getMzx(), T.getMzy() and T.getMzz():
double yaw = Math.atan2(T.getMzx(),Math.sqrt(T.getMzy()*T.getMzy()+T.getMzz()*T.getMzz()));
This will give for the above matrix the angles you are looking for (in radians):
roll: -0.39281984604895126
pitch: -0.356235553820928
yaw: 0.21033388848106072
I'm attempting to add a small value to a World Matrix in order to replicate the accuracy of a fired weapon [pistol, assault rifle]
Currently, my World Matrix resides at a Parent Objects' position, with the ability to rotate about the Y axis exclusively.
I've done this in Unity3D, running whenever the object needs to be created [once per]:
var coneRotation = Quaternion.Euler(Random.Range(-spread, spread), Random.Range(-spread, spread), 0);
var go = Instantiate(obj, parent.transform.position, transform.rotation * coneRotation) as GameObject;
and am attempting to replicate the results using Direct3D11.
This lambda returns a random value between [-1.5, 1.5] currently:
auto randF = [&](float lower_bound, float uppder_bound) -> float
{
return lower_bound + static_cast <float> (rand()) / (static_cast <float> (RAND_MAX / (uppder_bound - lower_bound)));
};
My first thought was to simply multiply a random x && y into the forward vector of an object upon initialization, and move it in this fashion: position = position + forward * speed * dt; [speed being 1800], though the rotation is incorrect (not to mention bullets fire up).
I've also attempted to make a Quaternion [as in Unity3D]: XMVECTOR quaternion = XMVectorSet(random_x, random_y, 0) and creating a Rotation Matrix using XMMatrixRotationQuaternion.
Afterwards I call XMStoreFloat4x4(&world_matrix, XMLoadFloat4x4(&world_matrix) * rotation);, and restore the position portion of the matrix [accessing world_matrix._41/._42/._43] (world_matrix being the matrix of the "bullet" itself, not the parent).
[I've also tried to reverse the order of the multiplication]
I've read that the XMMatrixRotationQuaternion doesn't return as an Euler Quaternion, and XMQuaternionToAxisAngle does, though I'm not entirely certain how to use it.
What would be the proper way to accomplish something like this?
Many thanks!
Your code XMVECTOR quaternion = XMVectorSet(random_x, random_y, 0); is not creating a valid quaternion. First, if you did not set the w component to 1, then the 4-vector quaternion doesn't actually represent a 3D rotation. Second, a quaternion's vector components are not Euler angles.
You want to use XMQuaternionRotationRollPitchYaw which constructs a quaternion rotation from Euler angle input, or XMQuaternionRotationRollPitchYawFromVector which takes the three Euler angles as a vector. These functions are doing what Unity's Quaternion.Euler method is doing.
Of course, if you want a rotation matrix and not a quaternion, then you can XMMatrixRotationRollPitchYaw or XMMatrixRotationRollPitchYawFromVector to directly construct a 4x4 rotation matrix from Euler angles--which actually uses quaternions internally anyhow. Based on your code snippet, it looks like you already have a base rotation as a quaternion you want to concatenate with your spread quaternion, so you probably don't want to use this option for this case.
Note: You should look at using the C++11 standard <random> rather than your home-rolled lambda wrapper around the terrible C rand function.
Something like:
std::random_device rd;
std::mt19937 gen(rd());
// spread should be in radians here (not degrees which is what Unity uses)
std::uniform_real_distribution<float> dis(-spread, spread);
XMVECTOR coneRotation = XMQuaternionRotationRollPitchYaw( dis(gen), dis(gen), 0 );
XMVECTOR rot = XMQuaternionMultiply( parentRot, coneRotation );
XMMATRIX transform = XMMatrixAffineTransformation( g_XMOne, g_XMZero, rot, parentPos );
BTW, if you are used to Unity or XNA Game Studio C# math libraries, you might want to check out the SimpleMath wrapper for DirectXMath in DirectX Tool Kit.
I want to create a graphic calculator and Im stuck with the graph bit. I want to know how to plot a graph for sin(x) cos(x) tan(x). I have made the grid already. I dont want to use core plot framework.
Any help would be appreciated.
Thanks.
To actually plot the function, do like you would with paper and pencil: evaluate the function for a number of inputs. Then draw lines to connect the resulting points.
Not that I would actually do this (I would look at Core Plot), but you could plot such a graph using a Core Image generator filter, like this:
//wavelength and magnitude are distances in destination pixels. Think of them as the width and height of each wave.
kernel vec4 sineWave(float wavelength, float magnitude, __color color)
{
vec2 coord = destCoord();
coord.y -= magnitude;
coord /= vec2(wavelength, magnitude / 2.0);
float pi = radians(180.0);
float value = sin(coord.x * pi);
//Smaller threshold = finer wave line. For a gradient, replace the comparison with 1.0 - abs(…).
float threshold = 0.1;
float alpha = abs(coord.y - value) <= threshold;
return color * alpha;
}
Here is some pseudo-code that could answer your question:
for i = xmin to xmax do
{
draw XY point at X=(i*x_scale_factor+x_offset) and Y=(sin(i)*y_scale_factor+Y_offset);
}
And beware: don't use floats in for loops
EDIT in response to comments
The easiest way to proceed, IMHO, would be to get the bounds of your view, to get the min and max values of your data on both X and Y axis.
You then can use a NSAffineTransform instance to transform the coordinates of your drawings. So everything can be done in your graphic coordinates, which is easier. You can write a label at coordinates (4.6, 3.2*10-7) if you wish to. This is a key point to get you started. The road is long. But using NSAffineTransform will make it easier.
I must be misunderstanding something about GLKit's handling of quaternions and rotation matrices. In the following snippet, I would expect matrices a and b to end up with identical contents (subject to floating point errors)...
GLKQuaternion q = GLKQuaternionMakeWithAngleAndAxis(M_PI / 2, 1, 1, 1);
GLKMatrix3 a = GLKMatrix3MakeWithQuaternion(q);
GLKMatrix3 b = GLKMatrix3MakeRotation(M_PI / 2, 1, 1, 1);
However, they don't agree. Not even close. In column major order, the arrays contain...
a.m[0]=0.000000 b.m[0]=0.333333
a.m[1]=1.000000 b.m[1]=0.910684
a.m[2]=0.000000 b.m[2]=-0.244017
a.m[3]=0.000000 b.m[3]=-0.244017
a.m[4]=0.000000 b.m[4]=0.333333
a.m[5]=1.000000 b.m[5]=0.910684
a.m[6]=1.000000 b.m[6]=0.910684
a.m[7]=0.000000 b.m[7]=-0.244017
a.m[8]=0.000000 b.m[8]=0.333333
I thought that both GLKQuaternionMakeWithAngleAndAxis and GLKMatrix3MakeRotation each took radians, x, y, z in order to represent a rotation of the specified radians around the specified axis. And I thought that GLKMatrix3MakeWithQuaternion was intended to convert from the quaternion representation to the matrix representation.
So, why don't those agree? Do I need to normalize the axis before the quaternion creation? This does, in fact, seem to fix the problem but I don't believe it is documented that way.
From GLKQuaternion.h
/*
Assumes the axis is already normalized.
*/
static inline GLKQuaternion GLKQuaternionMakeWithAngleAndAxis(float radians, float x, float y, float z);
So yes, you do need to normalize the axis before creating the quaternion.