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); }
// computer the vector caused by rotation q on vector v vec3f_t vec3f_rotate(vec3f_t v, quat4f_t q) { float w; vec3f_t r, wv, rv, tmp1, tmp2, delta, result; w = q.w; r.x = q.x; r.y = q.y; r.z = q.z; // v + 2r X (r X v + wv) // computer wv wv = vec3f_mul_scalar(w, v); // computer r X v rv = vec3f_mul_cross(r, v); // computer r X v + wv tmp1 = vec3f_add(rv, wv); // computer 2r tmp2 = vec3f_mul_scalar(2, r); // computer delta delta = vec3f_mul_cross(tmp2, tmp1); // computer final result result = vec3f_add(v, delta); return result; }
void ParticleEngine_Run(ParticleEngine *pengine) { int i; Particle *particle; for (i = 0; i < pengine->size; i++) { particle = &pengine->particles[i]; /* Respawn particle if dead */ if (particle->life-- <= 0) { Particle_Spawn(pengine, particle); } /* Add force to position */ vec3f_add(particle->position, particle->force, particle->position); /* Don't go through floor */ if (particle->position[1] < pengine->floor) { particle->position[1] = pengine->floor; } /* Render particle */ glColor3f(1,1,0); glDisable(GL_CULL_FACE); glEnable(GL_BLEND); glBlendFunc(GL_DST_COLOR, GL_SRC_COLOR); glBindTexture(GL_TEXTURE_2D, pengine->texture); glBegin(GL_QUADS); glTexCoord2d(0, 0); glVertex3f(particle->position[0] - 0.1, particle->position[1], particle->position[2] - 0.1); glTexCoord2d(0, 1); glVertex3f(particle->position[0] - 0.1, particle->position[1], particle->position[2] + 0.1); glTexCoord2d(1, 1); glVertex3f(particle->position[0] + 0.1, particle->position[1], particle->position[2] + 0.1); glTexCoord2d(1, 0); glVertex3f(particle->position[0] + 0.1, particle->position[1], particle->position[2] - 0.1); glEnd(); glDisable(GL_BLEND); glEnable(GL_CULL_FACE); /* Add gravity to force */ vec3f_add(particle->force, pengine->gravity, particle->force); } }
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; }
int main(void) { if(!glfwInit()) { fprintf(stderr, "Could not load GLFW, aborting.\n"); return(EXIT_FAILURE); } int WIDTH, HEIGHT; WIDTH = 800; HEIGHT = 600; GLFWwindow *window; window = glfwCreateWindow(WIDTH,HEIGHT,"05 camera.", NULL, NULL); if(!window) { fprintf(stderr, "Could not create main window, aborting.\n"); return(EXIT_FAILURE); } glfwMakeContextCurrent(window); glfwSetKeyCallback(window, key_callback); glfwSetCursorPosCallback(window, mouse_callback); glfwSetScrollCallback(window, scroll_callback); glewExperimental = GL_TRUE; if(glewInit() != GLEW_OK) { fprintf(stderr, "Could not initialize GLEW, aborting.\n"); return(EXIT_FAILURE); } glEnable(GL_DEPTH_TEST); glfwSetInputMode(window, GLFW_CURSOR, GLFW_CURSOR_DISABLED); GLuint VAO, VBO, lightVAO; glGenVertexArrays(1, &VAO); glGenBuffers(1, &VBO); glBindVertexArray(VAO); glBindBuffer(GL_ARRAY_BUFFER, VBO); glBufferData(GL_ARRAY_BUFFER, sizeof(vertices), vertices, GL_STATIC_DRAW); glVertexAttribPointer(0, 3, GL_FLOAT, GL_FALSE, stride, (GLvoid*)0); glEnableVertexAttribArray(0); glVertexAttribPointer(1, 3, GL_FLOAT, GL_FALSE, stride, (GLvoid*)(3 * sizeof(GLfloat))); glEnableVertexAttribArray(1); //glVertexAttribPointer(1, 2, GL_FLOAT, GL_FALSE, stride, // (GLvoid*)(3*sizeof(GLfloat))); //glEnableVertexAttribArray(1); glBindVertexArray(0); /* LightVAO definition. */ glGenVertexArrays(1, &lightVAO); glBindVertexArray(lightVAO); glBindBuffer(GL_ARRAY_BUFFER, VBO); // Same vertex data as cube. glVertexAttribPointer(0, 3, GL_FLOAT, GL_FALSE, stride, (GLvoid*)0); glEnableVertexAttribArray(0); glBindVertexArray(0); char* vertex_source = read_shader("shaders/07.vert"); char* fragment_source = read_shader("shaders/07.frag"); char* lamp_fragment_source = read_shader("shaders/07_lamp.frag"); GLuint shader_program, lamp_program; shader_program = create_shader_program(vertex_source, fragment_source); lamp_program = create_shader_program(vertex_source, lamp_fragment_source); free(vertex_source); free(fragment_source); free(lamp_fragment_source); //GLuint texture; //glActiveTexture(GL_TEXTURE0); //glGenTextures(1, &texture); //glBindTexture(GL_TEXTURE_2D, texture); //load_texture("textures/02_container.jpg"); //glUniform1i(glGetUniformLocation(shader_program, "texture_sampler"), 0); //glBindTexture(GL_TEXTURE_2D, 0); Mat4f *projection, *model, *view, *temp, *temp2; mat4f_allocate(&projection); mat4f_allocate(&model); mat4f_allocate(&view); mat4f_allocate(&temp); mat4f_allocate(&temp2); mat4f_translate(view, 0.0f, 0.0f, -3.0f); //mat4f_rotate_x(model, -M_PI/4); mat4f_rotate_x(model, 0.0f); Vec3f *light_position; vec3f_allocate(&light_position); vec3f_allocate(&camera_pos); vec3f_allocate(&camera_target); vec3f_allocate(&camera_up); vec3f_allocate(&camera_front); vec3f_allocate(&temp_vec3f); vec3f_set(camera_target, 0.0f, 0.0f, 0.0f); vec3f_set(camera_up, 0.0f, 1.0f, 0.0f); vec3f_set(camera_front, 0.0f, 0.0f, -1.0f); vec3f_set(camera_pos, 0.0f, 0.0f, 3.0f); vec3f_set(light_position, 1.2f, 1.0f, 2.0f); /* shader locations */ GLuint model_location, projection_location, view_location, light_position_location, view_position_location; glUseProgram(shader_program); model_location = glGetUniformLocation(shader_program, "model"); projection_location = glGetUniformLocation(shader_program, "perspective"); view_location = glGetUniformLocation(shader_program, "view"); light_position_location = glGetUniformLocation(shader_program, "light_position"); view_position_location = glGetUniformLocation(shader_program, "view_position"); GLuint object_color_location, light_color_location; object_color_location = glGetUniformLocation(shader_program, "object_color"); light_color_location = glGetUniformLocation(shader_program, "light_color"); glUniform3f(object_color_location, 1.0f, 0.5f, 0.31f); glUniform3f(light_color_location, 1.0f, 1.0f, 1.0); glUniform3f(light_position_location, light_position->data[0], light_position->data[1], light_position->data[2]); glUseProgram(0); glUseProgram(lamp_program); GLuint lamp_model_location, lamp_projection_location, lamp_view_location; lamp_model_location = glGetUniformLocation(lamp_program, "model"); lamp_projection_location = glGetUniformLocation(lamp_program, "perspective"); lamp_view_location = glGetUniformLocation(lamp_program, "view"); glUseProgram(0); glClearColor(0.0f, 0.0f, 0.0f, 1.0f); current_frame = 0.0f; last_frame = 0.0f; last_x = WIDTH / 2.0f; last_y = HEIGHT / 2.0f; fov = M_PI/4; yaw = -M_PI/2; pitch = 0.0f; first_mouse = true; while(!glfwWindowShouldClose(window)) { glfwPollEvents(); current_frame = glfwGetTime(); delta_time = current_frame - last_frame; last_frame = current_frame; do_movement(); glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); glUseProgram(shader_program); // glActiveTexture(GL_TEXTURE0); // glBindTexture(GL_TEXTURE_2D, texture); // cam_x = sinf(time) * radius; // cam_z = cosf(time) * radius; vec3f_add(camera_target, camera_pos, camera_front); mat4f_look_at(view, camera_pos, camera_target, camera_up); mat4f_perspective(projection, fov, (float)WIDTH/(float)HEIGHT, 0.1f, 100.0f); //mat4f_rotate_x(model, sinf(time) * M_PI); glUniformMatrix4fv(model_location, 1, GL_TRUE, mat4f_pointer(model)); glUniformMatrix4fv(view_location, 1, GL_TRUE, mat4f_pointer(view)); glUniformMatrix4fv(projection_location, 1, GL_TRUE, mat4f_pointer(projection)); glUniform3f(view_position_location, camera_pos->data[0], camera_pos->data[1], camera_pos->data[2]); glBindVertexArray(VAO); glDrawArrays(GL_TRIANGLES, 0, 36); // glBindTexture(GL_TEXTURE_2D, 0); glBindVertexArray(0); glUseProgram(lamp_program); //glUseProgram(shader_program); mat4f_scale(temp, 0.2f, 0.2f, 0.2f); mat4f_mul(temp, temp, model); mat4f_translate_vec3f(temp2, light_position); mat4f_mul(temp2, temp2, temp); //mat4f_print(temp); glUniformMatrix4fv(lamp_model_location, 1, GL_TRUE, mat4f_pointer(temp2)); glUniformMatrix4fv(lamp_view_location, 1, GL_TRUE, mat4f_pointer(view)); glUniformMatrix4fv(lamp_projection_location, 1, GL_TRUE, mat4f_pointer(projection)); glBindVertexArray(lightVAO); glDrawArrays(GL_TRIANGLES, 0, 36); glBindVertexArray(0); glfwSwapBuffers(window); } glfwTerminate(); return(EXIT_SUCCESS); }
void gravity_filter_run(gravity_filter_context_t* cx, imu_sensor_data_t* sensor) { quat4f_t q; vec3f_t acc_norm, angular_rate, axis_angles, e, gravity_new; float acc_magnitude, gravity_magnitude, err_magnitude, angle_magnitude, gain; acc_magnitude = sqrt(sensor->acc[0] * sensor->acc[0] + sensor->acc[1] * sensor->acc[1] + sensor->acc[2] * sensor->acc[2]); // normalize sensed gravity acc_norm.x = (double)1000.0 * sensor->acc[0] / acc_magnitude; acc_norm.y = (double)1000.0 * sensor->acc[1] / acc_magnitude; acc_norm.z = (double)1000.0 * sensor->acc[2] / acc_magnitude; if (cx->flags == 0) { cx->gravity.x = acc_norm.x; cx->gravity.y = acc_norm.y; cx->gravity.z = acc_norm.z; cx->flags = 1; return; } angular_rate.x = sensor->gyro[0] - cx->drift.x; angular_rate.y = sensor->gyro[1] - cx->drift.y; angular_rate.z = sensor->gyro[2] - cx->drift.z; // computer axis angles measured by gyroscopic axis_angles.x = -((double)angular_rate.x * dt * M_PI / 180.0); axis_angles.y = -((double)angular_rate.y * dt * M_PI / 180.0); axis_angles.z = -((double)angular_rate.z * dt * M_PI / 180.0); // form a quaternion // hereby, we assume the angles are tiny q.w = 1; q.x = axis_angles.x / 2; q.y = axis_angles.y / 2; q.z = axis_angles.z / 2; // update gravity vector by rotation gravity_new = vec3f_rotate(cx->gravity, q); // gyroscopic autocalibration if (cx->cycle < ZERO_MOTION_CYCLES) { if (cx->cycle == 0) { cx->acc_min.x = acc_norm.x; cx->acc_min.y = sensor->acc[1]; cx->acc_min.z = sensor->acc[2]; cx->acc_max.x = sensor->acc[0]; cx->acc_max.y = sensor->acc[1]; cx->acc_max.z = sensor->acc[2]; cx->gyro_min.x = sensor->gyro[0]; cx->gyro_min.y = sensor->gyro[1]; cx->gyro_min.z = sensor->gyro[2]; cx->gyro_max.x = sensor->gyro[0]; cx->gyro_max.y = sensor->gyro[1]; cx->gyro_max.z = sensor->gyro[2]; cx->drift_sum.x = 0; cx->drift_sum.y = 0; cx->drift_sum.z = 0; } else { if (sensor->acc[0] < cx->acc_min.x) cx->acc_min.x = sensor->acc[0]; if (sensor->acc[1] < cx->acc_min.y) cx->acc_min.y = sensor->acc[1]; if (sensor->acc[2] < cx->acc_min.z) cx->acc_min.z = sensor->acc[2]; if (sensor->acc[0] > cx->acc_max.x) cx->acc_max.x = sensor->acc[0]; if (sensor->acc[1] > cx->acc_max.y) cx->acc_max.y = sensor->acc[1]; if (sensor->acc[2] > cx->acc_max.z) cx->acc_max.z = sensor->acc[2]; if (sensor->gyro[0] < cx->gyro_min.x) cx->gyro_min.x = sensor->gyro[0]; if (sensor->gyro[1] < cx->gyro_min.y) cx->gyro_min.y = sensor->gyro[1]; if (sensor->gyro[2] < cx->gyro_min.z) cx->gyro_min.z = sensor->gyro[2]; if (sensor->gyro[0] > cx->gyro_max.x) cx->gyro_max.x = sensor->gyro[0]; if (sensor->gyro[1] > cx->gyro_max.y) cx->gyro_max.y = sensor->gyro[1]; if (sensor->gyro[2] > cx->gyro_max.z) cx->gyro_max.z = sensor->gyro[2]; } cx->drift_sum.x += sensor->gyro[0]; cx->drift_sum.y += sensor->gyro[1]; cx->drift_sum.z += sensor->gyro[2]; cx->cycle++; } if (cx->cycle >= ZERO_MOTION_CYCLES) { if ((abs(cx->acc_max.x - cx->acc_min.x) < ZERO_MOTION_THRESHOLD_ACC) && (abs(cx->acc_max.y - cx->acc_min.y) < ZERO_MOTION_THRESHOLD_ACC) && (abs(cx->acc_max.z - cx->acc_min.z) < ZERO_MOTION_THRESHOLD_ACC)&& (abs(cx->gyro_max.x - cx->gyro_min.x) < ZERO_MOTION_THRESHOLD_GYRO) && (abs(cx->gyro_max.y - cx->gyro_min.y) < ZERO_MOTION_THRESHOLD_GYRO) && (abs(cx->gyro_max.z - cx->gyro_min.z) < ZERO_MOTION_THRESHOLD_GYRO)) { cx->drift.x += (double)K_GYRO_DRIFT * (cx->drift_sum.x/ZERO_MOTION_CYCLES - cx->drift.x); cx->drift.y += (double)K_GYRO_DRIFT * (cx->drift_sum.y/ZERO_MOTION_CYCLES - cx->drift.y); cx->drift.z += (double)K_GYRO_DRIFT * (cx->drift_sum.z/ZERO_MOTION_CYCLES - cx->drift.z); } cx->cycle = 0; } // computer prediction error according to accelerometer's measurements e.x = acc_norm.x - gravity_new.x; e.y = acc_norm.y - gravity_new.y; e.z = acc_norm.z - gravity_new.z; if (cx->cycle == 0) printf("%f ", angular_rate.x); err_magnitude = vec3f_magnitude(e); angle_magnitude = vec3f_magnitude(angular_rate); // todo // should revise the gain // the principle is: // * when accelerometer reading is jumping around, filter it out // * otherwise, we should follow it quickly if (err_magnitude < 10) { // gyroscopic predicted result matches accelerometer readings // we are lucky! gain = 0.1; } else { // gyroscopic does not agree with accelerometer if ((angle_magnitude < 100) && (abs(acc_magnitude - 1000) > 40)) { // angular rate is low & acceleration is high // most likely there is high linear acceleration // so we don't trust accelerometer now gain = 0.0001; } else { // looks like there is rapid motion. // both gyroscopic and accelerometer are noisy in this case // follow accelerometer to prevent big lag gain = 0.5; } } // computer error estimation e = vec3f_mul_scalar(gain, e); // correct predicted gravity cx->gravity = vec3f_add(gravity_new, e); // renormalise gravity_magnitude = sqrt(cx->gravity.x * cx->gravity.x + cx->gravity.y * cx->gravity.y + cx->gravity.z * cx->gravity.z); cx->gravity.x = (double)1000.0 * cx->gravity.x / gravity_magnitude; cx->gravity.y = (double)1000.0 * cx->gravity.y / gravity_magnitude; cx->gravity.z = (double)1000.0 * cx->gravity.z / gravity_magnitude; }