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 predg3f_from_predh3f(struct predg3f_s *g, const struct predh3f_s *h)
{
struct vec3f_s r, nr;
calc_r(&r, &h->p.n);
vec3f_cross(&nr, &h->p.n, &r);
g->k = nr;
vec3f_cross(&g->l, &h->p.n, &nr);
g->a = h->b;
vec3f_neg(&g->b, &h->b);
g->c = 2 * h->p.d * vec3f_sqlen(&nr);
}
extern void dcm_update(vec3f gyro, vec3f acc, float dt)
{
dcm.offset_p = vec3f_zero;
// Apply accelerometer correction
//
vec3f down = vec3f_matmul(dcm.matrix, vec3f_norm(acc));
vec3f error = vec3f_cross(down, dcm.down_ref);
dcm.debug = error;
dcm.offset_p = vec3f_add(dcm.offset_p, vec3f_scale(error, dcm.acc_kp));
dcm.offset_i = vec3f_add(dcm.offset_i, vec3f_scale(error, dcm.acc_ki));
// todo: magnetometer correction, once we have one
// Calculate drift-corrected roll, pitch and yaw angles
//
dcm.omega = gyro;
dcm.omega = vec3f_add(dcm.omega, dcm.offset_p);
dcm.omega = vec3f_add(dcm.omega, dcm.offset_i);
// Apply rotation to the direction cosine matrix
//
dcm.matrix = dcm_integrate(dcm.matrix, dcm.omega, dt);
dcm.euler = mat3f_to_euler(dcm.matrix);
}
void update_object_orientation(void)
{
static int mx = 0, my = 0, last_mouse_x = 0, last_mouse_y = 0;
static int arcball = 0;
IF_FAILED(init);
if(!disable_mouse) {
if(!arcball && input_get_mousebtn_state(MOUSE_LEFTBTN)) {
arcball = 1;
input_get_mouse_position(&mx, &my);
last_mouse_x = mx;
last_mouse_y = my;
} else if(arcball && !input_get_mousebtn_state(MOUSE_LEFTBTN)) {
arcball = 0;
return;
}
if(arcball) {
input_get_mouse_position(&mx, &my);
if(mx < 0)
mx = 0;
else if(mx > window_width)
mx = window_width;
if(my < 0)
my = 0;
else if(my > window_height)
my = window_height;
if(last_mouse_x != mx || last_mouse_y != my) {
// получаем вектора вращения виртуальной сферы
vector3f v1 = compute_sphere_vector(last_mouse_x, last_mouse_y);
vector3f v2 = compute_sphere_vector(mx, my);
// угол вращения
rot_angle = RAD_TO_DEG(math_acosf(math_min(1.0f, vec3f_dot(v1, v2))));
matrix3f rotmat3, model3, rotmodel3;
mat4_submat(rotmat3, 3, 3, rotmat);
mat4_submat(model3, 3, 3, modelmat);
mat3_mult2(rotmodel3, rotmat3, model3);
// получаем ось вращения (переводим её в систему координат объекта)
rot_axis = mat3_mult_vec3(rotmodel3, vec3f_norm(vec3f_cross(v1, v2)));
// домножаем матрицу вращения
mat4_rotate_axis_mult(rotmat, rot_angle, rot_axis);
last_mouse_x = mx;
last_mouse_y = my;
}
}
}
}
void camera_handle_rotation(camera* cam)
{
vec3f vertical_axis = vec3f_init(0.0f, 1.0f, 0.0f);
// Rotate the view vector by the horizontal angle around the vertical axis
vec3f view = vec3f_init(1.0f, 0.0f, 0.0f);
view = vec3f_rotate_around_axis(view, vertical_axis, cam->angle_x);
view = vec3f_normalize(view);
// Rotate the view vector by the vertical angle around the horizontal axis
vec3f horizontal_axis = vec3f_cross(vertical_axis, view);
horizontal_axis = vec3f_normalize(horizontal_axis);
view = vec3f_rotate_around_axis(view, horizontal_axis, cam->angle_y);
cam->target = vec3f_normalize(view);
cam->up = vec3f_cross(cam->target, horizontal_axis);
cam->up = vec3f_normalize(cam->up);
}
static void calc_ellipsoidal_w(struct vec4f_s *w, const struct vec3f_s *p, const struct vec3f_s *q, const struct vec3f_s *u, const struct vec3f_s *v, double a, double b)
{
/*
* w = 1 - a b p^ u^ q^ v^ - a p^ q^ - b u^ v^
* = 1 + a b (p^xu^) (q^xv^) - a p^ q^ - b u^ v^
*/
struct vec3f_s ph, qh, uh, vh, phxuh, qhxvh;
struct pin3f_s wp, ws, phqh, uhvh, phuhqhvh;
vec3f_unit(&ph, p);
vec3f_unit(&qh, q);
vec3f_unit(&uh, u);
vec3f_unit(&vh, v);
vec3f_cross(&phxuh, &ph, &uh);
vec3f_cross(&qhxvh, &qh, &vh);
vec3f_cl(&phqh, &ph, &qh);
vec3f_cl(&uhvh, &uh, &vh);
vec3f_cl(&phuhqhvh, &phxuh, &qhxvh);
pin3f_mad4(&wp, &PIN3F_ONE, 1.0, &phuhqhvh, a * b, &phqh, -a, &uhvh, -b);
pin3f_mul(&ws, &wp, 0.5 / sqrt(pclamp(wp.p0)));
vec4f_from_pin3f(w, &ws);
}
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);
}
}
void camera_handle_movement(camera* cam, Uint32 delta_time)
{
//float y = cam->pos.v[1];
float dt = delta_time / 1000.0f;
if (cam->moving_forward)
{
cam->pos = vec3f_add(cam->pos, vec3f_mult(cam->target, cam->step * dt));
}
if (cam->moving_back)
{
cam->pos = vec3f_subtract(cam->pos, vec3f_mult(cam->target, cam->step * dt));
}
if (cam->moving_left)
{
vec3f left = vec3f_normalize(vec3f_cross(cam->target, cam->up));
cam->pos = vec3f_add(cam->pos, vec3f_mult(left, cam->step * dt));
}
if (cam->moving_right)
{
vec3f right = vec3f_normalize(vec3f_cross(cam->up, cam->target));
cam->pos = vec3f_add(cam->pos, vec3f_mult(right, cam->step * dt));
}
//cam->pos.v[1] = y;
}
/**
* 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
};
}
