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