Basically you have to:
- Create a rotation matrix from Yaw/Pitch and Roll
- Multiply your local direction vector (z-axis) by that matrix and multiply the resulting vector by speed, that is the translation
- Add that translation vector to your current position
I added a code example. There are 2 convenience classes that do most of the ugly work: vector and matrix. Scroll directly down to main, to see the implementation of 1) 2) and 3)
#include <cmath>
#include <cassert>
#include <cstdio>
#define NEARLY_EQUAL_EPS_F( a, b, eps ) (fabsf( (a)-(b) ) < (eps))
#define ASSERT_VECTOR( v ) (assert( NEARLY_EQUAL_EPS_F(0.0f, (v)[3], 1.e-7)))
#define FLOAT_FORMATOR "%10.6f"
namespace demo
{
class vector
{
public:
vector() {}
explicit vector( const float _v[4] ) { v[0]=_v[0]; v[1]=_v[1]; v[2]=_v[2]; v[3]=_v[3]; }
vector( const vector &_v ) { v[0]=_v[0]; v[1]=_v[1]; v[2]=_v[2]; v[3]=_v[3]; }
vector( float _x, float _y, float _z, float _w ) { v[0]=_x; v[1]=_y; v[2]=_z; v[3]=_w; }
float &operator[]( int i ) { return v[i]; }
const float &operator[]( int i ) const { return v[i]; }
// useful for glVertex3fv, glNormal3fv
const float* data_ptr() const { return v; }
// add vector s
vector operator+(const vector &b ) const { ASSERT_VECTOR( b ); return vector( v[0]+b[0], v[1]+b[1], v[2]+b[2], v[3]+b[3] ); }
// scale vector
vector operator*( float scale ) const { return vector( v[0]*scale, v[1]*scale, v[2]*scale, v[3]*scale ); }
private:
float v[4];
};
class matrix
{
public:
matrix() {}
vector& operator[]( int i ) { return cols[i]; }
const vector& operator[]( int i ) const { return cols[i]; }
// useful for glMultMatrixf, glLoadMatrixf
const float* data_ptr() const { return cols[0].data_ptr(); }
matrix& rotateY( float a );
matrix& rotateX( float a );
matrix& rotateZ( float a );
// return an identity matrix
static matrix identity();
// create a rotation matrix around y-axis
static matrix rotate_y( float a ) { return matrix::identity().rotateY( a ); }
// create a rotation matrix around y-axis
static matrix rotate_x( float a ) { return matrix::identity().rotateX( a ); }
// create a rotation matrix around y-axis
static matrix rotate_z( float a ) { return matrix::identity().rotateZ( a ); }
private:
vector cols[4];
};
inline
matrix
matrix::identity()
{
matrix m;
m[0] = vector( 1.0f, 0.0f, 0.0f, 0.0f );
m[1] = vector( 0.0f, 1.0f, 0.0f, 0.0f );
m[2] = vector( 0.0f, 0.0f, 1.0f, 0.0f );
m[3] = vector( 0.0f, 0.0f, 0.0f, 1.0f );
return m;
}
inline
matrix&
matrix::rotateY( float a )
{
cols[0][0] = cos(a); cols[0][1] = 0.0f; cols[0][2] = -sin(a); cols[0][3] = 0.0f;
cols[1][0] = 0.0f; cols[1][1] = 1.0f; cols[1][2] = 0.0f; cols[1][3] = 0.0f;
cols[2][0] = sin(a); cols[2][1] = 0.0f; cols[2][2] = cos(a); cols[2][3] = 0.0f;
cols[3][0] = cols[3][1] = cols[3][2] = 0.0f; cols[3][3] = 1.0f;
return *this;
}
inline
matrix&
matrix::rotateX( float a )
{
cols[0][0] = 1.0f; cols[0][1] = 0.0f; cols[0][2] = 0.0f; cols[0][3] = 0.0f;
cols[1][0] = 0.0f; cols[1][1] = cos(a); cols[1][2] = sin(a); cols[1][3] = 0.0f;
cols[2][0] = 0.0f; cols[2][1] = -sin(a); cols[2][2] = cos(a); cols[2][3] = 0.0f;
cols[3][0] = cols[3][1] = cols[3][2] = 0.0f; cols[3][3] = 1.0f;
return *this;
}
inline
matrix&
matrix::rotateZ( float a )
{
cols[0][0] = cos(a); cols[0][1] = sin(a); cols[0][2] = 0.0f; cols[0][3] = 0.0f;
cols[1][0] = -sin(a); cols[1][1] = cos(a); cols[1][2] = 0.0f; cols[1][3] = 0.0f;
cols[2][0] = 0.0f; cols[2][1] = 0.0f; cols[2][2] = 1.0f; cols[2][3] = 0.0f;
cols[3][0] = cols[3][1] = cols[3][2] = 0.0f; cols[3][3] = 1.0f;
return *this;
}
// matrix-matrix multiplication
inline
matrix operator*( const matrix &a, const matrix &b )
{
matrix result;
register unsigned int r;
register unsigned int c;
for( r = 0; r < 4; ++r )
{
for( c = 0; c < 4; ++c )
{
result[c][r] = a[0][r] * b[c][0] +
a[1][r] * b[c][1] +
a[2][r] * b[c][2] +
a[3][r] * b[c][3];
}
}
return result;
}
// matrix-vector multiplication
inline
vector operator*( const matrix &m, const vector &v )
{
vector ret;
ret[0] = v[0] * m[0][0] + v[1] * m[1][0] + v[2] * m[2][0] + v[3] * m[3][0];
ret[1] = v[0] * m[0][1] + v[1] * m[1][1] + v[2] * m[2][1] + v[3] * m[3][1];
ret[2] = v[0] * m[0][2] + v[1] * m[1][2] + v[2] * m[2][2] + v[3] * m[3][2];
ret[3] = v[0] * m[0][3] + v[1] * m[1][3] + v[2] * m[2][3] + v[3] * m[3][3];
return ret;
}
inline void DumpVectorf( FILE *file, const char *msg, const vector &p ) { fprintf( file, "%-15s: "FLOAT_FORMATOR" "FLOAT_FORMATOR" "FLOAT_FORMATOR" "FLOAT_FORMATOR"\n", msg, p[0], p[1], p[2], p[3] ); fflush( file ); }
inline void DumpMatrixf( FILE *file, const char *msg, const matrix &m )
{
fprintf( file, "%-15s:\n", msg );
fprintf( file, " row0: "FLOAT_FORMATOR" "FLOAT_FORMATOR" "FLOAT_FORMATOR" "FLOAT_FORMATOR"\n", m[0][0], m[1][0], m[2][0], m[3][0] );
fprintf( file, " row1: "FLOAT_FORMATOR" "FLOAT_FORMATOR" "FLOAT_FORMATOR" "FLOAT_FORMATOR"\n", m[0][1], m[1][1], m[2][1], m[3][1] );
fprintf( file, " row2: "FLOAT_FORMATOR" "FLOAT_FORMATOR" "FLOAT_FORMATOR" "FLOAT_FORMATOR"\n", m[0][2], m[1][2], m[2][2], m[3][2] );
fprintf( file, " row3: "FLOAT_FORMATOR" "FLOAT_FORMATOR" "FLOAT_FORMATOR" "FLOAT_FORMATOR"\n", m[0][3], m[1][3], m[2][3], m[3][3] );
fflush( file );
}
}
using namespace demo;
int main()
{
vector m_Rotation(1.0f, 0.5f, 0.0f, 0.0f);
vector m_Position(2.0f, 3.0f, 0.0f, 1.0f);
float speed = 3.0f;
// 1) first calculate the rotation matrix from m_Rotation
// !Mind the rotation order, here we use X, Y, Z, if you use a different one, reshuffle this
// We create 3 matrices, each one for a rotation around 1 of the axes,
// then we multiply them to find the final rotation matrix
matrix rot = matrix::rotate_x(m_Rotation[0]) * matrix::rotate_y(m_Rotation[1]) * matrix::rotate_z(m_Rotation[2]);
DumpMatrixf(stdout, "matrix rot", rot);
DumpVectorf(stdout, "position start", m_Position);
// 2) now calculate the translation vector by multiplying the local-space walking direction with that matrix to find the world-space walking direction
// I assume here your nomal walking direction in object space is {0, 0, -1} change it if you use a different one
vector translation = rot * vector(0.0f, 0.0f, -1.0f, 0.0f);
// multiply it with the "Speed"
translation = translation * speed;
DumpVectorf(stdout, "translation", translation);
// 3) add the final translation to the position
m_Position = m_Position + translation;
DumpVectorf(stdout, "position final", m_Position);
return 0;
}
The output of the example:
matrix rot :
row0: 0.877583 0.000000 0.479426 0.000000
row1: 0.403423 0.540302 -0.738460 0.000000
row2: -0.259035 0.841471 0.474160 0.000000
row3: 0.000000 0.000000 0.000000 1.000000
position start : 2.000000 3.000000 0.000000 1.000000
translation : -1.438277 2.215381 -1.422480 0.000000
position final : 0.561723 5.215381 -1.422480 1.000000