static GstStateChangeReturn
gst_cdaudio_change_state (GstElement * element, GstStateChange transition)
{
GstCDAudio *cdaudio;
GstStateChangeReturn ret;
gint res;
cdaudio = GST_CDAUDIO (element);
switch (transition) {
case GST_STATE_CHANGE_NULL_TO_READY:
break;
case GST_STATE_CHANGE_READY_TO_PAUSED:
if ((res = cd_init_device (cdaudio->device)) < 0)
goto init_failed;
cdaudio->cd_desc = res;
/* close tray */
if ((res = cd_close (cdaudio->cd_desc)) < 0)
goto close_failed;
if ((res = cd_stat (cdaudio->cd_desc, &cdaudio->info)) < 0) {
/* we just give a warning here */
GST_ELEMENT_WARNING (cdaudio, LIBRARY, INIT,
("Could not retrieve CD track info."), (NULL));
} else {
debug_track_info (cdaudio);
cdaudio->discid = cddb_discid (cdaudio->cd_desc);
/* FIXME, post message with discid */
}
cdaudio->was_playing = FALSE;
break;
case GST_STATE_CHANGE_PAUSED_TO_PLAYING:
{
if (cdaudio->was_playing)
res = cd_resume (cdaudio->cd_desc);
else
res = cd_play (cdaudio->cd_desc, 1);
if (res < 0)
goto play_failed;
cdaudio->was_playing = TRUE;
g_timer_start (cdaudio->timer);
break;
}
default:
break;
}
ret = GST_ELEMENT_CLASS (parent_class)->change_state (element, transition);
switch (transition) {
case GST_STATE_CHANGE_PLAYING_TO_PAUSED:
if ((res = cd_pause (cdaudio->cd_desc)) < 0)
goto pause_failed;
g_timer_stop (cdaudio->timer);
break;
case GST_STATE_CHANGE_PAUSED_TO_READY:
if ((res = cd_stop (cdaudio->cd_desc)) < 0)
goto stop_failed;
if ((res = cd_finish (cdaudio->cd_desc)) < 0)
goto finish_failed;
cdaudio->cd_desc = -1;
break;
case GST_STATE_CHANGE_READY_TO_NULL:
break;
default:
break;
}
return ret;
/* ERRORS */
init_failed:
{
GST_ELEMENT_ERROR (cdaudio, LIBRARY, INIT,
("Could not init CD device %s. (%d)", cdaudio->device, res), (NULL));
cdaudio->cd_desc = -1;
return GST_STATE_CHANGE_FAILURE;
}
close_failed:
{
GST_ELEMENT_ERROR (cdaudio, LIBRARY, INIT,
("Could not close CD tray for device %s. (%d)", cdaudio->device, res),
(NULL));
return GST_STATE_CHANGE_FAILURE;
}
play_failed:
{
GST_ELEMENT_ERROR (cdaudio, LIBRARY, INIT,
("Could not play CD device %s. (%d)", cdaudio->device, res), (NULL));
return GST_STATE_CHANGE_FAILURE;
}
pause_failed:
{
GST_ELEMENT_ERROR (cdaudio, LIBRARY, INIT,
("Could not pause CD device %s. (%d)", cdaudio->device, res), (NULL));
return GST_STATE_CHANGE_FAILURE;
}
stop_failed:
{
GST_ELEMENT_ERROR (cdaudio, LIBRARY, INIT,
("Could not stop CD device %s. (%d)", cdaudio->device, res), (NULL));
return GST_STATE_CHANGE_FAILURE;
}
finish_failed:
{
GST_ELEMENT_ERROR (cdaudio, LIBRARY, INIT,
("Could not finish CD device %s. (%d)", cdaudio->device, res), (NULL));
return GST_STATE_CHANGE_FAILURE;
}
}
int main(int argc, char *argv[])
{
volumenode *left_node, *right_node;
cd_parameter p;
p.max_length = atoi(argv[9]);
p.max_triangle = atoi(argv[10]);
left_node = cd_init(argv[1], &p);
right_node = cd_init(argv[2], &p);
clock_t start, end;
start = clock();
JAngle robotangle(atof(argv[3]), atof(argv[4]), atof(argv[5]), atof(argv[6]),
atof(argv[7]), atof(argv[8]));
/* JAngle robotangle(0.0, -23.00, 52.500, 16.00, 19.00, 0.00); */
JAngle exangle(0.00, 0.00, 0.00, 0.00, 0.00, 0.00);
TRANS j0_trans = Transform::getTransWorldToBase(exangle);
TRANS j1_trans, j2_trans, j3_trans, j4_trans, j5_trans, j6_trans;
TRANS part_trans, newgun_trans;
Transform::getTransBaseToJoints(robotangle, j1_trans, j2_trans, j3_trans, j4_trans, j5_trans, j6_trans);
newgun_trans = Transform::getTrans6ToGun();
j1_trans = j0_trans * j1_trans;
j2_trans = j0_trans * j2_trans;
j3_trans = j0_trans * j3_trans;
j4_trans = j0_trans * j4_trans;
j5_trans = j0_trans * j5_trans;
j6_trans = j0_trans * j6_trans;
newgun_trans = j6_trans * newgun_trans;
part_trans = Transform::getTransWorldToWorkpiece(exangle);
/* newgun_trans.rot.mem[0][0] = -0.31028574744121573; */
/* newgun_trans.rot.mem[0][1] = -0.76055620150283443; */
/* newgun_trans.rot.mem[0][2] = 0.57033062278859459; */
/* newgun_trans.rot.mem[1][0] = -0.20326345994589520; */
/* newgun_trans.rot.mem[1][1] = 0.63914569443991687; */
/* newgun_trans.rot.mem[1][2] = 0.74173900202816612; */
/* newgun_trans.rot.mem[2][0] = -0.92865855985161161; */
/* newgun_trans.rot.mem[2][1] = 0.11422366494950395; */
/* newgun_trans.rot.mem[2][2] = -0.35291108452389802; */
/* /\* newgun_trans.pos.dx = -600.00000000000000; *\/ */
/* /\* newgun_trans.pos.dy = 429.99999998934874; *\/ */
/* /\* newgun_trans.pos.dz = 2087.0000000021946; *\/ */
/* newgun_trans.pos.dx = 94.771302128496586; */
/* newgun_trans.pos.dy = -505.20287976357167; */
/* newgun_trans.pos.dz = 1222.0426991063437; */
newgun_trans.rot.mem[0][0] = -0.036801385144649992;
newgun_trans.rot.mem[0][1] = -0.74802231932451657;
newgun_trans.rot.mem[0][2] = 0.66265244875711904;
newgun_trans.rot.mem[1][0] = 0.73624785273584337;
newgun_trans.rot.mem[1][1] = 0.42807339281869838;
newgun_trans.rot.mem[1][2] = 0.52411093263024766;
newgun_trans.rot.mem[2][0] = -0.67571055740849195;
newgun_trans.rot.mem[2][1] = 0.50716445079782491;
newgun_trans.rot.mem[2][2] = 0.53497613260186283;
newgun_trans.pos.dx = 159.55751427696521;
newgun_trans.pos.dy = 700.79742899056282;
newgun_trans.pos.dz = 2790.4503015644741;
part_trans.rot.mem[0][0] =-5.1036340138841535e-012;
part_trans.rot.mem[0][1] = 1.0000000000000000;
part_trans.rot.mem[0][2] = -5.1036340138841535e-012;
part_trans.rot.mem[1][0] = -1.0000000000000000;
part_trans.rot.mem[1][1] = -5.1036340138581062e-012;
part_trans.rot.mem[1][2] = -5.1036340139102000e-012;
part_trans.rot.mem[2][0] = -5.1036340138841527e-012;
part_trans.rot.mem[2][1] = -5.1036340139102008e-012;
part_trans.rot.mem[2][2] = 1.0000000000000000;
part_trans.pos.dx = -600.0000000000;
part_trans.pos.dy = 429.99999998934874;
part_trans.pos.dz = 2087.0000000021946;
/* TRANS gun; */
/* TRANS part; */
/* newgun_trans.rot.mem[0][0] = -0.31028574744121573; */
/* newgun_trans.rot.mem[0][1] = -0.76055620150283443; */
/* newgun_trans.rot.mem[0][2] = 0.57033062278859459; */
/* newgun_trans.rot.mem[1][0] = -0.20326345994589520; */
/* newgun_trans.rot.mem[1][1] = 0.63914569443991687; */
/* newgun_trans.rot.mem[1][2] = 0.74173900202816612; */
/* newgun_trans.rot.mem[2][0] = -0.92865855985161161; */
/* newgun_trans.rot.mem[2][1] = 0.11422366494950395; */
/* newgun_trans.rot.mem[2][2] = -0.35291108452389802; */
/* newgun_trans.pos.dx = -600.00000000000000; */
/* newgun_trans.pos.dy = 429.99999998934874; */
/* newgun_trans.pos.dz = 2087.0000000021946; */
/* part_trans.rot.mem[0][0] =-5.1036340138841535e-012; */
/* part_trans.rot.mem[0][1] = 1.0000000000000000; */
/* part_trans.rot.mem[0][2] = -5.1036340138841535e-012; */
/* part_trans.rot.mem[1][0] = -1.0000000000000000; */
/* part_trans.rot.mem[1][1] = -5.1036340138581062e-012; */
/* part_trans.rot.mem[1][2] = -5.1036340139102000e-012; */
/* part_trans.rot.mem[2][0] = -5.1036340138841527e-012; */
/* part_trans.rot.mem[2][1] = -5.1036340139102008e-012; */
/* part_trans.rot.mem[2][2] = 1.0000000000000000; */
/* part_trans.pos.dx = 94.771302128496586; */
/* part_trans.pos.dy = -505.20287976357167; */
/* part_trans.pos.dz = 1222.0426991063437; */
vector3d tmp1, tmp2;
tmp1.x = newgun_trans.pos.dx;
tmp1.y = newgun_trans.pos.dy;
tmp1.z = newgun_trans.pos.dz;
tmp2.x = part_trans.pos.dx;
tmp2.y = part_trans.pos.dy;
tmp2.z = part_trans.pos.dz;
int i;
for (i = 0; i < 1; ++i) {
if (collision_detection2(right_node, part_trans.rot.mem, &tmp2, left_node, newgun_trans.rot.mem, &tmp1)) {
printf("\ncollision detected!\n\n");
} else {
printf("\nno collision!\n\n");
}
/* collision_detection2(left_top_node, right_top_node); */
}
end = clock();
printf("run %d times, time elapsed %f\n", i, (double)(end - start) / CLOCKS_PER_SEC);
#ifdef DEBUG
printf("cdcount = %d\n", cdcount);
printf("triangle_cd_count = %d\n", triangle_cd_count);
#endif
#ifdef SHOW
show_tree_recur(left_node, 0);
printf("rightnode\n");
show_tree_recur(right_node, 0);
#endif
cd_finish(left_node);
cd_finish(right_node);
return 0;
}
