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