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); }
/** A callback function that will get called whenever the tracker * provides us with new data. This may be called repeatedly for each * record that we have missed if many records have been delivered * since the last call to the VRPN mainloop() function. */ static void VRPN_CALLBACK handle_tracker(void *name, vrpn_TRACKERCB t) { float fps = kuhl_getfps(&fps_state); if(fps_state.frame == 0) msg(INFO, "VRPN records per second: %.1f\n", fps); /* Some tracking systems return large values when a point gets * lost. If the tracked point seems to be lost, ignore this * update. */ float pos[3]; vec3f_set(pos, t.pos[0], t.pos[1], t.pos[2]); long microseconds = (t.msg_time.tv_sec* 1000000L) + t.msg_time.tv_usec; if(0) { printf("Current time %ld; VRPN record time: %ld\n", kuhl_microseconds(), microseconds); printf("Received position from vrpn: "); vec3f_print(pos); } if(vec3f_norm(pos) > 100) return; // Store the data in our map so that someone can use it later. std::string s = (char*)name; nameToCallbackData[s] = t; smooth(nameToCallbackData[s]); }
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; } } } }
/** * 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 }; }
/** Update the vertex positions and the velocity stored in the * particles array. */ void update() { kuhl_geometry *g = modelgeom; for(unsigned int i=0; i<kuhl_geometry_count(modelgeom); i++) { int numFloats = 0; GLfloat *pos = kuhl_geometry_attrib_get(g, "in_Position", &numFloats); for(unsigned int j=0; j<g->vertex_count; j++) { /* If the first point isn't moving, don't update anything */ if(vec3f_norm(particles[i][j].velocity) == 0) return; /* Gravity is pushing particles down -Y, but we are * operating in object coordinates. If GeomTransform * (i.e., g->matrix) is used to rotate the model, then * gravity might not push the particles down in world * coordinates. */ float accel[3] = { 0, -1, 0}; float timestep = 0.1f; // change this to change speed of explosion for(int k=0; k<3; k++) { pos[j*3+k] += timestep * (particles[i][j].velocity[k] + timestep * accel[k]/2); particles[i][j].velocity[k] += timestep * accel[k]; } #if 1 /* Bounce the particles off the xz-plane. */ if(pos[j*3+1] < 0) { /* How much velocity is lost when a bounce occurs? */ float velocityLossFactor = .4; /* If particle fell through floor, negate its position */ pos[j*3+1] *= -velocityLossFactor; /* Negative the Y velocity */ particles[i][j].velocity[1] *= -1; /* Scale velocity in all directions */ vec3f_scalarMult(particles[i][j].velocity, velocityLossFactor); // lose energy on bounce } #endif } g = g->next; } }
static vector3f compute_sphere_vector(unsigned int mouse_x, unsigned int mouse_y) { float x = 0.0f, y = 0.0f, vec_length = 0.0f; vector3f result; // приводим координаты мыши к интервалу [-1,1] x = ((float) mouse_x / (float) window_width) * 2.0f - 1.0f; y = ((float) mouse_y / (float) window_height) * 2.0f - 1.0f; result = vec3f(x, -y, 0.0f); // v_len = x*x + y*y vec_length = x*x + y*y; // R*R = Z*Z + (X*X + Y*Y) <=> Z*Z = R*R - (X*X + Y*Y) => (R = 1, (X*X + Y*Y) = v_len) => Z = sqrt(1 - v_len) if(vec_length > 1.0f) { result = vec3f_norm(result); } else { result.z = math_sqrtf(1.0f - vec_length); } return result; }