static void kalman_correct(kalman_t *kf, float pos, float speed)
{
/* update H matrix: */
kf->H.me[1][1] = 1.0f * (kf->use_speed && speed != 0.0f);
kf->z.ve[0] = pos;
kf->z.ve[1] = speed;
/* K = P * HT * inv(H * P * HT + R) */
mat_mul(&kf->T0, &kf->H, &kf->P); // T0 = H * P
mmtr_mul(&kf->T1, &kf->T0, &kf->H); // T1 = T0 * HT
mat_add(&kf->T0, &kf->T1, &kf->R); // T0 = T1 + R
mat_inv(&kf->T1, &kf->T0); // T1 = inv(T0)
mmtr_mul(&kf->T0, &kf->P, &kf->H); // T0 = P * HT
mat_mul(&kf->K, &kf->T0, &kf->T1); // K = T0 * T1
/* x = x + K * (z - H * x) */
mat_vec_mul(&kf->t0, &kf->H, &kf->x); // t0 = H * x
vec_sub(&kf->t1, &kf->z, &kf->t0); // t1 = z - t0
mat_vec_mul(&kf->t0, &kf->K, &kf->t1); // t0 = K * t1
vec_add(&kf->x, &kf->x, &kf->t0); // x = x + t0
/* P = (I - K * H) * P */
mat_mul(&kf->T0, &kf->K, &kf->H); // T0 = K * H
mat_sub(&kf->T1, &kf->I, &kf->T0); // T1 = I - T0
mat_mul(&kf->T0, &kf->T1, &kf->P); // T0 = T1 * P
mat_copy(&kf->P, &kf->T0); // P = T0
}
static void kalman_predict(kalman_t *kf, float a)
{
/* x = A * x + B * u */
kf->u.ve[0] = a;
mat_vec_mul(&kf->t0, &kf->A, &kf->x); /* t0 = A * x */
mat_vec_mul(&kf->t1, &kf->B, &kf->u); /* t1 = B * u */
vec_add(&kf->x, &kf->t0, &kf->t1); /* x = t0 + t1 */
/* P = A * P * AT + Q */
mat_mul(&kf->T0, &kf->A, &kf->P); /* T0 = A * P */
mmtr_mul(&kf->T1, &kf->T0, &kf->A); /* T1 = T0 * AT */
mat_add(&kf->P, &kf->T1, &kf->Q); /* P = T1 * Q */
}
/**
* Main function
*
* @param argc
* @param argv
*/
int main(int argc, char **argv)
{
// create PAPI routines
PapiCounterList papi_routines;
papi_routines.AddRoutine("matvec");
// allocate arrays
float a[ROWS][COLS];
float b[COLS];
float c[ROWS] = {0.0f};
printf("Rows: %d\n", ROWS);
printf("Cols: %d\n", COLS);
printf("Runs: %d\n", RUNS);
// initialize matrix & vector
mat_init(ROWS, COLS, 1.0, a);
vec_init(COLS, 3.0, b);
// do the measurement
papi_routines["matvec"].Start();
for (unsigned i = 0; i < RUNS; i++)
mat_vec_mul(ROWS, COLS, a, b, c);
papi_routines["matvec"].Stop();
// print results
printf("Control sum: %f\n", vec_sum(ROWS, c));
printf("\n");
papi_routines.PrintScreen();
return EXIT_SUCCESS;
}
/**
* Main function
*
* @param argc
* @param argv
*/
int main(int argc, char **argv)
{
// allocate matrix
float *A = (float*)_mm_malloc(ROWS * COLS * sizeof(float), 64);
if(A == NULL)
{
fprintf(stderr, "_mm_malloc error !\n");
return EXIT_FAILURE;
}
// allocate vectors
float *B = (float*)_mm_malloc(COLS * sizeof(float), 64);
if(A == NULL)
{
fprintf(stderr, "_mm_malloc error !\n");
_mm_free(A);
return EXIT_FAILURE;
}
float *C = (float*)_mm_malloc(ROWS * sizeof(float), 64);
if(A == NULL)
{
fprintf(stderr, "_mm_malloc error !\n");
_mm_free(A);
_mm_free(B);
return EXIT_FAILURE;
}
printf("Rows: %d\n", ROWS);
printf("Cols: %d\n", COLS);
printf("Runs: %d\n", RUNS);
// initialize matrix & vector
const double tstart = omp_get_wtime();
mat_init(ROWS, COLS, 1.0, A);
vec_init(COLS, 3.0, B);
const double tinit = omp_get_wtime();
// do the measurement
for(unsigned i = 0; i < RUNS; i++)
{
mat_vec_mul(ROWS, COLS, A, B, C);
}
const double tcalc = omp_get_wtime();
// print results
printf("Control sum: %f\n", vec_sum(ROWS, C));
printf("Time Initialization: %0.3f\n", tinit - tstart);
printf("Time Calculation: %0.3f\n", tcalc - tinit);
printf("Time Total: %0.3f\n", tcalc - tstart);
// free memory
_mm_free(A);
_mm_free(B);
_mm_free(C);
return EXIT_SUCCESS;
}
void render_point_lamp(struct lamp *lamp, mat4 clip_from_view, mat4 view_from_world, mat4 lamp_transform)
{
static int prog = 0;
static int uni_viewport;
static int uni_view_from_clip;
static int uni_lamp_position;
static int uni_lamp_color;
static int uni_lamp_distance;
static int uni_use_sphere;
mat4 view_from_clip;
vec2 viewport;
vec3 lamp_position;
vec3 lamp_color;
if (!prog) {
prog = compile_shader(quad_vert_src, point_frag_src);
uni_viewport = glGetUniformLocation(prog, "viewport");
uni_view_from_clip = glGetUniformLocation(prog, "view_from_clip");
uni_lamp_position = glGetUniformLocation(prog, "lamp_position");
uni_lamp_color = glGetUniformLocation(prog, "lamp_color");
uni_lamp_distance = glGetUniformLocation(prog, "lamp_distance");
uni_use_sphere = glGetUniformLocation(prog, "use_sphere");
}
mat_invert(view_from_clip, clip_from_view);
viewport[0] = fbo_w;
viewport[1] = fbo_h;
mat_vec_mul(lamp_position, view_from_world, lamp_transform + 12);
vec_scale(lamp_color, lamp->color, lamp->energy);
glUseProgram(prog);
glUniform2fv(uni_viewport, 1, viewport);
glUniformMatrix4fv(uni_view_from_clip, 1, 0, view_from_clip);
glUniform3fv(uni_lamp_position, 1, lamp_position);
glUniform3fv(uni_lamp_color, 1, lamp_color);
glUniform1f(uni_lamp_distance, lamp->distance);
glUniform1i(uni_use_sphere, lamp->use_sphere);
draw_fullscreen_quad();
}
void draw_skel(mat4 *abs_pose_matrix, int *parent, int count)
{
vec3 x = { 0.1, 0, 0 };
int i;
for (i = 0; i < count; i++) {
float *a = abs_pose_matrix[i];
if (parent[i] >= 0) {
float *b = abs_pose_matrix[parent[i]];
draw_line(a[12], a[13], a[14], b[12], b[13], b[14]);
} else {
draw_line(a[12], a[13], a[14], 0, 0, 0);
}
if (!haschildren(parent, count, i)) {
vec3 b;
mat_vec_mul(b, abs_pose_matrix[i], x);
draw_line(a[12], a[13], a[14], b[0], b[1], b[2]);
}
}
}
void
model_iqm_animate (struct iqm_model *model,
struct iqm_animations *animations,
int a,
int f,
float t)
{
int i;
a %= animations->num_anims;
f %= animations->anims[a]->count;
printf ("anim %d frame %d\n", a, f);
int *table = _match_bones (model, animations->anims[a]);
_get_delta (model, animations->anims[a], table, f);
if (model->dnorm == NULL) {
model->dnorm = malloc (model->num_vertices * 3 * sizeof(float));
}
if (model->dpos == NULL) {
model->dpos = malloc (model->num_vertices * 3 * sizeof(float));
}
for (i=0; i<model->num_vertices; i++) {
unsigned char *bi = &model->blend_index[i*4];
//unsigned char *bw = &model->blend_weight[i*4];
mat_vec_mul (model->dpos + i*3,
model->skeleton->bones[bi[0]].diff,
model->pos + i*3);
mat_vec_mul_n (model->dnorm + i*3,
model->skeleton->bones[bi[0]].diff,
model->norm + i*3);
}
}
int main() {
mat_vec_mul(__out, input_array, input_vector, SIZE);
}
void render_spot_lamp(struct lamp *lamp, mat4 clip_from_view, mat4 view_from_world, mat4 lamp_transform)
{
static int prog = 0;
static int uni_viewport;
static int uni_view_from_clip;
static int uni_lamp_position;
static int uni_lamp_direction;
static int uni_lamp_color;
static int uni_lamp_distance;
static int uni_spot_size;
static int uni_spot_blend;
static int uni_use_sphere;
static const vec3 lamp_direction_init = { 0, 0, 1 };
mat4 view_from_clip;
vec2 viewport;
vec3 lamp_position;
vec3 lamp_direction_world;
vec3 lamp_direction_view;
vec3 lamp_direction;
vec3 lamp_color;
float spot_size;
float spot_blend;
if (!prog) {
prog = compile_shader(quad_vert_src, spot_frag_src);
uni_viewport = glGetUniformLocation(prog, "viewport");
uni_view_from_clip = glGetUniformLocation(prog, "view_from_clip");
uni_lamp_position = glGetUniformLocation(prog, "lamp_position");
uni_lamp_direction = glGetUniformLocation(prog, "lamp_direction");
uni_lamp_color = glGetUniformLocation(prog, "lamp_color");
uni_lamp_distance = glGetUniformLocation(prog, "lamp_distance");
uni_spot_size = glGetUniformLocation(prog, "spot_size");
uni_spot_blend = glGetUniformLocation(prog, "spot_blend");
uni_use_sphere = glGetUniformLocation(prog, "use_sphere");
}
mat_invert(view_from_clip, clip_from_view);
viewport[0] = fbo_w;
viewport[1] = fbo_h;
mat_vec_mul(lamp_position, view_from_world, lamp_transform + 12);
mat_vec_mul_n(lamp_direction_world, lamp_transform, lamp_direction_init);
mat_vec_mul_n(lamp_direction_view, view_from_world, lamp_direction_world);
vec_normalize(lamp_direction, lamp_direction_view);
vec_scale(lamp_color, lamp->color, lamp->energy);
spot_size = cos(M_PI * lamp->spot_angle / 360.0);
spot_blend = (1.0 - spot_size) * lamp->spot_blend;
glUseProgram(prog);
glUniform2fv(uni_viewport, 1, viewport);
glUniformMatrix4fv(uni_view_from_clip, 1, 0, view_from_clip);
glUniform3fv(uni_lamp_position, 1, lamp_position);
glUniform3fv(uni_lamp_direction, 1, lamp_direction);
glUniform3fv(uni_lamp_color, 1, lamp_color);
glUniform1f(uni_lamp_distance, lamp->distance);
glUniform1f(uni_spot_size, spot_size);
glUniform1f(uni_spot_blend, spot_blend);
glUniform1i(uni_use_sphere, lamp->use_sphere);
draw_fullscreen_quad();
}
int main() {
mat_vec_mul(C, I_A, I_B, 3);
}
