sin_y = sinf(yaw);
cos_y = cosf(yaw);
sin_p = sinf(pitch);
cos_p = cosf(pitch);
sin_r = sinf(roll);
cos_r = cosf(roll);
 
m11 = cos_y * cos_p;
m12 = (cos_y * sin_p * sin_r) - (sin_y * cos_r);
m13 = (cos_y * sin_p * cos_r) + (sin_y * sin_r);
 
m21 = sin_y * cos_p;
m22 = (sin_y * sin_p * sin_r) + (cos_y * cos_r);
m23 = (sin_y * sin_p * cos_r) - (cos_y * sin_r);
 
m31 = -sin_p;
m32 = cos_p * sin_r;
m33 = cos_p * cos_r;
 

VGnns.x = Pos[0].World_x - Pos[1].World_x;
VGnns.y = Pos[0].World_y - Pos[1].World_y;
VGnns.z = Pos[0].World_z - Pos[1].World_z;
 
Vector3_normalize(&VGnns,&VGnns);
 
cos_r = cosf(vehicle_roll)
sin_r = sinf(vehicle_roll)
 
cos_p = cosf(vehicle_pitch)
sin_p = sinf(vehicle_pitch)
 
m11 = ???
m12 = VGnns.x
m13 = ???
 
m21 = ???
m22 = VGnns.y
m23 = ???
 
m31 = -sin_p
m32 = sin_r * cos_p 
m33 = cos_p * cos_r