/* get_align_matrix_f:
* Floating point version of get_align_matrix().
*/
void get_align_matrix_f(MATRIX_f *m, float xfront, float yfront, float zfront, float xup, float yup, float zup)
{
float xright, yright, zright;
ASSERT(m);
xfront = -xfront;
yfront = -yfront;
zfront = -zfront;
normalize_vector_f(&xfront, &yfront, &zfront);
cross_product_f(xup, yup, zup, xfront, yfront, zfront, &xright, &yright, &zright);
normalize_vector_f(&xright, &yright, &zright);
cross_product_f(xfront, yfront, zfront, xright, yright, zright, &xup, &yup, &zup);
/* No need to normalize up here, since right and front are perpendicular and normalized. */
m->v[0][0] = xright;
m->v[0][1] = xup;
m->v[0][2] = xfront;
m->v[1][0] = yright;
m->v[1][1] = yup;
m->v[1][2] = yfront;
m->v[2][0] = zright;
m->v[2][1] = zup;
m->v[2][2] = zfront;
m->t[0] = m->t[1] = m->t[2] = 0;
}
/* get_camera_matrix_f:
* Floating point version of get_camera_matrix().
*/
void get_camera_matrix_f(MATRIX_f *m, float x, float y, float z, float xfront, float yfront, float zfront, float xup, float yup, float zup, float fov, float aspect)
{
MATRIX_f camera, scale;
float xside, yside, zside, width, d;
ASSERT(m);
/* make 'in-front' into a unit vector, and negate it */
normalize_vector_f(&xfront, &yfront, &zfront);
xfront = -xfront;
yfront = -yfront;
zfront = -zfront;
/* make sure 'up' is at right angles to 'in-front', and normalize */
d = dot_product_f(xup, yup, zup, xfront, yfront, zfront);
xup -= d * xfront;
yup -= d * yfront;
zup -= d * zfront;
normalize_vector_f(&xup, &yup, &zup);
/* calculate the 'sideways' vector */
cross_product_f(xup, yup, zup, xfront, yfront, zfront, &xside, &yside, &zside);
/* set matrix rotation parameters */
camera.v[0][0] = xside;
camera.v[0][1] = yside;
camera.v[0][2] = zside;
camera.v[1][0] = xup;
camera.v[1][1] = yup;
camera.v[1][2] = zup;
camera.v[2][0] = xfront;
camera.v[2][1] = yfront;
camera.v[2][2] = zfront;
/* set matrix translation parameters */
camera.t[0] = -(x*xside + y*yside + z*zside);
camera.t[1] = -(x*xup + y*yup + z*zup);
camera.t[2] = -(x*xfront + y*yfront + z*zfront);
/* construct a scaling matrix to deal with aspect ratio and FOV */
width = floattan(64.0 - fov/2);
get_scaling_matrix_f(&scale, width, -aspect*width, -1.0);
/* combine the camera and scaling matrices */
matrix_mul_f(&camera, &scale, m);
}
void lane_filtering2(unsigned char *img, PointKAIST *pt_left, PointKAIST *pt_right, int WIDTH, int HEIGHT)
{
/////// point 저장 변수
PointKAIST temp_pt_left[3] = {0,}, temp_pt_right[3] = {0,};
PointKAIST temp_left, temp_right;
int points_num = 0;
int x_left=0, x_right=0;
/////// line 파라미터 저장 변수
float left[3], left_s[3], left_line[3];
float right[3], right_s[3], right_line[3];
float left_right[3];
int r_length = 0, l_length = 0;
left_s[0] = pt_left[0].x; left_s[1] = pt_left[0].y; left_s[2] = 1;
left[0] = pt_left[2].x; left[1] = pt_left[2].y; left[2] = 1;
right_s[0] = pt_right[0].x; right_s[1] = pt_right[0].y; right_s[2] = 1;
right[0] = pt_right[2].x; right[1] = pt_right[2].y; right[2] = 1;
cross_product_f(left, left_s, left_line);
cross_product_f(right, right_s, right_line);
printf("coord : %f %f\n", left_line[0], left_line[1]);
// cross_product(left_line, right_line, left_right);
// 위에 있는지 여부 확인
//printf("%d ", abs((-left_line[0] * pt_left[1].x - left_line[2]) / left_line[1] - pt_left[1].y));
/////if(left_line[1] != 0 && (abs((-left_line[0] * pt_left[1].x - left_line[2]) / left_line[1] - pt_left[1].y)) < 20)
{
if(left_line[0] != 0)
{
temp_pt_left[0].x = (-left_line[1] * (HEIGHT-1) -left_line[2]) / left_line[0];//pt_right[2];
temp_pt_left[0].y = HEIGHT-1;//pt_right[2];
}
else
temp_pt_left[0] = pt_left[0];
if(left_line[0] != 0)
{
temp_pt_left[1].x = (-left_line[1] * (horizontal_center) -left_line[2]) / left_line[0];//pt_right[2];
temp_pt_left[1].y = horizontal_center;//pt_right[2];
}
else
temp_pt_left[1] = pt_left[1];
temp_pt_left[2] = pt_left[2];
l_length = 120 - pt_left[1].x;
}
/////else
/////{
/////temp_pt_left[1] = prev_pt_left[1];
/////temp_pt_left[2] = prev_pt_left[2];
/////l_length = 120 - temp_pt_left[1].x;
/////}
/////if(right_line[1] != 0 && (abs((-right_line[0] * pt_right[1].x - right_line[2]) / right_line[1] - pt_right[1].y)) < 20)
{
if(right_line[0] != 0)
{
temp_pt_right[0].x = (-right_line[1] * (HEIGHT-1) -right_line[2]) / right_line[0];//pt_right[2];
temp_pt_right[0].y = HEIGHT-1;//pt_right[2];
}
else
temp_pt_right[0] = pt_right[0];
if(right_line[0] != 0)
{
temp_pt_right[1].x = (-right_line[1] * (horizontal_center) -right_line[2]) / right_line[0];//pt_right[2];
temp_pt_right[1].y = horizontal_center;//pt_right[2];
}
else
temp_pt_right[1] = pt_right[1];
temp_pt_right[2] = pt_right[2];
r_length = pt_right[1].x - 120;
}
/////else
/////{
/////temp_pt_right[1] = prev_pt_right[1];
/////temp_pt_right[2] = prev_pt_right[2];
/////r_length = temp_pt_right[1].x - 120;
/////}
if(temp_pt_left[0].x == 0)
{
temp_pt_left[0] = prev_pt_left[0];
temp_pt_left[1] = prev_pt_left[1];
temp_pt_left[2] = prev_pt_left[2];
}
if(temp_pt_right[0].x == 0)
{
temp_pt_right[0] = prev_pt_right[0];
temp_pt_right[1] = prev_pt_right[1];
temp_pt_right[2] = prev_pt_right[2];
}
// printf("%d %d %d\n", temp_pt_right[0].x, temp_pt_right[1].x, temp_pt_right[2].x);
left_s[0] = temp_pt_left[0].x; left_s[1] = temp_pt_left[0].y; left_s[2] = 1;
left[0] = temp_pt_left[2].x; left[1] = temp_pt_left[2].y; left[2] = 1;
right_s[0] = temp_pt_right[0].x; right_s[1] = temp_pt_right[0].y; right_s[2] = 1;
right[0] = temp_pt_right[2].x; right[1] = temp_pt_right[2].y; right[2] = 1;
cross_product_f(left, left_s, left_line);
cross_product_f(right, right_s, right_line);
cross_product_f(left_line, right_line, left_right);
// left_right[0] = (float)left_right[0] / (float)left_right[2];
// left_right[1] = (float)left_right[1] / (float)left_right[2];
if(left_line[0] != 0)
x_left = (-left_line[1]*300-left_line[2]) / left_line[0];
if(right_line[0] != 0)
x_right = (-right_line[1]*300-right_line[2]) / right_line[0];
if(x_left < 0)
x_left = 0;
else if(x_left >= WIDTH)
x_left = WIDTH-1;
if(x_right < 0)
x_right = 0;
else if(x_right >= WIDTH)
x_right = WIDTH-1;
if((abs(horizontal_center - (float)left_right[1] / (float)left_right[2]) < vanishing_th) && temp_pt_right[2].x - temp_pt_left[2].x > 0 && x_left < x_right)
{
if(temp_pt_left[0].x < temp_pt_left[2].x)
{
prev_pt_left[0] = temp_pt_left[0];
prev_pt_left[1] = temp_pt_left[1];
prev_pt_left[2] = temp_pt_left[2];
}
if(temp_pt_right[0].x > temp_pt_left[2].x)
{
prev_pt_right[0] = temp_pt_right[0];
prev_pt_right[1] = temp_pt_right[1];
prev_pt_right[2] = temp_pt_right[2];
}
/*
prev_pt_left[2].x = x_left;
prev_pt_left[2].y = 150;
prev_pt_right[2].x = x_right;
prev_pt_right[2].y = 150;
**/
}
pt_left[0] = prev_pt_left[0];
pt_left[1] = prev_pt_left[1];
pt_left[2] = prev_pt_left[2];
pt_right[0] = prev_pt_right[0];
pt_right[1] = prev_pt_right[1];
pt_right[2] = prev_pt_right[2];
}
int main()
{
/* Setup Allegro/AllegroGL */
if (allegro_init())
return 1;
if (install_allegro_gl())
return 1;
if (install_keyboard() < 0)
{
allegro_message("Unable to install keyboard\n");
return 1;
}
if (install_mouse() == -1)
{
allegro_message("Unable to install mouse\n");
return 1;
}
if (install_timer() < 0)
{
allegro_message("Unable to install timers\n");
return 1;
}
/* lock timer */
LOCK_VARIABLE(rotation_counter);
LOCK_FUNCTION(rotation_counter_handler);
/* set desktop resolution */
DESKTOP_W = GetSystemMetrics(SM_CXVIRTUALSCREEN);
DESKTOP_H = GetSystemMetrics(SM_CYVIRTUALSCREEN);
/* get monitor resolution/count */
int monitor_count;
MONITOR *monitors = get_monitors(&monitor_count);
/* generate point data */
PLACE places[POINT_COUNT];
int c;
for (c = 1; c < POINT_COUNT - 1; c++)
{
places[c].x = sin((M_PI * (GLfloat)c) / 10.0f) * 200.0f;
places[c].y = cos((M_PI * (GLfloat)c) / 10.0f) * 200.0f;
}
places[0].x = 0.01;
places[0].y = 200.0f;
places[POINT_COUNT - 1].x = 0.01;
places[POINT_COUNT - 1].y = -200.0f;
/* setup display */
allegro_gl_set(AGL_Z_DEPTH, 8);
allegro_gl_set(AGL_COLOR_DEPTH, 16);
allegro_gl_set(AGL_SUGGEST, AGL_Z_DEPTH | AGL_COLOR_DEPTH);
glDepthFunc(GL_LEQUAL);
if (set_gfx_mode(GFX_OPENGL_WINDOWED_BORDERLESS, DESKTOP_W, DESKTOP_H, 0, 0)) {
set_gfx_mode(GFX_TEXT, 0, 0, 0, 0);
allegro_message("Unable to set graphic mode\n%s\n", allegro_error);
return 1;
}
/* move window so it covers the desktop */
position_window(0, 0);
/* fake information to use if only 1 monitor */
MONITOR fake = {0, 512, 512, 512};
/* setup lighting model */
glShadeModel(GL_FLAT);
glEnable(GL_LIGHT0);
glEnable(GL_COLOR_MATERIAL);
GLfloat light_position[] = { 1.0, 1.0, 1.0, 0.0 };
glLightfv(GL_LIGHT0, GL_POSITION, light_position);
GLfloat ambient[] = { 0.1f, 0.1f, 0.1f };
glLightfv(GL_LIGHT0, GL_AMBIENT, ambient);
int selected = -1; /* the point currently being moved */
GLfloat rotation[3] = {0, 0, 0}; /* the rotation of the mesh */
install_int(rotation_counter_handler, 20); /* install the rotation handler */
/* enter main program loop */
while(!key[KEY_ESC])
{
while (rotation_counter > 0)
{
/* rotate the mesh */
rotation[0] += M_PI / 24.0f;
rotation[1] += M_PI / 16.0f;
rotation[2] += M_PI / 8.0f;
rotation_counter--;
}
/* clear the buffers */
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
/* process monitor 0 */
MONITOR *m = &monitors[0];
/* adjust mouse so its relative to the monitor */
int mx = (mouse_x - m->x) - (m->w / 2);
int my = (mouse_y - m->y) - (m->h / 2);
/* if the left mouse is pushed, find a close point */
if (mouse_b & 1)
{
if (selected == -1)
{
GLfloat distance = 10;
for (c = 0; c < POINT_COUNT; c++)
{
GLfloat dx = mx - places[c].x;
GLfloat dy = my - places[c].y;
GLfloat d = sqrt(dx * dx + dy * dy);
if (d < distance)
{
distance = d;
selected = c;
}
}
}
}
else
selected = -1;
/* move selected point */
if (selected >= 0)
{
places[selected].x = mx;
places[selected].y = my;
}
/* center the viewport on monitor */
glViewport(m->x, DESKTOP_H - m->h - m->y, m->w, m->h);
/* setup viewport projection */
glMatrixMode(GL_PROJECTION);
glLoadIdentity();
gluOrtho2D(-m->w / 2, m->w / 2, m->h / 2, -m->h / 2);
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
/* draw points */
glColor3ub(0, 255, 0);
glBegin(GL_LINE_STRIP);
for (c = 0; c < POINT_COUNT; c++)
{
glVertex2f(places[c].x, places[c].y);
}
glEnd();
glColor3ub(255, 255, 255);
for (c = 0; c < POINT_COUNT; c++)
{
draw_square(places[c].x, places[c].y, 10);
}
/* draw vertical line */
glBegin(GL_LINE_STRIP);
glVertex2f(0.0f, -m->h);
glVertex2f(0.0f, m->h);
glEnd();
/* draw the mouse */
glColor3ub(255, 255, 255);
draw_square(mx, my, 20);
/* process viewport 1 */
/* select second monitor */
if (monitor_count > 1)
{
/* if 2nd monitor exists use it */
m = &monitors[1];
}
else
{
/* use fake monitor */
m = &fake;
}
/* adjust mouse so its relative to the monitor*/
mx = (mouse_x - m->x) - (m->w / 2);
my = (mouse_y - m->y) - (m->h / 2);
/* center the viewport on the monitor*/
glViewport(m->x, DESKTOP_H - m->h - m->y, m->w, m->h);
/* setup viewport projection */
glMatrixMode(GL_PROJECTION);
glLoadIdentity();
gluPerspective(45.0f, (float)m->w / (float)m->h, 0.1f, 2000.0f);
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
/* turn on lighting and depth testing */
glEnable(GL_LIGHTING);
glEnable(GL_DEPTH_TEST);
/* move mesh so its visible */
glTranslatef(0.0f, 0.0f, -1000.0f);
/* rotate mesh */
glRotatef(rotation[0], 1.0f, 0.0f, 0.0f);
glRotatef(rotation[1], 0.0f, 1.0f, 0.0f);
glRotatef(rotation[2], 0.0f, 0.0f, 1.0f);
GLfloat p1[3] = {0, 0, 0};
GLfloat p2[3] = {0, 0, 0};
GLfloat p3[3] = {0, 0, 0};
GLfloat p4[3] = {0, 0, 0};
GLfloat vec1[3];
GLfloat vec2[3];
GLfloat normal[3];
/* draw mesh to screen */
glColor3ub(0, 255, 0);
for (c = 0; c < (POINT_COUNT - 1); c++)
{
GLfloat a1 = 0;
GLfloat a2 = M_PI / 16.0f;
GLfloat d1 = places[c].x;
GLfloat d2 = places[c + 1].x;
p1[0] = sin(a1) * d1; p1[1] = places[c].y; p1[2] = cos(a1) * d1;
p2[0] = sin(a2) * d1; p2[1] = places[c].y; p2[2] = cos(a2) * d1;
p3[0] = sin(a2) * d2; p3[1] = places[c + 1].y; p3[2] = cos(a2) * d2;
p4[0] = sin(a1) * d2; p4[1] = places[c + 1].y; p4[2] = cos(a1) * d2;
buildVector(vec1, p1, p2);
buildVector(vec2, p1, p4);
cross_product_f(vec2[0], vec2[1], vec2[2], vec1[0], vec1[1], vec1[2], &normal[0], &normal[1], &normal[2]);
normalize_vector_f(&normal[0], &normal[1], &normal[2]);
glBegin(GL_QUAD_STRIP);
glNormal3fv(normal);
glVertex3fv(p1);
glVertex3fv(p4);
int s = 0;
for (s = 1; s < 32; s++)
{
a2 = (M_PI * (GLfloat)(s + 1)) / 16.0f;
d1 = places[c].x;
d2 = places[c + 1].x;
copyPoint(p1, p2);
copyPoint(p4, p3);
p2[0] = sin(a2) * d1; p2[1] = places[c].y; p2[2] = cos(a2) * d1;
p3[0] = sin(a2) * d2; p3[1] = places[c + 1].y; p3[2] = cos(a2) * d2;
buildVector(vec1, p1, p2);
buildVector(vec2, p1, p4);
cross_product_f(vec2[0], vec2[1], vec2[2], vec1[0], vec1[1], vec1[2], &normal[0], &normal[1], &normal[2]);
normalize_vector_f(&normal[0], &normal[1], &normal[2]);
glNormal3fv(normal);
glVertex3fv(p2);
glVertex3fv(p3);
}
glEnd();
}
/* turn off lighting and depth testing */
glDisable(GL_DEPTH_TEST);
glDisable(GL_LIGHTING);
/* if not using the fake monitor */
if (m != &fake)
{
/* setup viewport projection */
glMatrixMode(GL_PROJECTION);
glLoadIdentity();
gluOrtho2D(-m->w / 2, m->w / 2, m->h / 2, -m->h / 2);
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
/* draw the mouse */
glColor3ub(255, 255, 255);
draw_square(mx, my, 20);
}
/* flip the contents to the screen */
allegro_gl_flip();
}
free(monitors);
return 0;
}
