int ServerConnection::accept()
{
char dataformat;
#ifdef SHOWMSG
char tmpstr[255];
#endif
if (sock->accept() < 0)
{
return -1;
}
if (sock->write(&df_local_machine, 1) != 1)
{
//print_error(__LINE__, __FILE__,
// "invalid socket in ServerConnection::accept");
return -1;
}
while (sock->read(&dataformat, 1) < 1)
;
if (dataformat != df_local_machine)
if (df_local_machine != DF_IEEE)
convert_to = DF_IEEE;
#ifdef SHOWMSG
sprintf(tmpstr, "convert: %d", convert_to);
print_comment(__LINE__, __FILE__, tmpstr);
#endif
return 0;
}
main(int argc, char **argv)
{
init( argc, argv );
glutKeyboardFunc(keyEvent);
glutMouseFunc(mouseEvent);
glutMotionFunc(motionEvent);
glutIdleFunc(dispImage);
glutDisplayFunc(dispImage);
print_comment(0);
status = 0;
point_num = 0;
arVideoCapStart();
glutMainLoop();
}
int main(int argc, char **argv)
{
glutInit(&argc, argv);
init( argc, argv );
argSetKeyFunc(keyEvent);
argSetMouseFunc(mouseEvent);
argSetMotionFunc(motionEvent);
argSetDispFunc(dispImage, 1);
print_comment(0);
status = 0;
point_num = 0;
arVideoCapStart();
argMainLoop();
return (0);
}
void print_t_plr(t_list_player *lplr)
{
t_list_player *tmp;
tmp = lplr;
char *buff;
buff= NULL;
while (tmp)
{
ft_printf("__dans BIN __\nnumplr:%u\n", tmp->plr->num_plyr);
print_magic(*tmp->plr, buff);
print_prog_name(*tmp->plr);
print_prog_size(*tmp->plr, buff);
print_comment(*tmp->plr);
print_prog(*tmp->plr);
tmp = tmp->next;
}
}
/**
* @brief Print an XML element.
*/
void XmlWriter::print_element (std::string& xml, const XmlElement* value, const int indent_level)
{
// Prepare indention and opening tag.
std::string in0 (indent_level * indent, ' ');
xml += in0 + "<" + value->tag + print_attributes_list(value->attributes);
// If it's an empty element, close it, and we are done.
if (value->content.size() == 0) {
xml += " />";
return;
}
// If the element only contains a single markup, don't add new lines. However, if it contains
// more data, put each element in a new line.
xml += ">";
if (value->content.size() == 1 && value->content[0]->is_markup()) {
print_markup(xml, xml_value_to_markup(value->content[0].get()));
} else {
std::string in1 ((indent_level + 1) * indent, ' ');
xml += "\n";
for (auto& v : value->content) {
if (v->is_comment()) {
xml += in1;
print_comment(xml, xml_value_to_comment(v.get()));
} else if (v->is_markup()) {
xml += in1;
print_markup(xml, xml_value_to_markup(v.get()));
} else if (v->is_element()) {
print_element(xml, xml_value_to_element(v.get()), indent_level + 1);
} else {
// there are no other cases
assert(0);
}
xml += "\n";
}
xml += in0;
}
xml += "</" + value->tag + ">";
}
ClientConnection::ClientConnection(Host *h, int p, int id, sender_type
s_type,
int retries, double timeout)
{
char dataformat;
#ifdef DEBUG
char tmpstr[255];
#endif
lhost = NULL;
if (h) // host is not local
host = h;
else // host is local (usually DataManagerConnection uses this)
host = lhost = new Host("localhost");
port = p;
sender_id = id;
send_type = s_type;
sock = new Socket(host, port, retries, timeout);
if (get_id() == -1)
return; // connection not established
if (sock->Read(&dataformat, 1) != 1)
{
delete sock;
sock = NULL;
return; // connection failed
}
if (dataformat != df_local_machine)
if (df_local_machine != DF_IEEE)
convert_to = DF_IEEE;
if (sock->write(&df_local_machine, 1) == COVISE_SOCKET_INVALID)
printf("invalid socket in new ClientConnection\n");
#ifdef DEBUG
sprintf(tmpstr, "convert: %d", convert_to);
print_comment(__LINE__, __FILE__, tmpstr);
#endif
}
static void mouseEvent(int button, int state, int x, int y)
{
unsigned char *p, *p1;
int ssx, ssy, eex, eey;
int i, j, k;
if( button == GLUT_RIGHT_BUTTON && state == GLUT_UP ) {
if( status == 0 ) {
arVideoCapStop();
arVideoClose();
if( patt.loop_num > 0 ) {
calc_distortion( &patt, xsize, ysize, dist_factor );
printf("--------------\n");
printf("Center X: %f\n", dist_factor[0]);
printf(" Y: %f\n", dist_factor[1]);
printf("Dist Factor: %f\n", dist_factor[2]);
printf("Size Adjust: %f\n", dist_factor[3]);
printf("--------------\n");
status = 2;
check_num = 0;
print_comment(5);
}
else {
glutDestroyWindow( win );
exit(0);
}
}
else if( status == 1 ) {
if( patt.loop_num == 0 ) {printf("error!!\n"); exit(0);}
patt.loop_num--;
free( patt.point[patt.loop_num] );
free( patt.savedImage[patt.loop_num] );
status = 0;
point_num = 0;
arVideoCapStart();
if( patt.loop_num == 0 ) print_comment(0);
else print_comment(4);
}
}
if( button == GLUT_LEFT_BUTTON && state == GLUT_DOWN ) {
if( status == 1 && point_num < patt.h_num*patt.v_num ) {
sx = ex = x;
sy = ey = y;
p = &(patt.savedImage[patt.loop_num-1][(y*xsize+x)*AR_PIX_SIZE]);
p1 = &(clipImage[0]);
#ifdef AR_PIX_FORMAT_BGRA
k = (255*3 - (*(p+0) + *(p+1) + *(p+2))) / 3;
if( k < thresh ) k = 0;
else k = 255;
*(p1+0) = *(p1+1) = *(p1+2) = k;
#endif
#ifdef AR_PIX_FORMAT_ABGR
k = (255*3 - (*(p+1) + *(p+2) + *(p+3))) / 3;
if( k < thresh ) k = 0;
else k = 255;
*(p1+1) = *(p1+2) = *(p1+3) = k;
#endif
#ifdef AR_PIX_FORMAT_BGR
k = (255*3 - (*(p+0) + *(p+1) + *(p+2))) / 3;
if( k < thresh ) k = 0;
else k = 255;
*(p1+0) = *(p1+1) = *(p1+2) = k;
#endif
#ifdef AR_PIX_FORMAT_RGBA
k = (255*3 - (*(p+0) + *(p+1) + *(p+2))) / 3;
if( k < thresh ) k = 0;
else k = 255;
*(p1+0) = *(p1+1) = *(p1+2) = k;
#endif
#ifdef AR_PIX_FORMAT_RGB
k = (255*3 - (*(p+0) + *(p+1) + *(p+2))) / 3;
if( k < thresh ) k = 0;
else k = 255;
*(p1+0) = *(p1+1) = *(p1+2) = k;
#endif
}
}
if( button == GLUT_LEFT_BUTTON && state == GLUT_UP ) {
if( status == 0 && patt.loop_num < LOOP_MAX ) {
while( (p = (unsigned char *)arVideoGetImage()) == NULL ) {
arUtilSleep(2);
}
#ifdef USE_TEXMAP
patt.savedImage[patt.loop_num] = (unsigned char *)malloc( xsize*tex1Ysize*AR_PIX_SIZE );
#else
patt.savedImage[patt.loop_num] = (unsigned char *)malloc( xsize*ysize*AR_PIX_SIZE );
#endif
if( patt.savedImage[patt.loop_num] == NULL ) exit(0);
p1 = patt.savedImage[patt.loop_num];
for(i=0;i<xsize*ysize*AR_PIX_SIZE;i++) *(p1++) = *(p++);
arVideoCapStop();
patt.point[patt.loop_num] = (CALIB_COORD_T *)malloc( sizeof(CALIB_COORD_T)*patt.h_num*patt.v_num );
if( patt.point[patt.loop_num] == NULL ) exit(0);
patt.loop_num++;
status = 1;
sx = sy = ex= ey = -1;
print_comment(1);
}
else if( status == 1 && point_num == patt.h_num*patt.v_num ) {
status = 0;
point_num = 0;
arVideoCapStart();
printf("### No.%d ###\n", patt.loop_num);
for( j = 0; j < patt.v_num; j++ ) {
for( i = 0; i < patt.h_num; i++ ) {
printf("%2d, %2d: %6.2f, %6.2f\n", i+1, j+1,
patt.point[patt.loop_num-1][j*patt.h_num+i].x_coord,
patt.point[patt.loop_num-1][j*patt.h_num+i].y_coord);
}
}
printf("\n\n");
if( patt.loop_num < LOOP_MAX ) print_comment(4);
else print_comment(6);
}
else if( status == 1 ) {
if( sx < ex ) { ssx = sx; eex = ex; }
else { ssx = ex; eex = sx; }
if( sy < ey ) { ssy = sy; eey = ey; }
else { ssy = ey; eey = sy; }
patt.point[patt.loop_num-1][point_num].x_coord = 0.0;
patt.point[patt.loop_num-1][point_num].y_coord = 0.0;
p = clipImage;
k = 0;
for( j = 0; j < (eey-ssy+1); j++ ) {
for( i = 0; i < (eex-ssx+1); i++ ) {
patt.point[patt.loop_num-1][point_num].x_coord += i * *(p+1);
patt.point[patt.loop_num-1][point_num].y_coord += j * *(p+1);
k += *(p+1);
p += AR_PIX_SIZE;
}
}
if( k != 0 ) {
patt.point[patt.loop_num-1][point_num].x_coord /= k;
patt.point[patt.loop_num-1][point_num].y_coord /= k;
patt.point[patt.loop_num-1][point_num].x_coord += ssx;
patt.point[patt.loop_num-1][point_num].y_coord += ssy;
point_num++;
}
sx = sy = ex= ey = -1;
printf(" # %d/%d\n", point_num, patt.h_num*patt.v_num);
if( point_num == patt.h_num*patt.v_num ) print_comment(2);
}
else if( status == 2 ) {
check_num++;
if( check_num == patt.loop_num ) {
if(patt.loop_num >= 2) {
if( calc_inp(&patt, dist_factor, xsize, ysize, mat) < 0 ) {
printf("Calibration failed.\n");
exit(0);
}
save_param();
}
glutDestroyWindow( win );
exit(0);
}
if( check_num+1 == patt.loop_num ) {
printf("\nLeft Mouse Button: Next Step.\n");
}
else {
printf(" %d/%d.\n", check_num+1, patt.loop_num);
}
}
}
}
int Connection::recv_msg(Message *msg)
{
int bytes_read, bytes_to_read, tmp_read;
char *read_buf_ptr;
int *int_read_buf;
int data_length;
char *read_data;
#ifdef SHOWMSG
char retstr[255];
char tmp_str[255];
#endif
#ifdef CRAY
int tmp_buf[4];
#endif
msg->sender = msg->length = 0;
msg->send_type = UNDEFINED;
msg->type = EMPTY;
msg->conn = this;
message_to_do = 0;
if (!sock)
return 0;
/// aw: this looks like stdin/stdout sending
if (send_type == STDINOUT)
{
// print_comment(__LINE__, __FILE__, "in send_type == STDINOUT");
do
{
#ifdef _WIN32
unsigned long tru = 1;
ioctlsocket(sock->get_id(), FIONBIO, &tru);
#else
fcntl(sock->get_id(), F_SETFL, O_NDELAY); // this is non-blocking
#endif
bytes_read = sock->read(read_buf, READ_BUFFER_SIZE);
if (bytes_read < 0)
{
// print_comment(__LINE__, __FILE__, "bytes read == -1");
// perror("error after STDINOUT read");
// sprintf(retstr, "errno: %d", errno);
// print_comment(__LINE__, __FILE__, retstr);
msg->type = SOCKET_CLOSED;
return 0;
}
else
{
read_buf[bytes_read] = '\0';
cout << read_buf;
cout.flush();
sprintf(&read_buf[bytes_read], "bytes read: %d", bytes_read);
// print_comment(__LINE__, __FILE__, read_buf);
}
} while (bytes_read > 0);
msg->type = STDINOUT_EMPTY;
return 0;
}
////// this is sending to anything else than stdin/out
/* reading all in one call avoids the expensive read system call */
while (bytes_to_process < 16)
{
tmp_read = sock->Read(read_buf + bytes_to_process, READ_BUFFER_SIZE - bytes_to_process);
if (tmp_read == 0)
return (0);
if (tmp_read < 0)
{
msg->type = SOCKET_CLOSED;
msg->conn = this;
return tmp_read;
}
bytes_to_process += tmp_read;
}
read_buf_ptr = read_buf;
while (1)
{
int_read_buf = (int *)read_buf_ptr;
#ifdef SHOWMSG
sprintf(tmp_str, "read_buf: %d %d %d %d", int_read_buf[0],
int_read_buf[1], int_read_buf[2], int_read_buf[3]);
print_comment(__LINE__, __FILE__, tmp_str);
#endif
#ifdef CRAY
#ifdef _CRAYT3E
converter.exch_to_int_array(read_buf_ptr, tmp_buf, 4);
// conv_array_int_i4t8(int_read_buf, tmp_buf, 4, START_EVEN);
#else
conv_array_int_i4c8(int_read_buf, tmp_buf, 4, START_EVEN);
#endif
msg->sender = tmp_buf[0];
msg->send_type = sender_type(tmp_buf[1]);
msg->type = covise_msg_type(tmp_buf[2]);
msg->length = tmp_buf[3];
#else
#ifdef BYTESWAP
swap_bytes((unsigned int *)int_read_buf, 4);
#endif
msg->sender = int_read_buf[0];
msg->send_type = sender_type(int_read_buf[1]);
msg->type = covise_msg_type(int_read_buf[2]);
msg->length = int_read_buf[3];
// sprintf(retstr, "msg header: sender %d, sender_type %d, covise_msg_type %d, length %d",
// msg->sender, msg->send_type, msg->type, msg->length);
// print_comment(__LINE__, __FILE__, retstr);
#endif
#ifdef SHOWMSG
sprintf(tmp_str, "recv: s: %d st: %d mt: %s l: %d",
msg->sender, msg->send_type,
(msg->type < 0 || msg->type > LAST_DUMMY_MESSAGE) ? (msg->type == -1 ? "EMPTY" : "(invalid)") : covise_msg_types_array[msg->type],
msg->length);
print_comment(__LINE__, __FILE__, tmp_str);
#endif
bytes_to_process -= 4 * SIZEOF_IEEE_INT;
#ifdef SHOWMSG
sprintf(retstr, "bytes_to_process %d bytes, msg->length %d", bytes_to_process, msg->length);
print_comment(__LINE__, __FILE__, retstr);
#endif
read_buf_ptr += 4 * SIZEOF_IEEE_INT;
if (msg->length > 0) // if msg->length == 0, no data will be received
{
if (msg->data)
{
delete[] msg -> data;
msg->data = NULL;
}
// bring message data space to 16 byte alignment
data_length = msg->length + ((msg->length % 16 != 0) * (16 - msg->length % 16));
msg->data = new char[data_length];
if (msg->length > bytes_to_process)
{
bytes_read = bytes_to_process;
#ifdef SHOWMSG
sprintf(retstr, "bytes_to_process %d bytes, msg->length %d", bytes_to_process, msg->length);
print_comment(__LINE__, __FILE__, retstr);
#endif
if (bytes_read != 0)
memcpy(msg->data, read_buf_ptr, bytes_read);
bytes_to_process = 0;
bytes_to_read = msg->length - bytes_read;
read_data = &msg->data[bytes_read];
while (bytes_read < msg->length)
{
#ifdef SHOWMSG
sprintf(retstr, "bytes_to_process %d bytes", bytes_to_process);
print_comment(__LINE__, __FILE__, retstr);
sprintf(retstr, "bytes_to_read %d bytes", bytes_to_read);
print_comment(__LINE__, __FILE__, retstr);
sprintf(retstr, "bytes_read %d bytes", bytes_read);
print_comment(__LINE__, __FILE__, retstr);
#endif
tmp_read = sock->read(read_data, bytes_to_read);
// covise_time->mark(__LINE__, "nach weiterem sock->read(read_buf)");
bytes_read += tmp_read;
bytes_to_read -= tmp_read;
read_data = &msg->data[bytes_read];
}
#ifdef SHOWMSG
print_comment(__LINE__, __FILE__, "message_to_do = 0");
#endif
// covise_time->mark(__LINE__, " recv_msg: Ende");
return msg->length;
}
else if (msg->length < bytes_to_process)
{
memcpy(msg->data, read_buf_ptr, msg->length);
bytes_to_process -= msg->length;
read_buf_ptr += msg->length;
#ifdef SHOWMSG
sprintf(retstr, "bytes_to_process %d bytes, msg->length %d", bytes_to_process, msg->length);
print_comment(__LINE__, __FILE__, retstr);
#endif
memmove(read_buf, read_buf_ptr, bytes_to_process);
read_buf_ptr = read_buf;
while (bytes_to_process < 16)
{
#ifdef SHOWMSG
sprintf(retstr, "bytes_to_process %d bytes, msg->length %d", bytes_to_process, msg->length);
print_comment(__LINE__, __FILE__, retstr);
#endif
bytes_to_process += sock->read(&read_buf_ptr[bytes_to_process],
READ_BUFFER_SIZE - bytes_to_process);
}
message_to_do = 1;
#ifdef SHOWMSG
print_comment(__LINE__, __FILE__, "message_to_do = 1");
#endif
// covise_time->mark(__LINE__, " recv_msg: Ende");
return msg->length;
}
else
{
memcpy(msg->data, read_buf_ptr, bytes_to_process);
bytes_to_process = 0;
#ifdef SHOWMSG
print_comment(__LINE__, __FILE__, "message_to_do = 0");
#endif
// covise_time->mark(__LINE__, " recv_msg: Ende");
return msg->length;
}
}
else //msg->length == 0, no data will be received
{
if (msg->data)
{
delete[] msg -> data;
msg->data = NULL;
}
if (msg->length < bytes_to_process)
{
memmove(read_buf, read_buf_ptr, bytes_to_process);
read_buf_ptr = read_buf;
while (bytes_to_process < 16)
{
#ifdef SHOWMSG
sprintf(retstr, "bytes_to_process %d bytes, msg->length %d", bytes_to_process, msg->length);
print_comment(__LINE__, __FILE__, retstr);
#endif
bytes_to_process += sock->read(&read_buf_ptr[bytes_to_process],
READ_BUFFER_SIZE - bytes_to_process);
}
message_to_do = 1;
#ifdef SHOWMSG
print_comment(__LINE__, __FILE__, "message_to_do = 1");
#endif
}
return 0;
}
}
}
int Connection::send_msg(const Message *msg)
{
int retval = 0, tmp_bytes_written;
char write_buf[WRITE_BUFFER_SIZE];
int *write_buf_int;
if (!sock)
return 0;
#ifdef SHOWMSG
char tmp_str[256];
sprintf(tmp_str, "send: s: %d st: %d mt: %s l: %d",
sender_id, send_type, covise_msg_types_array[msg->type], msg->length);
print_comment(__LINE__, __FILE__, tmp_str);
#endif
#ifdef CRAY
int tmp_buf[4];
tmp_buf[0] = sender_id;
tmp_buf[1] = send_type;
tmp_buf[2] = msg->type;
tmp_buf[3] = msg->length;
#ifdef _CRAYT3E
converter.int_array_to_exch(tmp_buf, write_buf, 4);
#else
conv_array_int_c8i4(tmp_buf, (int *)write_buf, 4, START_EVEN);
#endif
#else
write_buf_int = (int *)write_buf;
write_buf_int[0] = sender_id;
write_buf_int[1] = send_type;
write_buf_int[2] = msg->type;
write_buf_int[3] = msg->length;
swap_bytes((unsigned int *)write_buf_int, 4);
#endif
if (msg->length == 0)
retval = sock->write(write_buf, 4 * SIZEOF_IEEE_INT);
else
{
#ifdef DEBUG
sprintf(tmp_str, "msg->length: %d", msg->length);
print_comment(__LINE__, __FILE__, tmp_str);
#endif
if (msg->length < WRITE_BUFFER_SIZE - 4 * SIZEOF_IEEE_INT)
{
memcpy(&write_buf[4 * SIZEOF_IEEE_INT], msg->data, msg->length);
retval = sock->write(write_buf, 4 * SIZEOF_IEEE_INT + msg->length);
}
else
{
memcpy(&write_buf[16], msg->data, WRITE_BUFFER_SIZE - 4 * SIZEOF_IEEE_INT);
#ifdef SHOWMSG
sprintf(tmp_str, "write_buf: %d %d %d %d", write_buf_int[0],
write_buf_int[1], write_buf_int[2], write_buf_int[3]);
print_comment(__LINE__, __FILE__, tmp_str);
#endif
retval = sock->write(write_buf, WRITE_BUFFER_SIZE);
#ifdef CRAY
tmp_bytes_written = sock->writea(&msg->data[WRITE_BUFFER_SIZE - 4 * SIZEOF_IEEE_INT],
msg->length - (WRITE_BUFFER_SIZE - 4 * SIZEOF_IEEE_INT));
#else
tmp_bytes_written = sock->write(&msg->data[WRITE_BUFFER_SIZE - 4 * SIZEOF_IEEE_INT],
msg->length - (WRITE_BUFFER_SIZE - 4 * SIZEOF_IEEE_INT));
#endif
if (tmp_bytes_written == COVISE_SOCKET_INVALID)
return COVISE_SOCKET_INVALID;
else
retval += tmp_bytes_written;
}
}
return retval;
}
static void mouseEvent(int button, int state, int x, int y)
{
AR2VideoBufferT *buff;
unsigned char *p, *p1;
int ssx, ssy, eex, eey;
int i, j, k;
char line[256];
if( x < 0 ) x = 0;
if( x >= xsize ) x = xsize-1;
if( y < 0 ) y = 0;
if( y >= ysize ) y = ysize-1;
x *= SCALE;
y *= SCALE;
if( button == GLUT_RIGHT_BUTTON && state == GLUT_UP ) {
if( status == 0 ) {
arVideoCapStop();
arVideoClose();
if( patt.loop_num > 0 ) {
calc_distortion( &patt, xsize, ysize, aspect_ratio, dist_factor, dist_function_version );
ARLOG("--------------\n");
if (dist_function_version == 3) {
ARLOG("Center X: %f\n", dist_factor[0]);
ARLOG(" Y: %f\n", dist_factor[1]);
ARLOG("Size Adjust: %f\n", dist_factor[2]);
ARLOG("Aspect Ratio: %f\n", dist_factor[3]);
ARLOG("Dist Factor1: %f\n", dist_factor[4]);
ARLOG("Dist Factor2: %f\n", dist_factor[5]);
} else if (dist_function_version == 2) {
ARLOG("Center X: %f\n", dist_factor[0]);
ARLOG(" Y: %f\n", dist_factor[1]);
ARLOG("Size Adjust: %f\n", dist_factor[2]);
ARLOG("Dist Factor1: %f\n", dist_factor[3]);
ARLOG("Dist Factor2: %f\n", dist_factor[4]);
} else if (dist_function_version == 1) {
ARLOG("Center X: %f\n", dist_factor[0]);
ARLOG(" Y: %f\n", dist_factor[1]);
ARLOG("Size Adjust: %f\n", dist_factor[2]);
ARLOG("Dist Factor: %f\n", dist_factor[3]);
}
ARLOG("--------------\n");
status = 2;
check_num = 0;
print_comment(5);
}
else {
exit(0);
}
}
else if( status == 1 ) {
if( patt.loop_num == 0 ) {ARLOGe("error!!\n"); exit(0);}
patt.loop_num--;
free( patt.point[patt.loop_num] );
free( patt.savedImage[patt.loop_num] );
status = 0;
point_num = 0;
arVideoCapStart();
if( patt.loop_num == 0 ) print_comment(0);
else print_comment(4);
}
}
if( button == GLUT_LEFT_BUTTON && state == GLUT_DOWN ) {
if( status == 1 && point_num < patt.h_num*patt.v_num ) {
sx = ex = x;
sy = ey = y;
p = &(patt.savedImage[patt.loop_num-1][(y*xsize+x)*pixelSize]);
p1 = &(clipImage[0]);
if (pixelFormat == AR_PIXEL_FORMAT_BGRA || pixelFormat == AR_PIXEL_FORMAT_RGBA) {
k = (255*3 - (*(p+0) + *(p+1) + *(p+2))) / 3;
if( k < thresh ) k = 0;
else k = 255;
*(p1+0) = *(p1+1) = *(p1+2) = k;
}
else if (pixelFormat == AR_PIXEL_FORMAT_ARGB || pixelFormat == AR_PIXEL_FORMAT_ABGR) {
k = (255*3 - (*(p+1) + *(p+2) + *(p+3))) / 3;
if( k < thresh ) k = 0;
else k = 255;
*(p1+1) = *(p1+2) = *(p1+3) = k;
}
else if (pixelFormat == AR_PIXEL_FORMAT_BGR || pixelFormat == AR_PIXEL_FORMAT_RGB) {
k = (255*3 - (*(p+0) + *(p+1) + *(p+2))) / 3;
if( k < thresh ) k = 0;
else k = 255;
*(p1+0) = *(p1+1) = *(p1+2) = k;
}
else if (pixelFormat == AR_PIXEL_FORMAT_MONO || pixelFormat == AR_PIXEL_FORMAT_420v || pixelFormat == AR_PIXEL_FORMAT_420f) {
k = 255 - *p;
if( k < thresh ) k = 0;
else k = 255;
*p1 = k;
}
else if (pixelFormat == AR_PIXEL_FORMAT_2vuy) {
k = 255 - *(p+1);
if( k < thresh ) k = 0;
else k = 255;
*(p1+1) = k;
}
else if (pixelFormat == AR_PIXEL_FORMAT_yuvs) {
k = 255 - *p;
if( k < thresh ) k = 0;
else k = 255;
*p1 = k;
}
}
}
if( button == GLUT_LEFT_BUTTON && state == GLUT_UP ) {
if( status == 0 && patt.loop_num < LOOP_MAX ) {
while (!(buff = arVideoGetImage()) || !buff->fillFlag) arUtilSleep(2);
p = buff->buff;
patt.savedImage[patt.loop_num] = (unsigned char *)malloc( xsize*ysize*pixelSize );
if( patt.savedImage[patt.loop_num] == NULL ) exit(0);
p1 = patt.savedImage[patt.loop_num];
for(i=0;i<xsize*ysize*pixelSize;i++) *(p1++) = *(p++);
arVideoCapStop();
patt.point[patt.loop_num] = (CALIB_COORD_T *)malloc( sizeof(CALIB_COORD_T)*patt.h_num*patt.v_num );
if( patt.point[patt.loop_num] == NULL ) exit(0);
patt.loop_num++;
status = 1;
sx = sy = ex= ey = -1;
print_comment(1);
}
else if( status == 1 && point_num == patt.h_num*patt.v_num ) {
status = 0;
point_num = 0;
arVideoCapStart();
ARLOG("### No.%d ###\n", patt.loop_num);
for( j = 0; j < patt.v_num; j++ ) {
for( i = 0; i < patt.h_num; i++ ) {
ARLOG("%2d, %2d: %6.2f, %6.2f\n", i+1, j+1,
patt.point[patt.loop_num-1][j*patt.h_num+i].x_coord,
patt.point[patt.loop_num-1][j*patt.h_num+i].y_coord);
}
}
ARLOG("\n\n");
if( patt.loop_num < LOOP_MAX ) print_comment(4);
else print_comment(6);
}
else if( status == 1 ) {
if( sx < ex ) { ssx = sx; eex = ex; }
else { ssx = ex; eex = sx; }
if( sy < ey ) { ssy = sy; eey = ey; }
else { ssy = ey; eey = sy; }
patt.point[patt.loop_num-1][point_num].x_coord = 0.0;
patt.point[patt.loop_num-1][point_num].y_coord = 0.0;
p = clipImage;
k = 0;
for( j = 0; j < (eey-ssy+1); j++ ) {
for( i = 0; i < (eex-ssx+1); i++ ) {
if( pixelSize == 1 ) {
patt.point[patt.loop_num-1][point_num].x_coord += i * *p;
patt.point[patt.loop_num-1][point_num].y_coord += j * *p;
k += *p;
}
else {
patt.point[patt.loop_num-1][point_num].x_coord += i * *(p+1);
patt.point[patt.loop_num-1][point_num].y_coord += j * *(p+1);
k += *(p+1);
}
p += pixelSize;
}
}
if( k != 0 ) {
patt.point[patt.loop_num-1][point_num].x_coord /= k;
patt.point[patt.loop_num-1][point_num].y_coord /= k;
patt.point[patt.loop_num-1][point_num].x_coord += ssx;
patt.point[patt.loop_num-1][point_num].y_coord += ssy;
point_num++;
}
sx = sy = ex= ey = -1;
ARLOG(" # %d/%d\n", point_num, patt.h_num*patt.v_num);
if( point_num == patt.h_num*patt.v_num ) print_comment(2);
}
else if( status == 2 ) {
check_num++;
if( check_num == patt.loop_num ) {
if(patt.loop_num >= 2) {
if( calc_inp(&patt, dist_factor, xsize, ysize, mat, dist_function_version) < 0 ) {
ARLOGe("Calibration failed.\n");
exit(0);
}
save_param();
if (dist_function_version == 3) {
printf("Do you want to repeat again?");
scanf("%s", line);
if( line[0] == 'y' ) {
aspect_ratio *= mat[0][0] / mat[1][1];
ARLOG("New aspect ratio = %f\n", aspect_ratio);
calc_distortion( &patt, xsize, ysize, aspect_ratio, dist_factor, dist_function_version );
ARLOG("--------------\n");
ARLOG("Center X: %f\n", dist_factor[0]);
ARLOG(" Y: %f\n", dist_factor[1]);
ARLOG("Size Adjust: %f\n", dist_factor[2]);
ARLOG("Aspect Ratio: %f\n", dist_factor[3]);
ARLOG("Dist Factor1: %f\n", dist_factor[4]);
ARLOG("Dist Factor2: %f\n", dist_factor[5]);
ARLOG("--------------\n");
status = 2;
check_num = 0;
print_comment(5);
return;
}
}
}
exit(0);
}
if( check_num+1 == patt.loop_num ) {
ARLOG("\nLeft Mouse Button: Next Step.\n");
}
else {
ARLOG(" %d/%d.\n", check_num+1, patt.loop_num);
}
}
}
return;
}