Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-22 15:53:56 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Math: made rotation matrices more C++

Browse Source 
thanks to Adam for the suggestion
This commit is contained in:
Andrew Tridgell 2012-03-10 17:06:35 +11:00
parent 92ada85f49
commit 6d08e38d5d
4 changed files with 46 additions and 45 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

39
libraries/AP_Math/AP_Math.cpp
View File

@ -30,42 +30,3 @@ float safe_sqrt(float v)
}
return ret;
}
// create a rotation matrix given some euler angles
// this is based on http://gentlenav.googlecode.com/files/EulerAngles.pdf
void rotation_matrix_from_euler(Matrix3f &m, float roll, float pitch, float yaw)
{
float cp = cos(pitch);
float sp = sin(pitch);
float sr = sin(roll);
float cr = cos(roll);
float sy = sin(yaw);
float cy = cos(yaw);
m.a.x = cp * cy;
m.a.y = (sr * sp * cy) - (cr * sy);
m.a.z = (cr * sp * cy) + (sr * sy);
m.b.x = cp * sy;
m.b.y = (sr * sp * sy) + (cr * cy);
m.b.z = (cr * sp * sy) - (sr * cy);
m.c.x = -sp;
m.c.y = sr * cp;
m.c.z = cr * cp;
}
// calculate euler angles from a rotation matrix
// this is based on http://gentlenav.googlecode.com/files/EulerAngles.pdf
void calculate_euler_angles(const Matrix3f &m, float *roll, float *pitch, float *yaw)
{
if (pitch != NULL) {
*pitch = -safe_asin(m.c.x);
}
if (roll != NULL) {
*roll = atan2(m.c.y, m.c.z);
}
if (yaw != NULL) {
*yaw = atan2(m.b.x, m.a.x);
}
}

6
libraries/AP_Math/AP_Math.h
View File

@ -24,10 +24,4 @@ float safe_asin(float v);
// a varient of sqrt() that always gives a valid answer.
float safe_sqrt(float v);
// create a rotation matrix given some euler angles
void rotation_matrix_from_euler(Matrix3f &m, float roll, float pitch, float yaw);
// calculate euler angles from a rotation matrix
void calculate_euler_angles(const Matrix3f &m, float *roll, float *pitch, float *yaw);
#endif

41
libraries/AP_Math/matrix3.cpp
View File

@ -94,5 +94,46 @@ void Matrix3<T>::rotation(enum Rotation r)
}
}
// create a rotation matrix given some euler angles
// this is based on http://gentlenav.googlecode.com/files/EulerAngles.pdf
template <typename T>
void Matrix3<T>::from_euler(float roll, float pitch, float yaw)
{
float cp = cos(pitch);
float sp = sin(pitch);
float sr = sin(roll);
float cr = cos(roll);
float sy = sin(yaw);
float cy = cos(yaw);
a.x = cp * cy;
a.y = (sr * sp * cy) - (cr * sy);
a.z = (cr * sp * cy) + (sr * sy);
b.x = cp * sy;
b.y = (sr * sp * sy) + (cr * cy);
b.z = (cr * sp * sy) - (sr * cy);
c.x = -sp;
c.y = sr * cp;
c.z = cr * cp;
}
// calculate euler angles from a rotation matrix
// this is based on http://gentlenav.googlecode.com/files/EulerAngles.pdf
template <typename T>
void Matrix3<T>::to_euler(float *roll, float *pitch, float *yaw)
{
if (pitch != NULL) {
*pitch = -safe_asin(c.x);
}
if (roll != NULL) {
*roll = atan2(c.y, c.z);
}
if (yaw != NULL) {
*yaw = atan2(b.x, a.x);
}
}
// only define for float
template void Matrix3<float>::rotation(enum Rotation);
template void Matrix3<float>::from_euler(float roll, float pitch, float yaw);
template void Matrix3<float>::to_euler(float *roll, float *pitch, float *yaw);

5
libraries/AP_Math/matrix3.h
View File

@ -131,6 +131,11 @@ public:
// fill in the matrix with a standard rotation
void rotation(enum Rotation rotation);
// create a rotation matrix from Euler angles
void from_euler(float roll, float pitch, float yaw);
// create eulers from a rotation matrix
void to_euler(float *roll, float *pitch, float *yaw);
};
typedef Matrix3<int16_t> Matrix3i;