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 }; }