NumberedPoses generatePoses(tf::Pose& object_center,const int circle_points, const int circle_rings, const double pitch_step, const double radius)
{
std::vector<NumberedPose> poses;
const int pose_id_max = circle_points * circle_rings;
int pose_id = 0;
double angle_step = 2* M_PI / circle_points;
double pitch_angle_lower = pitch_step * -1 * floor(circle_rings/2);
for (double yaw_pos = 0; yaw_pos < circle_points; yaw_pos++)
{
for (int altitude_pos = 0; altitude_pos < circle_rings; altitude_pos++)
{
pose_id++;
// Pitch angle
double angle_pitch = pitch_angle_lower + (pitch_step * (double)altitude_pos);
double angle_yaw_offset = (angle_step / circle_rings)*altitude_pos;
double angle_yaw = M_PI + angle_step * yaw_pos + angle_yaw_offset;
NumberedPose pose = generatePose(object_center, pose_id, pose_id_max, radius, angle_pitch, angle_yaw);
poses.push_back(pose);
}
}
return poses;
}
Motion *SKFRegion::generateMotion() {
float framedt = canvas->getMotion( 0 )->getFrameTime();
std::vector<Pose *> poseList;
for( unsigned i = 0; i < selectionPath.size(); i++ ) {
poseList.push_back( generatePose( selectionPath[i] ) );
}
return new Motion( poseList, selectionTimes, framedt );
}
void SKFRegion::mouseMove( Point2D p ) {
if( drawingBounds ) {
return;
}
if( !drawingBounds && dragging )
{
displayPose = new CanvasPose( *generatePose( p ), p, *canvas );
canvas->displayPose( *displayPose );
//delete displayPose;
}
if( drawingMotion ) {//!poses.empty() && dragging && contains( p ) && canvas->isDragging() ) {
selectionPath.push_back( p );
selectionTimes.push_back( clock() );
}
}
void SKFRegion::mouseDown( int button, Point2D p, bool doubleClick ) {
if( contains( p ) ) {
dragging = true;
if( selectionPath.contains( p ) ) {
if( selectionPath.size() == 1 ) {
displayPose->setDragging();
canvas->addElement( displayPose );
displayPose = NULL;
selectionPath.erase( selectionPath.begin(), selectionPath.end() );
selectionTimes.erase( selectionTimes.begin(), selectionTimes.end() );
} else if( selectionPath.size() > 1 ) {
tAdd = clock();
mAdd = generateMotion();
pAdd = p;
canvas->addMotion( mAdd );
selectionPath.erase( selectionPath.begin(), selectionPath.end() );
selectionTimes.erase( selectionTimes.begin(), selectionTimes.end() );
}
} else if( !poses.empty() ) {
selectionPath.erase( selectionPath.begin(), selectionPath.end() );
selectionTimes.erase( selectionTimes.begin(), selectionTimes.end() );
selectionPath.push_back( p );
selectionTimes.push_back( clock() );
//delete displayPose;
displayPose = new CanvasPose( *generatePose( p ), p, *canvas );
if( button == CanvasItem::MOUSE_BTN_RIGHT )
{
drawingMotion = true;
}
}
canvas->displayPose( *displayPose );
}
}
