void predg3f_pquv(struct vec3f_s *p, struct vec3f_s *q, struct vec3f_s *u, struct vec3f_s *v, const struct predg3f_s *g)
{
vec3f_cross(p, &g->k, &g->l);
vec3f_sub(q, &g->a, &g->b);
vec3f_sub(u, &g->k, &g->l);
vec3f_cross(v, &g->a, &g->b);
}
void do_movement() {
GLfloat camera_speed;
camera_speed = 5.0f * delta_time;
if(keys[GLFW_KEY_W]) {
vec3f_muls(temp_vec3f, camera_front, camera_speed);
vec3f_add(camera_pos, camera_pos, temp_vec3f);
}
if(keys[GLFW_KEY_S]) {
vec3f_muls(temp_vec3f, camera_front, camera_speed);
vec3f_sub(camera_pos, camera_pos, temp_vec3f);
}
if(keys[GLFW_KEY_A]) {
vec3f_cross(temp_vec3f, camera_front, camera_up);
vec3f_normalize(temp_vec3f, temp_vec3f);
vec3f_muls(temp_vec3f, temp_vec3f, camera_speed);
vec3f_sub(camera_pos, camera_pos, temp_vec3f);
}
if(keys[GLFW_KEY_D]) {
vec3f_cross(temp_vec3f, camera_front, camera_up);
vec3f_normalize(temp_vec3f, temp_vec3f);
vec3f_muls(temp_vec3f, temp_vec3f, camera_speed);
vec3f_add(camera_pos, camera_pos, temp_vec3f);
}
}
/**
* Apply an infinitesimal rotation w*dt to a direction cosine matrix A.
* An orthonormalization step is added to prevent rounding errors.
*
* Without the normalization, this would be a simple matrix multiplication:
*
* return mat3_matmul( A, (mat3f) {
* 1, -w.z, w.y,
* w.z, 1, -w.x,
* -w.y, w.x, 1
* };
*/
static mat3f dcm_integrate(mat3f A, vec3f w, float dt)
{
w = vec3f_scale(w, dt);
// Calculate the new x and y axes. z is calculated later.
//
vec3f x = vec3f_matmul(A, (vec3f){ 1, w.z, -w.y } );
vec3f y = vec3f_matmul(A, (vec3f){ -w.z, 1, w.x } );
// Orthonormalization
//
// First we compute the dot product of the x and y rows of the matrix, which
// is supposed to be zero, so the result is a measure of how much the X and Y
// rows are rotating toward each other
//
float error = vec3f_dot(x, y);
// We apportion half of the error each to the x and y rows, and approximately
// rotate the X and Y rows in the opposite direction by cross coupling:
//
vec3f xo, yo, zo;
xo = vec3f_sub(x, vec3f_scale(y, 0.5 * error));
yo = vec3f_sub(y, vec3f_scale(x, 0.5 * error));
// Scale them to unit length and take a cross product for the Z axis
//
xo = vec3f_norm(xo);
yo = vec3f_norm(yo);
zo = vec3f_cross(xo, yo);
return (mat3f) {
xo.x, yo.x, zo.x,
xo.y, yo.y, zo.y,
xo.z, yo.z, zo.z
};
}
