Sign in to follow this  

Unity Vector to Euler angles

This topic is 3784 days old which is more than the 365 day threshold we allow for new replies. Please post a new topic.

If you intended to correct an error in the post then please contact us.

Recommended Posts

Hello, I am working on an example from http://www.euclideanspace.com/maths/algebra/vectors/lookat/index.htm but unfortunately, it is impossible for me to have the good result. This example is used to give the Euler angles from a vector. The C code is showed below. This example does not give me the good result. I don't know why, I am not a matrix expert... This code must use a function named LookAt() but, this function is unable to give me the good matrix. It is impossible for me to use DX or OpenGl because this code must be inserted in microcontroler. Thank you for your help, Gorillus #include <string.h> #include <stdlib.h> #include <stdio.h> #include <math.h> //***************************************************************************** typedef struct{ double m00; double m01; double m02; double m10; double m11; double m12; double m20; double m21; double m22; } Matrix3d_t; typedef struct{ double Heading; /* Fi */ double Attitude;/* Teta */ double Bank; /* Psi */ } EulerAngle_t; typedef struct{ double x; double y; double z; double Norm; } Vector3_t; //***************************************************************************** #define PI 3.141592654 //***************************************************************************** static void LookAt(Vector3_t * pDir, Vector3_t * pTarget, Matrix3d_t * pM); static void VectorCrossProduct(Vector3_t * pVector1, Vector3_t * pVector2, Vector3_t * pCross); static void VectorNormalise(Vector3_t * pVector); static double VectorDotProduct(Vector3_t * pV1, Vector3_t * pV2); static void EulerAnglesGet(Matrix3d_t * pPoint, EulerAngle_t * pAngles); static void MatrixSetComponents(Matrix3d_t * pM, Vector3_t * pX, Vector3_t * pY, Vector3_t * pZ); //***************************************************************************** void main(void) { Matrix3d_t M; EulerAngle_t Angles; Vector3_t VectorUnity; Vector3_t VectorDir; /* Starting point (Known as Pcurrent) */ VectorUnity.x = 1; VectorUnity.y = 0; VectorUnity.z = 0; VectorNormalise(&VectorUnity); /* Direction point (Known as Ptarget) */ VectorDir.x = 1; VectorDir.y = 1; VectorDir.z = 0; LookAt(&VectorUnity, &VectorDir, &M); printf("Matrix created by LookAt() :\n"); printf("%lf \t %lf \t %lf\n", M.m00, M.m01, M.m02); printf("%lf \t %lf \t %lf\n", M.m10, M.m11, M.m12); printf("%lf \t %lf \t %lf\n", M.m20, M.m21, M.m22); printf("\n"); EulerAnglesGet(&M, &Angles); printf("Euler angles : Heading = %lf, Attitude = %lf, Bank = %lf\n", Angles.Heading, Angles.Attitude, Angles.Bank); } //***************************************************************************** static void LookAt(Vector3_t * pDir, Vector3_t * pTarget, Matrix3d_t * pM) { Vector3_t x; Vector3_t y; Vector3_t z; // z = pDir memcpy(&z, pDir, sizeof(Vector3_t)); VectorNormalise(&z); VectorCrossProduct(pTarget, &z, &x);// x = Target cross z VectorNormalise(&x); VectorCrossProduct(&z, &x, &y);// y = z cross x MatrixSetComponents(pM, &x, &y, &z); } //***************************************************************************** static void VectorCrossProduct(Vector3_t * pVector1, Vector3_t * pVector2, Vector3_t * pCross) { pCross->x = pVector1->y * pVector2->z - pVector1->z * pVector2->y; pCross->y = pVector1->z * pVector2->x - pVector1->x * pVector2->z; pCross->z = pVector1->x * pVector2->y - pVector1->y * pVector2->x; } //***************************************************************************** static void VectorNormalise(Vector3_t * pVector) { double fLength = sqrt( pVector->x * pVector->x + pVector->y * pVector->y + pVector->z * pVector->z); // Will also work for zero-sized vectors, but will change nothing if ( fLength > 1e-06 ) { double fInvLength = 1.0 / fLength; pVector->x *= fInvLength; pVector->y *= fInvLength; pVector->z *= fInvLength; } pVector->Norm = fLength; } //***************************************************************************** static double VectorDotProduct(Vector3_t * pV1, Vector3_t * pV2) { return pV1->x * pV2->x + pV1->y * pV2->y + pV1->z * pV2->z; } //***************************************************************************** static void EulerAnglesGet(Matrix3d_t * pPoint, EulerAngle_t * pAngles) { if (pPoint->m10 > 0.998) { // singularity at north pole pAngles->Heading = atan2(pPoint->m02, pPoint->m22); pAngles->Attitude = PI / 2; pAngles->Bank = 0; return; } if (pPoint->m10 < -0.998) { // singularity at south pole pAngles->Heading = atan2(pPoint->m02, pPoint->m22); pAngles->Attitude = -PI / 2; pAngles->Bank = 0; return; } pAngles->Heading = atan2(-pPoint->m20, pPoint->m00); pAngles->Bank = atan2(-pPoint->m12, pPoint->m11); pAngles->Attitude = asin(pPoint->m10); } //***************************************************************************** static void MatrixSetComponents(Matrix3d_t * pM, Vector3_t * pX, Vector3_t * pY, Vector3_t * pZ) { pM->m00 = pX->x; pM->m01 = pY->x; pM->m02 = pZ->x; pM->m10 = pX->y; pM->m11 = pY->y; pM->m12 = pZ->y; pM->m20 = pX->z; pM->m21 = pY->z; pM->m22 = pZ->z; } //*****************************************************************************

Share this post


Link to post
Share on other sites
I'll be happy to take a look at this, but could you edit your post so that the code is formatted correctly? (It'll be easier to read that way.)

Usually, putting it in [source][/source] tags will do the trick.

Share this post


Link to post
Share on other sites

This topic is 3784 days old which is more than the 365 day threshold we allow for new replies. Please post a new topic.

If you intended to correct an error in the post then please contact us.

Create an account or sign in to comment

You need to be a member in order to leave a comment

Create an account

Sign up for a new account in our community. It's easy!

Register a new account

Sign in

Already have an account? Sign in here.

Sign In Now

Sign in to follow this