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