How to estimate the surface normal of point I(i,j) on a depth image (pixel value in mm) without using Point Cloud Library(PCL)? I've gone through (1), (2), and (3) but I'm looking for a simple estimation of surface normal on each pixel with C++ standard library or openCV.
You need to know the camera's intrinsic parameters, so that you can also know the distance between pixels in the same units (mm). This distance between pixels is obviously true for a certain distance from the camera (i.e. the value of the center pixel)
If the camera matrix is K which is typically something like:
f 0 cx
K= 0 f cy
0 0 1
Then, taking a pixel coordinates (x,y), then a ray from the camera origin through the pixel (in camera world coordinate space) is defined using:
x
P = inv(K) * y
1
Depending of whether the distance in your image is a projection on the Z axis, or just a euclidean distance from the center, you need to either normalize the vector P such that the magnitude is the distance to the pixel you want, or make sure the z component of P is this distance. For pixels around the center of the frame this should be close to identical.
If you do the same operation to nearby pixels (say, left and right) you get Pl and Pr in units of mm
Then just find the norm of (Pl-Pr) which is twice the distance between adjacent pixels in mm.
Then, you calculate the gradient in X and Y
gx = (Pi+1,j - Pi-1,j) / (2*pixel_size)
Then, take the two gradients as direction vectors:
ax = atan(gx), ay=atan(gy)
| cos ax 0 sin ax | |1|
dx = | 0 1 0 | * |0|
| -sin ax 0 cos ax | |0|
| 1 0 0 | |0|
dy = | 0 cos ay -sin ay | * |1|
| 0 sin ay cos ay | |0|
N = cross(dx,dy);
You may need to see if the signs make sense, by looking at a certain gradient and seeing of the dx,dy point to the expected direction. You may need to use a negative for none/one/both angles and same for the N vector.
I am working on a game project using OpenGl. I am building a game from skeleton code I found online. I have a character that can move around in a 2D plane. (x and z, ie you are viewing the character from above.) I am currently stuck on making him rotate as he moves, and I can't seem to find a solution online that solves my problem.
At the moment when the character is being drawn he faces a certain way (along the arrow in my diagram below.). I can rotate him an arbitrary number of degrees from his default direction using glm::rotate.
I have updated the code to log the characters position from a frame ago when he moves, so I have this information:
character old position (known)-> O
character starting angle (unknown)-> |\
| \
| \
|(X)\
| \
V O <- character new position (known)
How do I compute the angle (X)? Is it possible with the information I have?
I have been doodling on a page trying to figure this out for the last hour but can't seem to figure it out. Thanks very much.
Yes. This answer gives you an example of how to do it: How to calculate the angle between a line and the horizontal axis?
Note however that that will give you the angle between the horizontal axsis and the point. You can however just add 90 degrees.
What you're doing sounds somewhat convoluted. From the description, it seems like you want a rotation matrix that matches the direction. There's really no need to calculate an angle. You can build the rotation matrix directly, which is easier and more efficient.
I'll illustrate the calculations with points/vectors in the xy-plane, since that's much more standard. It sounds like you're operating in the xz-plane. While that doesn't change things much, you might need slight changes because you have a left-handed coordinate system.
If you have the direction vector (difference between new position and old position), all you need to do is normalize it, and you already have what's needed for the rotation matrix. I'll write the calculation explicitly, but your matrix/vector library most likely has a method to normalize a vector.
float vx = nexPosX - oldPosX;
float vy = newPosY - oldPosY;
float s = 1.0f / sqrt(vx * vx + vy * vy);
vx *= s;
vy *= s;
vx is now the cosine of the rotation angle, and vy the sine of the rotation angle. Substituting this into the standard form of a rotation matrix, you get:
R = ( cos(phi) -sin(phi) ) = ( vx -vy )
( sin(phi) cos(phi) ) ( vy vx )
This is the absolute rotation for the new direction. If you need the relative rotation between old direction and new direction, it just takes a few more operations. Let's say you already calculated the normalized vectors for the old and new directions as (v1x, v1y) and (v2x, v2y). The cosine of the rotation angle is the scalar product of the two vectors:
cosPhi = v1x * v2x + v1y * v2y;
and the sine is the length of the cross product. Since both vectors are in the xy-plane, that's simply the z-component of the cross product:
sinPhi = v1x * v2y - v1y * v2x;
With these two values, you can directly build the rotation matrix again:
R = ( cosPhi -sinPhi )
( sinPhi cosPhi )
As the title says, i have a problem to convert a Quaternion to a Matrix4f. Eigen has the method Quaternion.toRotationMatrix() which gives me a Matrix3f.
Now i need a Matrix4f ( because our program is designed to take only Matrix4f), is there an easy way to achieve this?
#Zacharias' answer contains the necessary theory. I just reiterate my earlier comment I made there with the actual Eigen C++ code.
Eigen::Matrix3f mat3 = Eigen::Quaternionf(W, X, Y, Z).toRotationMatrix();
Eigen::Matrix4f mat4 = Eigen::Matrix4f::Identity();
mat4.block(0,0,3,3) = mat3;
Eigen::Matrix4f::Identity() takes care of initializing the ones and zeros of the 4th and last row and column. mat4.block(0,0,3,3) = mat3 then overwrites the values obtained from the rotation matrix.
M3 to M4
The answere is already there, given by Rob and Najzero.
In most cases, it will be sufficient to construct the matrix as follows:
m3:
|a00|a01|a02|
|a10|a11|a12|
|a20|a21|a22|
to m4:
|a00|a01|a02| 0 |
|a10|a11|a12| 0 |
|a20|a21|a22| 0 |
| 0 | 0 | 0 | 1 |
The 4x4 matrix does not only allow to rotate a vector, but also to shift(translate) and scale (in all 3 directions) any vector. So basically you got a full transformation matrix - thats why it is often used in computer graphics, describing the transformation of an object. Depending on row-column order, we might identify the matrix as:
|rot|rot|rot| sx |
|rot|rot|rot| sy |
|rot|rot|rot| sz |
| x | y | z | 1 |
with sx,sy,sz as scaling coefficients, and x,y,z as translation coefficients.
PS: of course, if you want to rotate a vector with m4, you will than have to use a 4-dimensional vector, e.g. (x,y,z,w) with w=1 (in most cases).
The direct approach
Convert Quaternion rotation to rotation matrix?
And my personal recommendation:
http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToMatrix/
There you will find also other transformations, backtrafos and so on.
I have drawn a triangle mesh with 10000 vertices(100x100) and it will be a grass ground. I used gldrawelements() for it. I have looked all day and still can't understand how to calculate the normals for this. Does each vertex have its own normals or does each triangle have its own normals? Can someone point me in the right direction on how to edit my code to incorporate normals?
struct vertices {
GLfloat x;
GLfloat y;
GLfloat z;
}vertices[10000];
GLuint indices[60000];
/*
99..9999
98..9998
........
01..9901
00..9900
*/
void CreateEnvironment() {
int count=0;
for (float x=0;x<10.0;x+=.1) {
for (float z=0;z<10.0;z+=.1) {
vertices[count].x=x;
vertices[count].y=0;
vertices[count].z=z;
count++;
}
}
count=0;
for (GLuint a=0;a<99;a++){
for (GLuint b=0;b<99;b++){
GLuint v1=(a*100)+b;indices[count]=v1;count++;
GLuint v2=(a*100)+b+1;indices[count]=v2;count++;
GLuint v3=(a*100)+b+100;indices[count]=v3;count++;
}
}
count=30000;
for (GLuint a=0;a<99;a++){
for (GLuint b=0;b<99;b++){
indices[count]=(a*100)+b+100;count++;//9998
indices[count]=(a*100)+b+1;count++;//9899
indices[count]=(a*100)+b+101;count++;//9999
}
}
}
void ShowEnvironment(){
//ground
glPushMatrix();
GLfloat GroundAmbient[]={0.0,0.5,0.0,1.0};
glMaterialfv(GL_FRONT,GL_AMBIENT,GroundAmbient);
glEnableClientState(GL_VERTEX_ARRAY);
glIndexPointer( GL_UNSIGNED_BYTE, 0, indices );
glVertexPointer(3,GL_FLOAT,0,vertices);
glDrawElements(GL_TRIANGLES,60000,GL_UNSIGNED_INT,indices);
glDisableClientState(GL_VERTEX_ARRAY);
glPopMatrix();
}
EDIT 1
Here is the code I have written out. I just used arrays instead of vectors and I stored all of the normals in the struct called normals. It still doesn't work however. I get an unhandled exception at *indices.
struct Normals {
GLfloat x;
GLfloat y;
GLfloat z;
}normals[20000];
Normals* normal = normals;
//***************************************ENVIRONMENT*************************************************************************
struct vertices {
GLfloat x;
GLfloat y;
GLfloat z;
}vertices[10000];
GLuint indices[59403];
/*
99..9999
98..9998
........
01..9901
00..9900
*/
void CreateEnvironment() {
int count=0;
for (float x=0;x<10.0;x+=.1) {
for (float z=0;z<10.0;z+=.1) {
vertices[count].x=x;
vertices[count].y=rand()%2-2;;
vertices[count].z=z;
count++;
}
}
//calculate normals
GLfloat vector1[3];//XYZ
GLfloat vector2[3];//XYZ
count=0;
for (int x=0;x<9900;x+=100){
for (int z=0;z<99;z++){
vector1[0]= vertices[x+z].x-vertices[x+z+1].x;//vector1x
vector1[1]= vertices[x+z].y-vertices[x+z+1].y;//vector1y
vector1[2]= vertices[x+z].z-vertices[x+z+1].z;//vector1z
vector2[0]= vertices[x+z+1].x-vertices[x+z+100].x;//vector2x
vector2[1]= vertices[x+z+1].y-vertices[x+z+100].y;//vector2y
vector2[2]= vertices[x+z+1].z-vertices[x+z+100].z;//vector2z
normals[count].x= vector1[1] * vector2[2]-vector1[2]*vector2[1];
normals[count].y= vector1[2] * vector2[0] - vector1[0] * vector2[2];
normals[count].z= vector1[0] * vector2[1] - vector1[1] * vector2[0];count++;
}
}
count=10000;
for (int x=100;x<10000;x+=100){
for (int z=0;z<99;z++){
vector1[0]= vertices[x+z].x-vertices[x+z+1].x;//vector1x -- JUST ARRAYS
vector1[1]= vertices[x+z].y-vertices[x+z+1].y;//vector1y
vector1[2]= vertices[x+z].z-vertices[x+z+1].z;//vector1z
vector2[0]= vertices[x+z+1].x-vertices[x+z-100].x;//vector2x
vector2[1]= vertices[x+z+1].y-vertices[x+z-100].y;//vector2y
vector2[2]= vertices[x+z+1].z-vertices[x+z-100].z;//vector2z
normals[count].x= vector1[1] * vector2[2]-vector1[2]*vector2[1];
normals[count].y= vector1[2] * vector2[0] - vector1[0] * vector2[2];
normals[count].z= vector1[0] * vector2[1] - vector1[1] * vector2[0];count++;
}
}
count=0;
for (GLuint a=0;a<99;a++){
for (GLuint b=0;b<99;b++){
GLuint v1=(a*100)+b;indices[count]=v1;count++;
GLuint v2=(a*100)+b+1;indices[count]=v2;count++;
GLuint v3=(a*100)+b+100;indices[count]=v3;count++;
}
}
count=30000;
for (GLuint a=0;a<99;a++){
for (GLuint b=0;b<99;b++){
indices[count]=(a*100)+b+100;count++;//9998
indices[count]=(a*100)+b+1;count++;//9899
indices[count]=(a*100)+b+101;count++;//9999
}
}
}
void ShowEnvironment(){
//ground
glPushMatrix();
GLfloat GroundAmbient[]={0.0,0.5,0.0,1.0};
GLfloat GroundDiffuse[]={1.0,0.0,0.0,1.0};
glMaterialfv(GL_FRONT,GL_AMBIENT,GroundAmbient);
glMaterialfv(GL_FRONT,GL_DIFFUSE,GroundDiffuse);
glEnableClientState(GL_VERTEX_ARRAY);
glEnableClientState(GL_NORMAL_ARRAY);
glNormalPointer( GL_FLOAT, 0, normal);
glVertexPointer(3,GL_FLOAT,0,vertices);
glDrawElements(GL_TRIANGLES,60000,GL_UNSIGNED_INT,indices);
glDisableClientState(GL_VERTEX_ARRAY);
glDisableClientState(GL_NORMAL_ARRAY);
glPopMatrix();
}
//***************************************************************************************************************************
Does each vertex have its own normals or does each triangle have its own normals?
Like so often the answer is: "It depends". Since a normal is defined as being the vector perpendicular to all vectors within a given plane (in N dimensions), you need a plane to calculate a normal. A vertex position is just a point and thus singular, so you actually need a face to calculate the normal. Thus, naively, one could assume that normals are per face as the first step in normal calculation is determining the face normals, by evaluating the cross product of the faces edges.
Say you have a triangle with points A, B, C, then these points have position vectors ↑A, ↑B, ↑C and the edges have vectors ↑B - ↑A and ↑C - ↑A so the face normal vector is ↑Nf = (↑B - ↑A) × (↑C - ↑A)
Note that the magnitude of ↑Nf as it's stated above is directly proportional to the face's area.
In smooth surfaces vertices are shared between faces (or you could say those faces share a vertex). In that case the normal at the vertex is not one of the face normals of the faces it is part of, but a linear combination of them:
↑Nv = ∑ p ↑Nf ; where p is a weighting for each face.
One could either assume a equal weighting between the participating face normals. But it makes more sense to assume that the larger a face is, the more it contributes to the normal.
Now recall that you normalize by a vector ↑v by scaling it with it's recipocal length: ↑vi = ↑v/|↑v|. But as already told the length of the face normals already depends on the face's area. So the weighting factor p given above is already contained in the vector itself: Its length, aka magnitude. So we can get the vertex normal vector by simply summing up all the face normals.
In lighting calculations the normal vector must be unit length, i.e. normalized to be useable. So after summing up, we normalize the newly found vertex normal and use that.
The carefull reader may have noticed I specifically said smooth surfaces share vertices. And in fact, if you have some creases / hard edges in your geometry, then the faces on either side don't share vertices. In OpenGL a vertex is the whole combination of
position
normal
(colour)
N texture coordinates
M further attributes
You change one of these and you got a completely different vertex. Now some 3D modelers see a vertex only as a point's position and store the rest of those attributes per face (Blender is such a modeler). This saves some memory (or considerable memory, depending on the number of attributes). But OpenGL needs the whole thing, so if working with such a mixed paradigm file you will have to decompose it into OpenGL compatible data first. Have a look at one of Blender's export scripts, like the PLY exporter to see how it's done.
Now to cover some other thing. In your code you have this:
glIndexPointer( GL_UNSIGNED_BYTE, 0, indices );
The index pointer has nothing to do with vertex array indices! This is an anachronsim from the days, when graphics still used palettes instead of true color. A pixels colour wasn't set by giving it's RGB values, but by a single number offsetting into a limited palette of colours. Palette colours can still be found in several graphics file formats, but no decent piece of hardware uses them anymore.
Please erase glIndexPointer (and glIndex) from your memory and your code, they don't do what you think they do The whole indexed color mode is arcane to used, and frankly I don't know of any hardware built after 1998 that still supported it.
Thumbs up for datenwolf! I completely agree with his approach. Adding the normal vectors of the adjacent triangles for each vertex and then normalising is the way to go. I just want to push the answer a little bit and have a closer look at the particular but quite common case of a rectangular, smooth mesh that has a constant x/y step. In other words, a rectangular x/y grid with a variable height at each point.
Such a mesh is created by looping over x and y and setting a value for z and can represent things like the surface of a hill. So each point of the mesh is represented by a vector
P = (x, y, f(x,y))
where f(x,y) is a function giving the z of each point on the grid.
Usually to draw such a mesh we use a TriangleStrip or a TriangleFan but any technique should give a similar topography for the resulting triangles.
|/ |/ |/ |/
...--+----U----UR---+--...
/| /| 2 /| /| Y
/ | / | / | / | ^
| / | / | / | / |
|/ 1 |/ 3 |/ |/ |
...--L----P----R----+--... +-----> X
/| 6 /| 4 /| /|
/ | / | / | / |
| /5 | / | / | /
|/ |/ |/ |/
...--DL---D----+----+--...
/| /| /| /|
For a triangleStrip each vertex P=(x0, y0, z0) has 6 adjacent vertices denoted
up = (x0 , y0 + ay, Zup)
upright = (x0 + ax, y0 + ay, Zupright)
right = (x0 + ax, y0 , Zright)
down = (x0 , y0 - ay, Zdown)
downleft = (x0 - ax, y0 - ay, Zdownleft)
left = (x0 - ax, y0 , Zleft)
where ax/ay is the constant grid step on the x/y axis respectively. On a square grid ax = ay.
ax = width / (nColumns - 1)
ay = height / (nRows - 1)
Thus each vertex has 6 adjacent triangles each one with its own normal vector (denoted N1 to N6). These can be calculated using the cross product of the two vectors defining the side of the triangle and being careful on the order in which we do the cross product. If the normal vector points in the Z direction towards you :
N1 = up x left =
= (Yup*Zleft - Yleft*Zup, Xleft*Zup - Xup*ZLeft, Xleft*Yup - Yleft*Xup)
=( (y0 + ay)*Zleft - y0*Zup,
(x0 - ax)*Zup - x0*Zleft,
x0*y0 - (y0 + ay)*(x0 - ax) )
N2 = upright x up
N3 = right x upright
N4 = down x right
N5 = downleft x down
N6 = left x downleft
And the resulting normal vector for each point P is the sum of N1 to N6. We normalise after summing. It's very easy to create a loop, calculate the values of each normal vector, add them and then normalise. However, as pointed out by Mr. Shickadance, this can take quite a while, especially for large meshes and/or on embedded devices.
If we have a closer look and perform the calculations by hand, we will find out that most of the terms cancel out each other, leaving us with a very elegant and easy to calculate final solution for the resulting vector N. The point here is to speed up calculations by avoiding calculating the coordinates of N1 to N6, doing 6 cross-products and 6 additions for each point. Algebra helps us to jump straight to the solution, use less memory and less CPU time.
I will not show the details of the calculations as it is long but straight-forward and will jump to the final expression of the Normal vector for any point on the grid. Only N1 is decomposed for the sake of clarity, the other vectors look alike. After summing we obtain N which is not yet normalized :
N = N1 + N2 + ... + N6
= .... (long but easy algebra) ...
= ( (2*(Zleft - Zright) - Zupright + Zdownleft + Zup - Zdown) / ax,
(2*(Zdown - Zup) + Zupright + Zdownleft - Zup - Zleft) / ay,
6 )
There you go! Just normalise this vector and you have the normal vector for any point on the grid, provided you know the Z values of its surrounding points and the horizontal/vertical step of your grid.
Note that this is the weighed average of the surrounding triangles' normal vectors. The weight is the area of the triangles and is already included in the cross product.
You can even simplify it more by only taking into account the Z values of four surrounding points (up,down,left and right). In that case you get :
| \|/ |
N = N1 + N2 + N3 + N4 ..--+----U----+--..
= ( (Zleft - Zright) / ax, | /|\ |
(Zdown - Zup ) / ay, | / | \ |
2 ) \ | / 1|2 \ | /
\|/ | \|/
..--L----P----R--...
/|\ | /|\
/ | \ 4|3 / | \
| \ | / |
| \|/ |
..--+----D----+--..
| /|\ |
which is even more elegant and even faster to calculate.
Hope this will make some meshes faster.
Cheers
Per-vertex.
Use cross-products to calculate the face normals for the triangles surrounding a given vertex, add them together, and normalize.
As simple as it may seem, calculating the normal of a triangle is only part of the problem. The cross product of 2 sides of the polygon is sufficient in triangular cases, unless the triangle is collapsed onto itself and degenerate; in that case there is no one valid normal, so you can select one to your liking.
So why is the normalized cross product only part of the problem? The winding order of the vertices in that polygon defines the direction of the normal, i.e. if one pair of vertices is swapped in place, the normal will point in the opposite direction. So in fact this can be problematic if the mesh itself contains inconsistencies in that regard, i.e. parts of it assume one ordering, while other parts assume different orderings. One famous example is the original Stanford Bunny model, where some parts of the surface will point inwards, while others point outwards. The reason for that is because the model was constructed using a scanner, and no care was taken to produce triangles with regular winding patterns. (obviously, clean versions of the bunny also exist)
The winding problem is even more prominent if polygons can have multiple vertices, because in that case you would be averaging partial normals of the semi-triangulation of that polygon. Consider the case where partial normals are pointing in opposite directions, resulting in normal vectors of length 0 when taking the mean!
In the same sense, disconnected polygon soups and point clouds present challenges for accurate reconstruction due to the ill-defined winding number.
One potential strategy that is often used to solve this problem is to shoot random rays from outward to the center of each semi-triangulation (i.e. ray-stabbing). But one cannot assume that the triangulation is valid if polygons can contain multiple vertices, so rays may miss that particular sub-triangle. If a ray hits, then normal opposite to the ray direction, i.e. with dot(ray, n) < .5 satisfied, can be used as the normal for the entire polygon. Obviously this is rather expensive and scales with the number of vertices per polygon.
Thankfully, there's great new work that describes an alternative method that is not only faster (for large and complex meshes) but also generalizes the 'winding order' concept for constructions beyond polygon meshes, such as point clouds and polygon soups, iso-surfaces, and point-set surfaces, where connectivity may not even be defined!
As outlined in the paper, the method constructs a hierarchical splitting tree representation that is refined progressively, taking the parent 'dipole' orientation into account at every split operation. A polygon normal would then simply be an integration (mean) over all di-poles (i.e. point+normal pairs) of the polygon.
For people who are dealing with unclean mesh/pcl data from Lidar scanners or other sources, this could def. be a game-changer.
For those like me who came across this question, your answer might be this :
// Compute Vertex Normals
std::vector<sf::Glsl::Vec3> verticesNormal;
verticesNormal.resize(verticesCount);
for (i = 0; i < indices.size(); i += 3)
{
// Get the face normal
auto vector1 = verticesPos[indices[(size_t)i + 1]] - verticesPos[indices[i]];
auto vector2 = verticesPos[indices[(size_t)i + 2]] - verticesPos[indices[i]];
auto faceNormal = sf::VectorCross(vector1, vector2);
sf::Normalize(faceNormal);
// Add the face normal to the 3 vertices normal touching this face
verticesNormal[indices[i]] += faceNormal;
verticesNormal[indices[(size_t)i + 1]] += faceNormal;
verticesNormal[indices[(size_t)i + 2]] += faceNormal;
}
// Normalize vertices normal
for (i = 0; i < verticesNormal.size(); i++)
sf::Normalize(verticesNormal[i]);
The easy way is to translate one of the triangles (p1,p2,p3) points (say p1) to (0,0,0) so that means (x2,y2,z2)->(x2-x1,y2-y1,z2-z1) and (x3,y3,z3)->(x3-x1,y3-y1,z3-z1). Then you perform a dot product on the transformed points to obtain the planar slope, or cross-product to obtain the outward normal.
See:
https://en.wikipedia.org/wiki/Cross_product#/media/File:Cross_product_vector.svg
for a simple visual representation of the difference between cross product and dot product.
The moving of one of the points to the origin is basically equivalent to generating vectors along p1p2 and p2p3.
I have pitch, roll, and yaw angles. How would I convert these to a directional vector?
It'd be especially cool if you can show me a quaternion and/or matrix representation of this!
Unfortunately there are different conventions on how to define these things (and roll, pitch, yaw are not quite the same as Euler angles), so you'll have to be careful.
If we define pitch=0 as horizontal (z=0) and yaw as counter-clockwise from the x axis, then the direction vector will be
x = cos(yaw)*cos(pitch)
y = sin(yaw)*cos(pitch)
z = sin(pitch)
Note that I haven't used roll; this is direction unit vector, it doesn't specify attitude. It's easy enough to write a rotation matrix that will carry things into the frame of the flying object (if you want to know, say, where the left wing-tip is pointing), but it's really a good idea to specify the conventions first. Can you tell us more about the problem?
EDIT:
(I've been meaning to get back to this question for two and a half years.)
For the full rotation matrix, if we use the convention above and we want the vector to yaw first, then pitch, then roll, in order to get the final coordinates in the world coordinate frame we must apply the rotation matrices in the reverse order.
First roll:
| 1 0 0 |
| 0 cos(roll) -sin(roll) |
| 0 sin(roll) cos(roll) |
then pitch:
| cos(pitch) 0 -sin(pitch) |
| 0 1 0 |
| sin(pitch) 0 cos(pitch) |
then yaw:
| cos(yaw) -sin(yaw) 0 |
| sin(yaw) cos(yaw) 0 |
| 0 0 1 |
Combine them, and the total rotation matrix is:
| cos(yaw)cos(pitch) -cos(yaw)sin(pitch)sin(roll)-sin(yaw)cos(roll) -cos(yaw)sin(pitch)cos(roll)+sin(yaw)sin(roll)|
| sin(yaw)cos(pitch) -sin(yaw)sin(pitch)sin(roll)+cos(yaw)cos(roll) -sin(yaw)sin(pitch)cos(roll)-cos(yaw)sin(roll)|
| sin(pitch) cos(pitch)sin(roll) cos(pitch)sin(roll)|
So for a unit vector that starts at the x axis, the final coordinates will be:
x = cos(yaw)cos(pitch)
y = sin(yaw)cos(pitch)
z = sin(pitch)
And for the unit vector that starts at the y axis (the left wing-tip), the final coordinates will be:
x = -cos(yaw)sin(pitch)sin(roll)-sin(yaw)cos(roll)
y = -sin(yaw)sin(pitch)sin(roll)+cos(yaw)cos(roll)
z = cos(pitch)sin(roll)
There are six different ways to convert three Euler Angles into a Matrix depending on the Order that they are applied:
typedef float Matrix[3][3];
struct EulerAngle { float X,Y,Z; };
// Euler Order enum.
enum EEulerOrder
{
ORDER_XYZ,
ORDER_YZX,
ORDER_ZXY,
ORDER_ZYX,
ORDER_YXZ,
ORDER_XZY
};
Matrix EulerAnglesToMatrix(const EulerAngle &inEulerAngle,EEulerOrder EulerOrder)
{
// Convert Euler Angles passed in a vector of Radians
// into a rotation matrix. The individual Euler Angles are
// processed in the order requested.
Matrix Mx;
const FLOAT Sx = sinf(inEulerAngle.X);
const FLOAT Sy = sinf(inEulerAngle.Y);
const FLOAT Sz = sinf(inEulerAngle.Z);
const FLOAT Cx = cosf(inEulerAngle.X);
const FLOAT Cy = cosf(inEulerAngle.Y);
const FLOAT Cz = cosf(inEulerAngle.Z);
switch(EulerOrder)
{
case ORDER_XYZ:
Mx.M[0][0]=Cy*Cz;
Mx.M[0][1]=-Cy*Sz;
Mx.M[0][2]=Sy;
Mx.M[1][0]=Cz*Sx*Sy+Cx*Sz;
Mx.M[1][1]=Cx*Cz-Sx*Sy*Sz;
Mx.M[1][2]=-Cy*Sx;
Mx.M[2][0]=-Cx*Cz*Sy+Sx*Sz;
Mx.M[2][1]=Cz*Sx+Cx*Sy*Sz;
Mx.M[2][2]=Cx*Cy;
break;
case ORDER_YZX:
Mx.M[0][0]=Cy*Cz;
Mx.M[0][1]=Sx*Sy-Cx*Cy*Sz;
Mx.M[0][2]=Cx*Sy+Cy*Sx*Sz;
Mx.M[1][0]=Sz;
Mx.M[1][1]=Cx*Cz;
Mx.M[1][2]=-Cz*Sx;
Mx.M[2][0]=-Cz*Sy;
Mx.M[2][1]=Cy*Sx+Cx*Sy*Sz;
Mx.M[2][2]=Cx*Cy-Sx*Sy*Sz;
break;
case ORDER_ZXY:
Mx.M[0][0]=Cy*Cz-Sx*Sy*Sz;
Mx.M[0][1]=-Cx*Sz;
Mx.M[0][2]=Cz*Sy+Cy*Sx*Sz;
Mx.M[1][0]=Cz*Sx*Sy+Cy*Sz;
Mx.M[1][1]=Cx*Cz;
Mx.M[1][2]=-Cy*Cz*Sx+Sy*Sz;
Mx.M[2][0]=-Cx*Sy;
Mx.M[2][1]=Sx;
Mx.M[2][2]=Cx*Cy;
break;
case ORDER_ZYX:
Mx.M[0][0]=Cy*Cz;
Mx.M[0][1]=Cz*Sx*Sy-Cx*Sz;
Mx.M[0][2]=Cx*Cz*Sy+Sx*Sz;
Mx.M[1][0]=Cy*Sz;
Mx.M[1][1]=Cx*Cz+Sx*Sy*Sz;
Mx.M[1][2]=-Cz*Sx+Cx*Sy*Sz;
Mx.M[2][0]=-Sy;
Mx.M[2][1]=Cy*Sx;
Mx.M[2][2]=Cx*Cy;
break;
case ORDER_YXZ:
Mx.M[0][0]=Cy*Cz+Sx*Sy*Sz;
Mx.M[0][1]=Cz*Sx*Sy-Cy*Sz;
Mx.M[0][2]=Cx*Sy;
Mx.M[1][0]=Cx*Sz;
Mx.M[1][1]=Cx*Cz;
Mx.M[1][2]=-Sx;
Mx.M[2][0]=-Cz*Sy+Cy*Sx*Sz;
Mx.M[2][1]=Cy*Cz*Sx+Sy*Sz;
Mx.M[2][2]=Cx*Cy;
break;
case ORDER_XZY:
Mx.M[0][0]=Cy*Cz;
Mx.M[0][1]=-Sz;
Mx.M[0][2]=Cz*Sy;
Mx.M[1][0]=Sx*Sy+Cx*Cy*Sz;
Mx.M[1][1]=Cx*Cz;
Mx.M[1][2]=-Cy*Sx+Cx*Sy*Sz;
Mx.M[2][0]=-Cx*Sy+Cy*Sx*Sz;
Mx.M[2][1]=Cz*Sx;
Mx.M[2][2]=Cx*Cy+Sx*Sy*Sz;
break;
}
return(Mx);
}
FWIW, some CPU's can compute Sin & Cos simultaneously (for example fsincos on x86). If you do this, you can make it a bit faster with three calls rather than 6 to compute the initial sin & cos values.
Update: There are actually 12 ways depending if you want right-handed or left-handed results -- you can change the "handedness" by negating the angles.
Beta saved my day. However I'm using a slightly different reference coordinate system and my definition of pitch is up\down (nodding your head in agreement) where a positive pitch results in a negative y-component. My reference vector is OpenGl style (down the -z axis) so with yaw=0, pitch=0 the resulting unit vector should equal (0, 0, -1).
If anyone comes across this post and has difficulties translating Beta's formulas to this particular system, the equations I use are:
vDir->X = sin(yaw);
vDir->Y = -(sin(pitch)*cos(yaw));
vDir->Z = -(cos(pitch)*cos(yaw));
Note the sign change and the yaw <-> pitch swap. Hope this will save someone some time.
You need to be clear about your definitions here - in particular, what is the vector you want? If it's the direction an aircraft is pointing, the roll doesn't even affect it, and you're just using spherical coordinates (probably with axes/angles permuted).
If on the other hand you want to take a given vector and transform it by these angles, you're looking for a rotation matrix. The wiki article on rotation matrices contains a formula for a yaw-pitch-roll rotation, based on the xyz rotation matrices. I'm not going to attempt to enter it here, given the greek letters and matrices involved.
If someone stumbles upon looking for implementation in FreeCAD.
import FreeCAD, FreeCADGui
from FreeCAD import Vector
from math import sin, cos, pi
cr = FreeCADGui.ActiveDocument.ActiveView.getCameraOrientation().toEuler()
crx = cr[2] # Roll
cry = cr[1] # Pitch
crz = cr[0] # Yaw
crx = crx * pi / 180.0
cry = cry * pi / 180.0
crz = crz * pi / 180.0
x = sin(crz)
y = -(sin(crx) * cos(crz))
z = cos(crx) * cos(cry)
view = Vector(x, y, z)