void Cube::Finalize()
{
int count = 0;
for (int i = 0; i < 2; i++) {
int x_sign = (i == 0) ? -1 : 1;
for (int j = 0; j < 2; j++) {
int y_sign = (j == 0) ? -1 : 1;
for (int k = 0; k < 2; k++) {
int z_sign = (k == 0) ? -1 : 1;
double x[3], y[3], z[3];
matrix_scale(3, 1, m_x_axis, x_sign * 0.5 * m_x_scale, x);
matrix_scale(3, 1, m_y_axis, y_sign * 0.5 * m_y_scale, y);
matrix_scale(3, 1, m_z_axis, z_sign * 0.5 * m_z_scale, z);
m_vertices[3 * count + 0] = m_origin[0] + x[0] + y[0] + z[0];
m_vertices[3 * count + 1] = m_origin[1] + x[1] + y[1] + z[1];
m_vertices[3 * count + 2] = m_origin[2] + x[2] + y[2] + z[2];
count++;
}
}
}
}
void EKF::_predictState(double *state, double *state_new) const
{
// copy old to new
matrix_scale(1, 3 * _num_feats_init_structure + _num_motion_states, state, 1.0, state_new);
double LinVel[3];
double AngVel[3];
LinVel[0] = state[3 * _num_feats_init_structure + 0];
LinVel[1] = state[3 * _num_feats_init_structure + 1];
LinVel[2] = state[3 * _num_feats_init_structure + 2];
AngVel[0] = state[3 * _num_feats_init_structure + 3];
AngVel[1] = state[3 * _num_feats_init_structure + 4];
AngVel[2] = state[3 * _num_feats_init_structure + 5];
double curr[3], new_pos[3];
double R[3 * 3];
matrix_scale(1, 3, LinVel, _dt, LinVel);
matrix_scale(1, 3, AngVel, _dt, AngVel);
RODR(AngVel, R);
for (int i = 0; i < _num_feats_init_structure; i++)
{
curr[0] = state[3 * i + 0];
curr[1] = state[3 * i + 1];
curr[2] = state[3 * i + 2];
matrix_product(3, 3, 3, 1, R, curr, new_pos);
matrix_sum(3, 1, 3, 1, new_pos, LinVel, new_pos);
//new_pos = LinVel*dt + ublas::prod(RODR(AngVel*dt),curr);
state_new[3 * i + 0] = new_pos[0];
state_new[3 * i + 1] = new_pos[1];
state_new[3 * i + 2] = new_pos[2];
}
}
void axis_angle_to_matrix4(double *axis, double angle, double *R) {
double ident[9];
double n[9] = { 0.0, -axis[2], axis[1],
axis[2], 0.0, -axis[0],
-axis[1], axis[0], 0.0 };
double nsq[9], sn[9], cnsq[9], tmp[9];
double R3[9];
double c, s;
c = cos(angle);
s = sin(angle);
matrix_ident(3, ident);
matrix_product33(n, n, nsq);
matrix_scale(3, 3, n, s, sn);
matrix_scale(3, 3, nsq, (1 - c), cnsq);
matrix_sum(3, 3, 3, 3, ident, sn, tmp);
matrix_sum(3, 3, 3, 3, tmp, cnsq, R3);
matrix_ident(4, R);
memcpy(R, R3, 3 * sizeof(double));
memcpy(R + 4, R3 + 3, 3 * sizeof(double));
memcpy(R + 8, R3 + 6, 3 * sizeof(double));
}
bool BundlerApp::EstimateRelativePose2(int i1, int i2,
camera_params_t &camera1,
camera_params_t &camera2)
{
// int num_images = GetNumImages();
MatchIndex list_idx;
if (i1 < i2)
list_idx = GetMatchIndex(i1, i2); // i1 * num_images + i2;
else
list_idx = GetMatchIndex(i2, i1); // i2 * num_images + i1;
std::vector<KeypointMatch> &matches = m_matches.GetMatchList(list_idx);
// int num_matches = (int) m_match_lists[list_idx].size();
int num_matches = (int) matches.size();
// double f1 = m_image_data[i1].m_init_focal;
// double f2 = m_image_data[i2].m_init_focal;
double K1[9], K2[9];
GetIntrinsics(camera1, K1);
GetIntrinsics(camera2, K2);
double R0[9], t0[3];
int num_inliers = 0;
if (!m_optimize_for_fisheye) {
num_inliers =
EstimatePose5Point(m_image_data[i1].m_keys,
m_image_data[i2].m_keys,
matches,
512, /* m_fmatrix_rounds, 8 * m_fmatrix_rounds */
0.25 * m_fmatrix_threshold, // 0.003, // 0.004 /*0.001,*/ // /*0.5 **/ m_fmatrix_threshold,
K1, K2, R0, t0);
} else {
std::vector<Keypoint> k1 = m_image_data[i1].UndistortKeysCopy();
std::vector<Keypoint> k2 = m_image_data[i2].UndistortKeysCopy();
num_inliers =
EstimatePose5Point(k1, k2, matches,
1024, /*512*/ /* m_fmatrix_rounds, 8 * m_fmatrix_rounds */
0.25 * m_fmatrix_threshold, // 0.004, /*0.001,*/ // /*0.5 **/ m_fmatrix_threshold,
K1, K2, R0, t0);
}
if (num_inliers == 0)
return false;
printf(" Found %d / %d inliers (%0.3f%%)\n", num_inliers, num_matches,
100.0 * num_inliers / num_matches);
bool initialized = false;
if (!initialized) {
memcpy(camera2.R, R0, sizeof(double) * 9);
matrix_transpose_product(3, 3, 3, 1, R0, t0, camera2.t);
matrix_scale(3, 1, camera2.t, -1.0, camera2.t);
}
return true;
}
double PlaneData::ProjectV(double *p, double *p_proj)
{
double dot;
matrix_product(1, 3, 3, 1, p, m_vaxis, &dot);
matrix_scale(3, 1, m_vaxis, dot, p_proj);
return dot;
}
static void lakka_draw_icon(lakka_handle_t *lakka,
GLuint texture, float x, float y,
float alpha, float rotation, float scale)
{
struct gl_coords coords;
if (!lakka)
return;
if (alpha > lakka->global_alpha)
alpha = lakka->global_alpha;
if (alpha == 0)
return;
gl_t *gl = (gl_t*)driver_video_resolve(NULL);
if (!gl)
return;
if (x < -lakka->icon_size || x > gl->win_width + lakka->icon_size
|| y < -lakka->icon_size || y > gl->win_height + lakka->icon_size)
return;
GLfloat color[] = {
1.0f, 1.0f, 1.0f, alpha,
1.0f, 1.0f, 1.0f, alpha,
1.0f, 1.0f, 1.0f, alpha,
1.0f, 1.0f, 1.0f, alpha,
};
if (gl->shader && gl->shader->use)
gl->shader->use(gl, GL_SHADER_STOCK_BLEND);
glViewport(x, gl->win_height - y, lakka->icon_size, lakka->icon_size);
coords.vertices = 4;
coords.vertex = lakka_vertex;
coords.tex_coord = lakka_tex_coord;
coords.lut_tex_coord = lakka_tex_coord;
coords.color = color;
glBindTexture(GL_TEXTURE_2D, texture);
math_matrix mymat;
math_matrix mrot;
matrix_rotate_z(&mrot, rotation);
matrix_multiply(&mymat, &mrot, &gl->mvp_no_rot);
math_matrix mscal;
matrix_scale(&mscal, scale, scale, 1);
matrix_multiply(&mymat, &mscal, &mymat);
gl->shader->set_coords(&coords);
gl->shader->set_mvp(gl, &mymat);
glEnable(GL_BLEND);
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
glDrawArrays(GL_TRIANGLE_STRIP, 0, 4);
glDisable(GL_BLEND);
}
int main(int argc, char *argv[]){
double mmvalues[] = {2.0,3.0,1.0,1.0,4.0,2.0,2.0,1.0,2.0,1.0,4.0,2.0,2.0,3.0,1.0,1.0,4.0,2.0,2.0,1.0,2.0,1.0,4.0,2.0,5.0,4.0,2.0,2.0,1.0,2.0,\
4.0,2.0,2.0,1.0,2.0,1.0,6.0,3.0,1.0,1.0,4.0,2.0,2.0,1.0,9.0,1.0,4.0,2.0,5.0,4.0,2.0,2.0,1.0,2.0,5.0,5.0,2.0,2.0,1.0,2.0};
matrix_t mm, ma, mb, mc, md, mx, my, mz;
init_matrix(&mm, 'm', 6, 6, mmvalues);
init_matrix(&ma, 'a', 3, 3, NULL);
init_matrix(&mb, 'b', 3, 3, NULL);
init_matrix(&mc, 'c', 3, 3, NULL);
display_matrix(&mm);
display_matrix(&ma);
display_matrix(&mb);
display_matrix(&mc);
free_matrix(&ma);
ma = get_submatrix(&mm, 2, 1, 4, 4);
display_matrix(&ma);
free_matrix(&mb);
mb = get_submatrix(&mm, 3, 2, 4, 4);
display_matrix(&mb);
mx = matrix_mul(&ma, &mb, 'X');
my = square_matrix_mul_recursive(&ma, &mb, 'Y');
mz = square_matrix_mul_strassen(&ma, &mb, 'Z');
printf("the result is \n");
display_matrix(&mx);
display_matrix(&my);
display_matrix(&mz);
ma = get_submatrix(&mm, 1, 2, 6, 1);
mb = get_submatrix(&mm, 2, 1, 1, 6);
display_matrix(&ma);
display_matrix(&mb);
mc = matrix_mul(&mb, &ma, '1');
display_matrix(&mc);
md = matrix_mul(&ma, &mb, 'S');
display_matrix(&md);
free_matrix(&mc);
mc = matrix_scale(&md, 2, '2');
display_matrix(&mc);
free_matrix(&ma);
free_matrix(&mb);
free_matrix(&mc);
free_matrix(&md);
free_matrix(&mx);
free_matrix(&my);
free_matrix(&mz);
return 0;
}//main
//3D viewing pipeline. VTM is complete view matrix. none of the values of the View structure should be edited.
void matrix_setView3D(Matrix *vtm, View3D *view){
Vector u, vup, vpn;
double b, d;
matrix_identity(vtm);
matrix_translate(vtm, -view->vrp.val[0], -view->vrp.val[1], -view->vrp.val[2]);
vpn = view->vpn;
vector_cross(&view->vup, &vpn, &u);
vector_cross(&vpn, &u, &vup);
vector_normalize(&u);
vector_normalize(&vup);
vector_normalize(&vpn);
matrix_rotateXYZ(vtm, &u, &vup, &vpn);
matrix_translate(vtm, 0.0, 0.0, view->d);
// in lecture notes here (6 and 7) it says to shear but as we only have d to define the COP I don't think we have to
b = view->d + view->b;
matrix_scale(vtm, ((2.0*view->d) / (b*view->du)), ((2.0*view->d) / (b*view->dv)), (1/b));
d = view->d / b;
matrix_perspective(vtm, d);
matrix_scale2D(vtm, (-view->screenx / (2.0*d)), (-view->screeny / (2.0*d)));
matrix_translate2D(vtm, (view->screenx / 2.0), (view->screeny / 2.0));
}
void TwoFrameModel::WriteSparse(FILE *f)
{
// WriteCameraPose(f, m_camera0);
// WriteCameraPose(f, m_camera1);
/* Compute the camera pose of camera1 relative to camera0 */
double pos0[3], pos1[3];
// matrix_transpose_product(3, 3, 3, 1, m_camera0.R, m_camera0.t, pos0);
// matrix_transpose_product(3, 3, 3, 1, m_camera1.R, m_camera1.t, pos1);
memcpy(pos0, m_camera0.t, 3 * sizeof(double));
memcpy(pos1, m_camera1.t, 3 * sizeof(double));
// matrix_scale(3, 1, pos0, -1.0, pos0);
// matrix_scale(3, 1, pos1, -1.0, pos1);
double diff[3];
matrix_diff(3, 1, 3, 1, pos1, pos0, diff);
double R1[9], tr[3];
matrix_transpose_product2(3, 3, 3, 3, m_camera0.R, m_camera1.R, R1);
// matrix_transpose_product(3, 3, 3, 3, m_camera1.R, m_camera0.R, R1);
matrix_product(3, 3, 3, 1, m_camera0.R, diff, tr);
double norm = matrix_norm(3, 1, tr);
matrix_scale(3, 1, tr, 1.0 / norm, tr);
double viewdir[3] = { -R1[2], -R1[5], -R1[8] };
double twist_angle = GetTwist(R1);
/* Compute the distance to the scene */
double z_avg = 0.0;
for (int p = 0; p < m_num_points; p++) {
v3_t &pt = m_points[p];
double diff1[3], diff2[3];
matrix_diff(3, 1, 3, 1, pt.p, pos0, diff1);
matrix_diff(3, 1, 3, 1, pt.p, pos1, diff2);
double dist1 = matrix_norm(3, 1, diff1);
double dist2 = matrix_norm(3, 1, diff2);
z_avg += 0.5 * (dist1 + dist2) / norm;
}
z_avg /= m_num_points;
WriteVector(f, 9, R1);
/* Write the viewing direction */
// WriteVector(f, 3, viewdir);
/* Write the twist angle */
// fprintf(f, "%0.8f\n", twist_angle);
/* Write the translation */
WriteVector(f, 3, tr);
fprintf(f, "%0.6f\n", z_avg);
}
void matrix_setView3D(Matrix *vtm, View3D *view){
if(NULL != vtm && NULL != view){
Vector u;
Vector vup = view->vup;
Vector vpn = view->vpn;
Matrix project;
double bPrime = view->d +view->b;
double dPrime = view->d/bPrime;
matrix_identity(vtm);
printf("before everything:\n");
matrix_print(vtm, stdout);
vector_cross(&vup,&vpn,&u);
vector_cross(&vpn,&u,&vup);
printf("vrp:\n");
vector_print(&view->vrp,stdout);
matrix_translate(vtm, -view->vrp.val[0], -view->vrp.val[1],-view->vrp.val[2]);
printf("After VRP translation:\n");
matrix_print(vtm, stdout);
vector_normalize(&u);
vector_normalize(&vpn);
vector_normalize(&vup);
matrix_rotateXYZ(vtm, &u, &vup, &vpn );
printf("After Rxyz :\n");
matrix_print(vtm, stdout);
matrix_translate(vtm, 0, 0,view->d);
printf("After translating COP to origin:\n");
matrix_print(vtm, stdout);
matrix_scale(vtm, (2*view->d)/(bPrime*view->du), (2*view->d)/(bPrime*view->dv), 1/bPrime);
printf("After scaling to CVV:\n");
matrix_print(vtm, stdout);
matrix_identity(&project);
project.m[3][3]=0;
project.m[3][2]=1/dPrime;
printf("projection:\n");
matrix_print(&project, stdout);
matrix_multiply(&project,vtm,vtm);
printf("After perspective:\n");
matrix_print(vtm, stdout);
matrix_scale2D(vtm, -view->screenx/(2*dPrime), -view->screeny/(2*dPrime));
printf("After scale to image coords:\n");
matrix_print(vtm, stdout);
matrix_translate2D(vtm, view->screenx/2, view->screeny/2);
printf("After final translation to image coords:\n");
matrix_print(vtm, stdout);
}
}
/* Compute an updated rotation matrix given the initial rotation (R)
* and the correction (w) */
void rot_update(double *R, double *w, double *Rnew)
{
double theta, sinth, costh, n[3];
double nx[9], nxsq[9];
double term2[9], term3[9];
double tmp[9], dR[9];
double ident[9] = {1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0};
theta = sqrt(w[0] * w[0] + w[1] * w[1] + w[2] * w[2]);
if (theta == 0.0)
{
memcpy(Rnew, R, sizeof(double) * 9);
return;
}
n[0] = w[0] / theta;
n[1] = w[1] / theta;
n[2] = w[2] / theta;
nx[0] = 0.0;
nx[1] = -n[2];
nx[2] = n[1];
nx[3] = n[2];
nx[4] = 0.0;
nx[5] = -n[0];
nx[6] = -n[1];
nx[7] = n[0];
nx[8] = 0.0;
matrix_product33(nx, nx, nxsq);
sinth = sin(theta);
costh = cos(theta);
matrix_scale(3, 3, nx, sinth, term2);
matrix_scale(3, 3, nxsq, 1.0 - costh, term3);
matrix_sum(3, 3, 3, 3, ident, term2, tmp);
matrix_sum(3, 3, 3, 3, tmp, term3, dR);
matrix_product33(dR, R, Rnew);
}
void lakka_draw_icon(GLuint texture, float x, float y, float alpha, float rotation, float scale)
{
GLfloat color[] = {
1.0f, 1.0f, 1.0f, alpha,
1.0f, 1.0f, 1.0f, alpha,
1.0f, 1.0f, 1.0f, alpha,
1.0f, 1.0f, 1.0f, alpha,
};
static const GLfloat vtest[] = {
0, 0,
1, 0,
0, 1,
1, 1
};
gl_t *gl = (gl_t*)driver.video_data;
if (!gl)
return;
if (alpha > global_alpha)
alpha = global_alpha;
glViewport(x, gl->win_height - y, dim, dim);
glEnable(GL_BLEND);
gl->coords.vertex = vtest;
gl->coords.tex_coord = vtest;
gl->coords.color = color;
glBindTexture(GL_TEXTURE_2D, texture);
if (gl->shader && gl->shader->use)
gl->shader->use(gl, GL_SHADER_STOCK_BLEND);
math_matrix mymat;
math_matrix mrot;
matrix_rotate_z(&mrot, rotation);
matrix_multiply(&mymat, &mrot, &gl->mvp_no_rot);
math_matrix mscal;
matrix_scale(&mscal, scale, scale, 1);
matrix_multiply(&mymat, &mscal, &mymat);
gl->coords.vertices = 4;
gl_shader_set_coords(gl, &gl->coords, &mymat);
glDrawArrays(GL_TRIANGLE_STRIP, 0, 4);
glDisable(GL_BLEND);
gl->coords.vertex = gl->vertex_ptr;
gl->coords.tex_coord = gl->tex_coords;
gl->coords.color = gl->white_color_ptr;
}
// only works for 2x2 matrices
matrix matrix_inverse(matrix A){
double data[A.rows*A.columns];
data[0] = *A.data[0];
data[3] = *A.data[3];
data[1] = -*A.data[2];
data[2] = -*A.data[1];
double s = data[0] * *A.data[3] - data[2] * *A.data[1];
matrix m = matrix_create(A.rows, A.columns, data);
return matrix_scale(m, 1.0/s);
}
/* Return the homogeneous form of the line */
void LineSegment2D::Homogeneous(double *l)
{
double p1[3] = { m_p1[0], m_p1[1], 1.0 };
double p2[3] = { m_p2[0], m_p2[1], 1.0 };
matrix_cross(p1, p2, l);
double norm = matrix_norm(3, 1, l);
matrix_scale(3, 1, l, 1.0 / norm, l);
}
static int
lscale(lua_State *L) {
struct matrix *m = (struct matrix *)lua_touserdata(L, 1);
double sx = luaL_checknumber(L,2);
double sy = luaL_optnumber(L,3,sx);
matrix_scale(m, sx * 1024, sy * 1024);
lua_settop(L,1);
return 1;
}
/* Project a point onto the plane */
void PlaneData::Project(double *p, double *p_proj)
{
/* Subtract the distance vector */
double vec[3];
matrix_scale(3, 1, m_normal, m_dist, vec);
double p_norm[3];
matrix_diff(3, 1, 3, 1, p, vec, p_norm);
double dot;
matrix_product(1, 3, 3, 1, m_normal, p_norm, &dot);
double p_par[3];
matrix_scale(3, 1, m_normal, dot, p_par);
double p_perp[3];
matrix_diff(3, 1, 3, 1, p_norm, p_par, p_perp);
matrix_sum(3, 1, 3, 1, p_perp, vec, p_proj);
}
/* initialize identity */
void initialize_identity_matrix(mat *D){
int i;
matrix_scale(D, 0);
#pragma omp parallel shared(D) private(i)
{
#pragma omp parallel for
for(i=0; i<(D->nrows); i++){
matrix_set_element(D,i,i,1.0);
}
}
}
/**
* Set the transformation to map a pixel to the paint coordinates.
*/
void shader_set_paint_matrix(struct shader *shader, const struct matrix *mat)
{
const struct st_framebuffer *stfb = shader->context->draw_buffer;
const VGfloat px_center_offset = 0.5f;
memcpy(&shader->paint_matrix, mat, sizeof(*mat));
/* make it window-to-paint for the shaders */
matrix_translate(&shader->paint_matrix, px_center_offset,
stfb->height - 1.0f + px_center_offset);
matrix_scale(&shader->paint_matrix, 1.0f, -1.0f);
}
/*
* Matrix operand to add a 3D scale to the Module.
*/
void module_scale(Module *md, double sx, double sy, double sz){
if(!md){
printf("Null md passed to module_scale\n");
return;
}
Element *e;
Matrix m;
matrix_identity(&m);
matrix_scale(&m, sx, sy, sz);
e = element_init(ObjMatrix, &m);
module_insert(md, e);
}
void axis_angle_to_matrix(double *axis, double angle, double *R) {
double ident[9];
double n[9] = { 0.0, -axis[2], axis[1],
axis[2], 0.0, -axis[0],
-axis[1], axis[0], 0.0 };
double nsq[9], sn[9], cnsq[9], tmp[9];
double c, s;
c = cos(angle);
s = sin(angle);
matrix_ident(3, ident);
matrix_product33(n, n, nsq);
matrix_scale(3, 3, n, s, sn);
matrix_scale(3, 3, nsq, (1 - c), cnsq);
matrix_sum(3, 3, 3, 3, ident, sn, tmp);
matrix_sum(3, 3, 3, 3, tmp, cnsq, R);
}
static void
draw(void)
{
GLfloat mat[16], projection[16];
struct cube c;
/* Set modelview/projection matrix */
matrix_make_unity(mat);
matrix_rotate_x(view_rotx, mat);
matrix_rotate_y(view_roty, mat);
matrix_rotate_z(view_rotz, mat);
matrix_translate(0.0, 0.0, view_transz, mat);
matrix_scale(view_scale, view_scale, view_scale, mat);
matrix_make_projection(0.9, projection);
//print_matrix(mat);
glUniformMatrix4fv(u_projection, 1, GL_FALSE, projection);
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
start_cube();
int a;
for (a = 0; a < N_CUBES; a++) {
// print_cube(&cubes[a]);
update_cube(&cubes[a]);
draw_a_cube(&cubes[a], mat);
printf("a=%d\n",a);
print_cube(&cubes[a]);
}
/*
GLfloat i,j,k;
#define LOW -4.0
#define HIGH 4.0
#define STEP 2.0
for (i = LOW; i < HIGH; i += STEP) {
for (j = LOW; j < HIGH; j += STEP) {
for (k = HIGH+10.0; k > LOW+10.0; k -= STEP) {
draw_cube(i, j, k, 0.0, 0.0, 0.0, 1.0, mat);
}
}
}
#undef LOW
#undef HIGH
#undef STEP
*/
end_cube();
#undef N
}
/* Compute the covariance of a set of (zero-mean) vectors */
void v3_covariance_zm(int n, const v3_t *v, double *cov) {
int i;
for (i = 0; i < 9; i++)
cov[i] = 0.0;
for (i = 0; i < n; i++) {
double tmp[9];
matrix_product(3, 1, 1, 3, v[i].p, v[i].p, tmp);
matrix_sum(3, 3, 3, 3, cov, tmp, cov);
}
matrix_scale(3, 3, cov, 1.0 / n, cov);
}
static void get_brick_vertices(
Brick *br,
VRState *state,
Coord cpt[8],
VRVolumeData *vd,
Matrix RTTMat
)
{
int i;
Matrix BTRMat;
/* Locate brick cube at the proper location and rotate it */
matrix_copy(IdentityMatrix, 4, BTRMat);
matrix_translate(1.0, 1.0, 1.0, BTRMat);
matrix_scale(0.5, 0.5, 0.5, BTRMat);
matrix_translate(br->xOff, br->yOff, br->zOff, BTRMat);
matrix_scale(1.0 / (float) vd->nxBricks,
1.0 / (float) vd->nyBricks,
1.0 / (float) vd->nzBricks, BTRMat);
matrix_scale(2.0, 2.0, 2.0, BTRMat);
matrix_translate(-1.0, -1.0, -1.0, BTRMat);
matrix_mult_safe(BTRMat, vd->VTRMat, BTRMat);
/* Transform unit cube */
for (i = 0; i < 8; i++)
{
coord_transform(PlusMinusCube[i], BTRMat, cpt[i]);
}
/* Store invers transformation so texture coords are in range [0.0 1.0] */
matrix_invert(BTRMat, RTTMat);
matrix_translate(1.0, 1.0, 1.0, RTTMat);
matrix_scale(0.5, 0.5, 0.5, RTTMat);
matrix_translate(br->txOff, br->tyOff, br->tzOff, RTTMat);
matrix_scale(br->txScl, br->tyScl, br->tzScl, RTTMat);
}
v3_t BundlerApp::GeneratePointAtInfinity(const ImageKeyVector &views,
int *added_order,
camera_params_t *cameras,
double &error,
bool explicit_camera_centers)
{
camera_params_t *cam = NULL;
int camera_idx = views[0].first;
int image_idx = added_order[camera_idx];
int key_idx = views[0].second;
Keypoint &key = GetKey(image_idx, key_idx);
cam = cameras + camera_idx;
double p3[3] = { key.m_x, key.m_y, 1.0 };
if (m_optimize_for_fisheye) {
/* Undistort the point */
double x = p3[0], y = p3[1];
m_image_data[image_idx].UndistortPoint(x, y, p3[0], p3[1]);
}
double K[9], Kinv[9];
GetIntrinsics(cameras[camera_idx], K);
matrix_invert(3, K, Kinv);
double ray[3];
matrix_product(3, 3, 3, 1, Kinv, p3, ray);
/* We now have a ray, put it at infinity */
double ray_world[3];
matrix_transpose_product(3, 3, 3, 1, cam->R, ray, ray_world);
double pos[3] = { 0.0, 0.0, 0.0 };
double pt_inf[3] = { 0.0, 0.0, 0.0 };
if (!explicit_camera_centers) {
} else {
memcpy(pos, cam->t, 3 * sizeof(double));
double ray_extend[3];
matrix_scale(3, 1, ray, 100.0, ray_extend);
matrix_sum(3, 1, 3, 1, pos, ray, pt_inf);
}
return v3_new(pt_inf[0], pt_inf[1], pt_inf[2]);
}
void
draw_cube(GLfloat x, GLfloat y, GLfloat z, GLfloat rx, GLfloat ry, GLfloat rz, GLfloat scale, GLfloat *m)
{
GLfloat mat[16];
memcpy(mat, m, sizeof(mat));
matrix_scale(scale, scale, scale, mat);
matrix_translate(x, y, z, mat);
matrix_rotate_z(rz, mat);
matrix_rotate_y(ry, mat);
matrix_rotate_x(rx, mat);
glUniformMatrix4fv(u_matrix, 1, GL_FALSE, mat);
glDrawArrays(GL_TRIANGLE_STRIP, 0, N);
}
/**
* Propogate OpenVG state changes to the renderer. Only framebuffer, blending
* and scissoring states are relevant here.
*/
void renderer_validate(struct renderer *renderer,
VGbitfield dirty,
const struct st_framebuffer *stfb,
const struct vg_state *state)
{
assert(renderer->state == RENDERER_STATE_INIT);
dirty |= renderer->dirty;
renderer->dirty = 0;
if (dirty & FRAMEBUFFER_DIRTY) {
struct pipe_framebuffer_state *fb = &renderer->g3d.fb;
struct matrix *proj = &renderer->projection;
memset(fb, 0, sizeof(struct pipe_framebuffer_state));
fb->width = stfb->width;
fb->height = stfb->height;
fb->nr_cbufs = 1;
fb->cbufs[0] = stfb->strb->surface;
fb->zsbuf = stfb->dsrb->surface;
cso_set_framebuffer(renderer->cso, fb);
vg_set_viewport(renderer, VEGA_Y0_BOTTOM);
matrix_load_identity(proj);
matrix_translate(proj, -1.0f, -1.0f);
matrix_scale(proj, 2.0f / fb->width, 2.0f / fb->height);
/* we also got a new depth buffer */
if (dirty & DEPTH_STENCIL_DIRTY) {
renderer->pipe->clear(renderer->pipe,
PIPE_CLEAR_DEPTHSTENCIL, NULL, 0.0, 0);
}
}
/* must be last because it renders to the depth buffer*/
if (dirty & DEPTH_STENCIL_DIRTY) {
update_clip_state(renderer, state);
cso_set_depth_stencil_alpha(renderer->cso, &renderer->g3d.dsa);
}
if (dirty & BLEND_DIRTY)
renderer_validate_blend(renderer, state, stfb->strb->format);
}
bool PlaneData::SetCorners(double *R, double *t, double f, int w, int h, int ymin, int ymax, int xmin, int xmax)
{
if(xmax==-1) xmax=w;
if(ymax==-1) ymax=h;
if(xmax>w || ymax>h ||xmax<-1 || ymax<-1) printf("Error in bounding box data w=%d h=%d xmin=%d xmax=%d ymin=%d ymax=%d\n",w,h,xmin,xmax,ymin,ymax);
//printf("xmin=%d xmax=%d ymin=%d ymax=%d\n",xmin,xmax,ymin,ymax);
double Rt[9];
matrix_transpose(3, 3, R, Rt);
double center[3];
matrix_product(3, 3, 3, 1, Rt, t, center);
matrix_scale(3, 1, center, -1.0, center);
/* Create rays for the four corners */
//uncomment the follwing code to use the 4 image corners
//double ray1[3] = { -0.5 * w, -0.5 * h, -f };
//double ray2[3] = { 0.5 * w, -0.5 * h, -f };
//double ray3[3] = { 0.5 * w, 0.5 * h, -f };
//double ray4[3] = { -0.5 * w, 0.5 * h, -f };
double ray1[3] = { xmin-0.5 * w, ymin-0.5 * h, -f };
double ray2[3] = { xmax-0.5 * w, ymin-0.5 * h, -f };
double ray3[3] = { xmax -0.5 * w, ymax-0.5 * h, -f };
double ray4[3] = {xmin -0.5 * w, ymax - 0.5 * h, -f };
double ray_world[18];
matrix_product(3, 3, 3, 1, Rt, ray1, ray_world + 0);
matrix_product(3, 3, 3, 1, Rt, ray2, ray_world + 3);
matrix_product(3, 3, 3, 1, Rt, ray3, ray_world + 6);
matrix_product(3, 3, 3, 1, Rt, ray4, ray_world + 9);
double t0 = IntersectRay(center, ray_world + 0, m_corners + 0);
double t1 = IntersectRay(center, ray_world + 3, m_corners + 3);
double t2 = IntersectRay(center, ray_world + 6, m_corners + 6);
double t3 = IntersectRay(center, ray_world + 9, m_corners + 9);
if (t0 > 0.0 && t1 > 0.0 && t2 > 0.0 && t3 > 0.0)
return true;
else
return false;
}
static void lakka_draw_fbo(void)
{
gl_t *gl = (gl_t*)driver_video_resolve(NULL);
if (!gl)
return;
struct gl_coords coords;
coords.vertices = 4;
coords.vertex = lakka_vertex;
coords.tex_coord = lakka_vertex;
coords.color = gl->white_color_ptr;
glBindTexture(GL_TEXTURE_2D, lakka->fbo_color);
math_matrix mymat;
math_matrix mrot;
matrix_rotate_z(&mrot, 0);
matrix_multiply(&mymat, &mrot, &gl->mvp_no_rot);
math_matrix mscal;
matrix_scale(&mscal, lakka->global_scale, lakka->global_scale, 1);
matrix_multiply(&mymat, &mscal, &mymat);
gl->shader->set_coords(&coords);
gl->shader->set_mvp(gl, &mymat);
glEnable(GL_BLEND);
// shadow
glViewport(2, -2, gl->win_width, gl->win_height);
glBlendFunc(GL_ZERO, GL_ONE_MINUS_SRC_ALPHA);
glDrawArrays(GL_TRIANGLE_STRIP, 0, 4);
glViewport(0, 0, gl->win_width, gl->win_height);
glBlendFunc(GL_ONE, GL_ONE_MINUS_SRC_ALPHA);
glDrawArrays(GL_TRIANGLE_STRIP, 0, 4);
glDisable(GL_BLEND);
}
void view_rotate_circle(Polygon* poly_vrp, Point* center, int sides, double scale, double thetax, double thetay, double thetaz){
if(NULL != poly_vrp){
//make a unit circle of "sides" number fo sides and store the points in the poly_vrp
int i;
double x, z;
polygon_init(poly_vrp);
Point p[sides];
Matrix LTM;
matrix_identity(<M);
matrix_scale(<M, scale, scale, scale);
matrix_rotateX(<M, cos(thetax*M_PI/180), sin(thetax*M_PI/180));
matrix_rotateY(<M, cos(thetay*M_PI/180), sin(thetay*M_PI/180));
matrix_rotateZ(<M, cos(thetaz*M_PI/180), sin(thetaz*M_PI/180));
matrix_translate(<M, center->val[0], center->val[1], center->val[2]);
for(i=0; i<sides; i++){
x = cos( i * M_PI * 2.0 / sides );
z = sin( i * M_PI * 2.0 / sides );
point_set3D(&p[i], x, 0.1, z);
}
polygon_set(poly_vrp, sides, &p[0]);
matrix_xformPolygon(<M, poly_vrp);
}
}
int main(int argc, char *argv[]){
//glfwSetKeyCallback(window, &keypress);
GLFWwindow *window;
GLuint VAO, VBO;
GLuint program;
GLuint crate;
matrix camera, perspective, rotate, scale, translate;
GLuint l_camera, l_rotate, l_scale, l_translate, l_time, l_perspective;
float time;
// init window
window = dg_window(1920-192, 1080-108, "OpenGL Experiment");
// init shaders
dg_program(&program);
dg_attach("vertex.glsl", GL_VERTEX_SHADER);
dg_attach("fragment.glsl", GL_FRAGMENT_SHADER);
dg_compile();
// init texture
crate = dg_texture("crate.jpg");
// init model
glGenBuffers(1, &VBO);
glGenVertexArrays(1, &VAO);
glBindVertexArray(VAO);
glEnable(GL_DEPTH_TEST);
glBindBuffer(GL_ARRAY_BUFFER, VBO);
glBufferData(GL_ARRAY_BUFFER, sizeof(MODEL_CUBE), MODEL_CUBE, GL_STATIC_DRAW);
glVertexAttribPointer(0, 3, GL_FLOAT, GL_FALSE, 5 * sizeof(GLfloat), (GLvoid*)(0));
glVertexAttribPointer(1, 2, GL_FLOAT, GL_FALSE, 5 * sizeof(GLfloat), (GLvoid*)(3*sizeof(GLfloat)));
glEnableVertexAttribArray(0);
glEnableVertexAttribArray(1);
glBindVertexArray(0);
// locate shader variables
l_scale = glGetUniformLocation(program, "scale");
l_camera = glGetUniformLocation(program, "camera");
l_rotate = glGetUniformLocation(program, "rotate");
l_translate = glGetUniformLocation(program, "translate");
l_perspective = glGetUniformLocation(program, "perspective");
// init matrices
matrix_perspective(&perspective, 90.0f, 90.0f, 0.1f, 100.0f);
matrix_identity(&rotate);
matrix_identity(&scale);
matrix_identity(&translate);
//glPolygonMode(GL_FRONT_AND_BACK, GL_LINE); // uncomment for wireframe
while(!glfwWindowShouldClose(window)){
glfwPollEvents();
time = (float)(glfwGetTime() * 100);
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glClearColor(0.0f, 0.0f, 0.0f, 1.0f);
//eye target up
matrix_camera(&camera,
// position
0.0f, 0.0f, 0.0f,
// angle
0.0f, 0.0f, 0.0f
);
glUseProgram(program);
// send to gpu
glUniform1f(l_time, time);
glUniformMatrix4fv(l_camera, 1, GL_TRUE, camera.data);
glUniformMatrix4fv(l_perspective, 1, GL_TRUE, perspective.data);
// bind model
glBindVertexArray(VAO);
// calculate transformations
matrix_scale(&scale, 1.0f, 1.0f, 1.0f);
matrix_rotate_y(&rotate, time);
matrix_translate(&translate, 0.0f, 0.0f, -1.0f);
// send to gpu
glUniformMatrix4fv(l_scale, 1, GL_TRUE, scale.data);
glUniformMatrix4fv(l_rotate, 1, GL_TRUE, rotate.data);
glUniformMatrix4fv(l_translate, 1, GL_TRUE, translate.data);
// texture
glBindTexture(GL_TEXTURE_2D, crate);
// draw
glDrawArrays(GL_TRIANGLES, 0, 36);
// unbind model VAO
glBindVertexArray(0);
glfwSwapBuffers(window);
}
glfwTerminate();
return EXIT_SUCCESS;
}
