geometry_msgs::Pose Pcl_grabing::getBlockPose()
{
    geometry_msgs::Pose pose;
    pose.position.x = BlockTopCentroid[0];
    pose.position.y = BlockTopCentroid[1];
    pose.position.z = BlockTopCentroid[2];
    
    Eigen::Vector3f x_axis(1,0,0);
    double sn_theta = Block_Major.dot(x_axis);
    double theta = acos(sn_theta);
    
    pose.orientation.x = 0;
    pose.orientation.y = 0;
    pose.orientation.z = sin(theta/2);
    pose.orientation.w = cos(theta/2);
    
    return pose;
}
Beispiel #2
0
  tf::Stamped<tf::Pose> getHandlePose(const door_msgs::Door& door, int side)
  {
    Vector x_axis(1,0,0);
    
    double dist = sqrt(pow(door.frame_p1.x - door.handle.x,2)+pow(door.frame_p1.y - door.handle.y,2));
    if(door.hinge == door_msgs::Door::HINGE_P2)
    {
      dist = sqrt(pow(door.frame_p2.x - door.handle.x,2)+pow(door.frame_p2.y - door.handle.y,2));
    }
    double angle = getDoorAngle(door);

    // get hinge point
    Vector hinge, frame_vec;
    if (door.hinge == door_msgs::Door::HINGE_P1){
      hinge = Vector(door.door_p1.x, door.door_p1.y, door.door_p1.z);
      frame_vec = Vector(door.frame_p2.x - door.frame_p1.x, door.frame_p2.y - door.frame_p1.y, door.frame_p2.z - door.frame_p1.z);
    }
    else if (door.hinge == door_msgs::Door::HINGE_P2){
      hinge = Vector(door.door_p2.x, door.door_p2.y, door.door_p2.z);
      frame_vec = Vector(door.frame_p1.x - door.frame_p2.x, door.frame_p1.y - door.frame_p2.y, door.frame_p1.z - door.frame_p2.z);
    }

    // get gripper pos
    frame_vec.Normalize();
    frame_vec = frame_vec * dist;
    Rotation rot_angle = Rotation::RotZ(angle);
    Vector handle_pos = hinge + (rot_angle * frame_vec);
    
    tf::Stamped<tf::Pose> handle_pose;
    Vector normal_frame = getFrameNormal(door);
    if(side == -1)
    {
      normal_frame = -normal_frame;
    }
    handle_pose.frame_id_ = door.header.frame_id;
    handle_pose.stamp_ = door.header.stamp;
    handle_pose.setOrigin( tf::Vector3(handle_pos(0), handle_pos(1), gripper_height));
    handle_pose.setRotation( tf::createQuaternionFromRPY(getVectorAngle(x_axis, normal_frame)+angle, 0, 0) ); 
    
    tf::Pose gripper_rotate(tf::createQuaternionFromRPY(0.0,0.0,M_PI/2.0),tf::Vector3(0.0,0.0,0.0));
    handle_pose.mult(handle_pose,gripper_rotate);
    return handle_pose;  
  }
Beispiel #3
0
 tf::Stamped<tf::Pose> getRobotPose(const door_msgs::Door& door, double dist)
 {
   Vector x_axis(1,0,0);
   
   // get vector to center of frame
   Vector frame_1(door.frame_p1.x, door.frame_p1.y, door.frame_p1.z);
   Vector frame_2(door.frame_p2.x, door.frame_p2.y, door.frame_p2.z);
   Vector frame_center = (frame_2 + frame_1)/2.0;
   
   // get robot pos
   Vector frame_normal = getFrameNormal(door);
   Vector robot_pos = frame_center + (frame_normal * dist);
   tf::Stamped<tf::Pose> robot_pose;
   robot_pose.frame_id_ = door.header.frame_id;
   robot_pose.stamp_ = door.header.stamp;
   robot_pose.setOrigin( tf::Vector3(robot_pos(0), robot_pos(1), robot_pos(2)));
   robot_pose.setRotation( tf::createQuaternionFromRPY(getVectorAngle(x_axis, frame_normal), 0, 0) ); 
   
   return robot_pose;  
 }
Beispiel #4
0
void renderer::Render(void) {
	const int N_repeat = 13;

	glm::vec3 x_axis(1.0, 0.0, 0.0);
	glm::vec3 y_axis(0.0, 1.0, 0.0);
	glm::vec3 z_axis(0.0, 0.0, 1.0);

	glm::vec3 cam_pos(0, 0, 0);
	glm::vec3 cam_up = y_axis;
	glm::vec3 cam_right = x_axis;
	glm::vec3 cam_front = -z_axis; //oikeakatinen koordinaatisto
	glm::mat4 P = glm::lookAt(cam_pos, cam_pos + cam_front, cam_up);


	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
	glUseProgram(programID);
	glBindVertexArray(VertexArrayID);
	glActiveTexture(GL_TEXTURE0);
	glBindTexture(GL_TEXTURE_2D, Texture);
	// Set our "myTextureSampler" sampler to user Texture Unit 0
	glUniform1i(TextureID, 0);

	int width, height;
	glfwGetFramebufferSize(window, &width, &height);
	glm::mat4 V = glm::ortho(-1.0f, 1.0f, -1.0f*height / width, 1.0f*height / width);
	glm::mat4 VP = V*P;
	for (int j = 0; j < N_repeat; j++) {
		glm::mat4 M =
			glm::rotate(alpha + j*2.0f*3.14159265f / N_repeat, z_axis)*
			glm::translate(glm::vec3(0.0, 0.3, 0));
			//glm::scale(glm::vec3(0.75));
		MVP = VP*M;
		glUniformMatrix4fv(MVP_MatrixID, 1, GL_FALSE, &MVP[0][0]);
		glVertexAttrib3f(VERTEX_POSITION, 0.0f, 0.0f, 0.0f);
		glPointSize(width / 15);
		glDrawArrays(GL_POINTS, 0, 12);
	}
	glfwSwapBuffers(window);
	alpha += 0.005f;
}
Beispiel #5
0
osg::Geometry* NaturalCubicSpline::drawTangentCoordinateSystems()
{
    osg::Vec4 vec;               // Ortsvektor
    osg::Vec4 x_axis(1,0,0,1);
    osg::Vec4 y_axis(0,1,0,1);
    osg::Vec4 z_axis(0,0,1,1);
    osg::Matrix mat;

    osg::ref_ptr<osg::Vec4Array>tangent_vertices = new osg::Vec4Array;

    for(int i = 0; i < _vertices->getNumElements(); i++)
    {
        vec = osg::Vec4(
                (*_vertices)[i].x(),
                (*_vertices)[i].y(),
                (*_vertices)[i].z(),
                1
                );

        mat = _matrices[i];

        tangent_vertices->push_back(vec);
        tangent_vertices->push_back(mat*x_axis);

        tangent_vertices->push_back(vec);
        tangent_vertices->push_back(mat*y_axis);

        tangent_vertices->push_back(vec);
        tangent_vertices->push_back(mat*z_axis);
    }

    osg::ref_ptr<osg::Geometry> tangent_geo = new osg::Geometry;
    tangent_geo->setVertexArray( tangent_vertices ); 
    tangent_geo->addPrimitiveSet( 
            new osg::DrawArrays(GL_LINES, 0, tangent_vertices->getNumElements()) );

    return tangent_geo.release();
}
Beispiel #6
0
  tf::Stamped<tf::Pose> getGripperPose(const door_msgs::Door& door, double angle, double dist, int side)
  {
    Vector x_axis(1,0,0);
    
    // get hinge point
    Vector hinge, frame_vec;
    if (door.hinge == door_msgs::Door::HINGE_P1){
      hinge = Vector(door.door_p1.x, door.door_p1.y, door.door_p1.z);
      frame_vec = Vector(door.frame_p2.x - door.frame_p1.x, door.frame_p2.y - door.frame_p1.y, door.frame_p2.z - door.frame_p1.z);
    }
    else if (door.hinge == door_msgs::Door::HINGE_P2){
      hinge = Vector(door.door_p2.x, door.door_p2.y, door.door_p2.z);
      frame_vec = Vector(door.frame_p1.x - door.frame_p2.x, door.frame_p1.y - door.frame_p2.y, door.frame_p1.z - door.frame_p2.z);
    }

    // get gripper pos
    frame_vec.Normalize();
    frame_vec = frame_vec * dist;
    Rotation rot_angle = Rotation::RotZ(angle);
    Vector gripper_pos = hinge + (rot_angle * frame_vec);
    
    tf::Stamped<tf::Pose> gripper_pose;
    Vector normal_frame = getFrameNormal(door);
    gripper_pose.frame_id_ = door.header.frame_id;
    gripper_pose.stamp_ = door.header.stamp;
    gripper_pose.setOrigin( tf::Vector3(gripper_pos(0), gripper_pos(1), gripper_height));
    if(side == door_msgs::DoorCmd::PULL)
    {
      gripper_pose.setRotation( tf::createQuaternionFromRPY(getVectorAngle(-x_axis, normal_frame)+angle, 0, 0) ); 
    }
    else
    {
      gripper_pose.setRotation( tf::createQuaternionFromRPY(getVectorAngle(x_axis, normal_frame)+angle, 0, 0) ); 
    }
    return gripper_pose;  
  }
    static int moment_of_inertia_features(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr& inputCluster) {
    
      pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ> ());
      
      // Get cloud from publisher
      pcl::copyPointCloud(*inputCluster, *cloud);
      
      std::cout << "Hello1" << std::endl; //*
      pcl::MomentOfInertiaEstimation <pcl::PointXYZ> feature_extractor;
      std::cout << "Hello2" << std::endl; //*
      feature_extractor.setInputCloud (cloud);
      std::cout << "Hello3" << std::endl; //*
      feature_extractor.compute ();
      std::cout << "Hello4" << std::endl; //*
      
      std::vector <float> moment_of_inertia;
      std::vector <float> eccentricity;
      pcl::PointXYZ min_point_AABB;
      pcl::PointXYZ max_point_AABB;
      pcl::PointXYZ min_point_OBB;
      pcl::PointXYZ max_point_OBB;
      pcl::PointXYZ position_OBB;
      Eigen::Matrix3f rotational_matrix_OBB;
      float major_value, middle_value, minor_value;
      Eigen::Vector3f major_vector, middle_vector, minor_vector;
      Eigen::Vector3f mass_center;
      
      feature_extractor.getMomentOfInertia (moment_of_inertia);
      std::cout << "Hello5" << std::endl; //*
      feature_extractor.getEccentricity (eccentricity);
      std::cout << "Hello6" << std::endl; //*
      feature_extractor.getAABB (min_point_AABB, max_point_AABB);
      std::cout << "Hello7" << std::endl; //*
      feature_extractor.getOBB (min_point_OBB, max_point_OBB, position_OBB, rotational_matrix_OBB);
      std::cout << "Hello8" << std::endl; //*
      feature_extractor.getEigenValues (major_value, middle_value, minor_value);
      std::cout << "Hello9" << std::endl; //*
      feature_extractor.getEigenVectors (major_vector, middle_vector, minor_vector);
      std::cout << "Hello10" << std::endl; //*
      feature_extractor.getMassCenter (mass_center);
      std::cout << "Hello11" << std::endl; //*
      
      boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));
      viewer->setBackgroundColor (0, 0, 0);
      viewer->addCoordinateSystem (1.0);
      viewer->initCameraParameters ();
      viewer->addPointCloud<pcl::PointXYZ> (cloud, "sample cloud");
      viewer->addCube (min_point_AABB.x, max_point_AABB.x, min_point_AABB.y, max_point_AABB.y, min_point_AABB.z, max_point_AABB.z, 1.0, 1.0, 0.0, "AABB");
      
      Eigen::Vector3f position (position_OBB.x, position_OBB.y, position_OBB.z);
      Eigen::Quaternionf quat (rotational_matrix_OBB);
      viewer->addCube (position, quat, max_point_OBB.x - min_point_OBB.x, max_point_OBB.y - min_point_OBB.y, max_point_OBB.z - min_point_OBB.z, "OBB");

      pcl::PointXYZ center (mass_center (0), mass_center (1), mass_center (2));
      pcl::PointXYZ x_axis (major_vector (0) + mass_center (0), major_vector (1) + mass_center (1), major_vector (2) + mass_center (2));
      pcl::PointXYZ y_axis (middle_vector (0) + mass_center (0), middle_vector (1) + mass_center (1), middle_vector (2) + mass_center (2));
      pcl::PointXYZ z_axis (minor_vector (0) + mass_center (0), minor_vector (1) + mass_center (1), minor_vector (2) + mass_center (2));
      viewer->addLine (center, x_axis, 1.0f, 0.0f, 0.0f, "major eigen vector");
      viewer->addLine (center, y_axis, 0.0f, 1.0f, 0.0f, "middle eigen vector");
      viewer->addLine (center, z_axis, 0.0f, 0.0f, 1.0f, "minor eigen vector");
      
      while(!viewer->wasStopped()) {
        viewer->spinOnce (100);
        boost::this_thread::sleep (boost::posix_time::microseconds (100000));
      }

      return (0);
    }
//----------------------------------------------
//  Draw the scene.  A box on the end of the wand
//----------------------------------------------
void sonixApp::myDraw()
{
   glMatrixMode(GL_MODELVIEW);

      // -- Draw box on wand --- //
   gmtl::Matrix44f wandMatrix = mWand->getData();      // Get the wand matrix

   glPushMatrix();
      glPushMatrix();
         glMultMatrixf(wandMatrix.mData);  // Push the wand matrix on the stack

         float wand_color[3];
         wand_color[0] = wand_color[1] = wand_color[2] = 0.0f;

         if(mButton0->getData() == gadget::Digital::ON)
         {
            wand_color[0] += 0.5f;
         }

         if(mButton1->getData() == gadget::Digital::ON)
         {
            wand_color[1] += 0.5f;
         }

         if(mButton2->getData() == gadget::Digital::ON)
         {
            wand_color[2] += 0.5f;
         }

         if(mButton3->getData() == gadget::Digital::ON)
         {
            wand_color[0] += 0.5f;
         }

         if(mButton4->getData() == gadget::Digital::ON)
         {
            wand_color[1] += 0.5f;
         }

         if(mButton5->getData() == gadget::Digital::ON)
         {
            wand_color[2] += 0.5f;
         }

         glColor3fv(wand_color);
         glScalef(0.25f, 0.25f, 0.25f);
         drawCube();
      glPopMatrix();

         // A little laser pointer
      glLineWidth(5.0f);

      // Draw Axis
      glDisable(GL_LIGHTING);
      glPushMatrix();
         glMultMatrixf(wandMatrix.mData);    // Goto wand position

         gmtl::Vec3f x_axis(7.0f, 0.0f, 0.0f);
         gmtl::Vec3f y_axis(0.0f, 7.0f, 0.0f);
         gmtl::Vec3f z_axis(0.0f, 0.0f, 7.0f);
         gmtl::Vec3f origin(0.0f, 0.0f, 0.0f);

         glBegin(GL_LINES);
            glColor3f(1.0f, 0.0f, 0.0f);
            glVertex3fv(origin.mData);
            glVertex3fv(x_axis.mData);

            glColor3f(0.0f, 1.0f, 0.0f);
            glVertex3fv(origin.mData);
            glVertex3fv(y_axis.mData);

            glColor3f(0.0f, 0.0f, 1.0f);
            glVertex3fv(origin.mData);
            glVertex3fv(z_axis.mData);
         glEnd();
      glPopMatrix();
      glEnable(GL_LIGHTING);
   glPopMatrix();
}
void MapFrame::paintEvent(QPaintEvent* event)
{
    // Colorblind friendly colors
    QColor green(17, 192, 131);
    QColor red(255, 65, 30);

    float max_seen_height = -std::numeric_limits<float>::max(); // std::numeric_limits<float>::max() is the max possible floating point value
    float max_seen_width = -std::numeric_limits<float>::max();

    float min_seen_x = std::numeric_limits<float>::max();
    float min_seen_y = std::numeric_limits<float>::max();

    // Set the max and min seen values depending on which data the user has selected to view
    // Check each of the display data options and choose the most extreme value from those selected by the user
    if (display_ekf_data)
    {
        min_seen_x = min_ekf_seen_x[rover_to_display];
        min_seen_y = min_ekf_seen_y[rover_to_display];
        max_seen_height = max_ekf_seen_height[rover_to_display];
        max_seen_width = max_ekf_seen_height[rover_to_display];
    }

    if (display_gps_data)
    {
        if (min_seen_x > min_gps_seen_x[rover_to_display]) min_seen_x = min_gps_seen_x[rover_to_display];
        if (min_seen_y > min_gps_seen_y[rover_to_display]) min_seen_y = min_gps_seen_y[rover_to_display];
        if (max_seen_height < max_gps_seen_height[rover_to_display]) max_seen_height = max_gps_seen_height[rover_to_display];
        if (max_seen_width < max_gps_seen_width[rover_to_display]) max_seen_width = max_gps_seen_width[rover_to_display];
    }

    if (display_encoder_data)
    {
        if (min_seen_x > min_encoder_seen_x[rover_to_display]) min_seen_x = min_encoder_seen_x[rover_to_display];
        if (min_seen_y > min_encoder_seen_y[rover_to_display]) min_seen_y = min_encoder_seen_y[rover_to_display];
        if (max_seen_height < max_encoder_seen_height[rover_to_display]) max_seen_height = max_encoder_seen_height[rover_to_display];
        if (max_seen_width < max_encoder_seen_width[rover_to_display]) max_seen_width = max_encoder_seen_width[rover_to_display];
    }

    // Begin drawing the map
    QPainter painter(this);
    painter.setPen(Qt::white);

    // Track the frames per second for development purposes
    QString frames_per_second;
    frames_per_second = QString::number(frames /(frame_rate_timer.elapsed() / 1000.0), 'f', 0) + " FPS";

    QFontMetrics fm(painter.font());
    painter.drawText(this->width()-fm.width(frames_per_second), fm.height(), frames_per_second);

    frames++;

    if (!(frames % 100)) // time how long it takes to dispay 100 frames
    {
        frame_rate_timer.start();
        frames = 0;
    }

    // end frames per second

    if (ekf_rover_path[rover_to_display].empty() && encoder_rover_path[rover_to_display].empty() && gps_rover_path[rover_to_display].empty() && target_locations[rover_to_display].empty() && collection_points[rover_to_display].empty())
    {
        painter.drawText(QPoint(50,50), "Map Frame: Nothing to display.");
        return;
    }

    int map_origin_x = fm.width(QString::number(-max_seen_height, 'f', 1)+"m");
    int map_origin_y = 2*fm.height();

    int map_width = this->width()-map_origin_x;
    int map_height = this->height()-map_origin_y;

    int map_center_x = map_origin_x+((map_width-map_origin_x)/2);
    int map_center_y = map_origin_y+((map_height-map_origin_y)/2);

    // Draw the scale bars
    //painter.setPen(Qt::gray);
    //painter.drawLine(QPoint(map_center_x, map_origin_y), QPoint(map_center_x, map_height));
    //painter.drawLine(QPoint(map_origin_x, map_center_y), QPoint(map_width, map_center_y));
    //painter.setPen(Qt::white);

    // Cross hairs at map display center
    QPoint axes_origin(map_origin_x,map_origin_y);
    QPoint x_axis(map_width,map_origin_y);
    QPoint y_axis(map_origin_x,map_height);
    painter.drawLine(axes_origin, x_axis);
    painter.drawLine(axes_origin, y_axis);
    painter.drawLine(QPoint(map_width, map_origin_y), QPoint(map_width, map_height));
    painter.drawLine(QPoint(map_origin_x, map_height), QPoint(map_width, map_height));

    // Draw north arrow
    QPoint northArrow_point(map_center_x, 0);
    QPoint northArrow_left(map_center_x - 5, 5);
    QPoint northArrow_right(map_center_x + 5, 5);
    QRect northArrow_textBox(northArrow_left.x(), northArrow_left.y(), 10, 15);
    painter.drawLine(northArrow_left, northArrow_right);
    painter.drawLine(northArrow_left, northArrow_point);
    painter.drawLine(northArrow_right, northArrow_point);
    painter.drawText(northArrow_textBox, QString("N"));

    // Draw rover origin crosshairs
    // painter.setPen(green);

    // Check encoder has any values in it
    if (ekf_rover_path[rover_to_display].empty())
          {
            painter.drawText(QPoint(50,50), "Map Frame: No encoder data received.");
           return;
        }

    float initial_x = 0.0; //ekf_rover_path[rover_to_display].begin()->first;
    float initial_y = 0.001; //ekf_rover_path[rover_to_display].begin()->second;
    float rover_origin_x = map_origin_x+((initial_x-min_seen_x)/max_seen_width)*(map_width-map_origin_x);
    float rover_origin_y = map_origin_y+((initial_y-min_seen_y)/max_seen_height)*(map_height-map_origin_y);
    painter.setPen(Qt::gray);
    painter.drawLine(QPoint(rover_origin_x, map_origin_y), QPoint(rover_origin_x, map_height));
    painter.drawLine(QPoint(map_origin_x, rover_origin_y), QPoint(map_width, rover_origin_y));
    painter.setPen(Qt::white);

    int n_ticks = 6;
    float tick_length = 5;
    QPoint x_axis_ticks[n_ticks];
    QPoint y_axis_ticks[n_ticks];

    for (int i = 0; i < n_ticks-1; i++)
    {
        x_axis_ticks[i].setX(axes_origin.x()+(i+1)*map_width/n_ticks);
        x_axis_ticks[i].setY(axes_origin.y());

        y_axis_ticks[i].setX(axes_origin.x());
        y_axis_ticks[i].setY(axes_origin.y()+(i+1)*map_height/n_ticks);
    }

    for (int i = 0; i < n_ticks-1; i++)
    {
        painter.drawLine(x_axis_ticks[i], QPoint(x_axis_ticks[i].x(), x_axis_ticks[i].y()+tick_length));
        painter.drawLine(y_axis_ticks[i], QPoint(y_axis_ticks[i].x()+tick_length, y_axis_ticks[i].y()));
    }

    for (int i = 0; i < n_ticks-1; i++)
    {
        float fraction_of_map_to_rover_x = (rover_origin_x-map_origin_x)/map_width;
        float fraction_of_map_to_rover_y = (rover_origin_y-map_origin_y)/map_height;
        float x_label_f = (i+1)*max_seen_width/n_ticks-fraction_of_map_to_rover_x*max_seen_width;
        float y_label_f = (i+1)*max_seen_height/n_ticks-fraction_of_map_to_rover_y*max_seen_height;

        QString x_label = QString::number(x_label_f, 'f', 1) + "m";
        QString y_label = QString::number(-y_label_f, 'f', 1) + "m";

        int x_labels_offset_x = -(fm.width(x_label))/2;
        int x_labels_offset_y = 0;

        int y_labels_offset_x = -(fm.width(y_label));
        int y_labels_offset_y = fm.height()/3;

        painter.drawText(x_axis_ticks[i].x()+x_labels_offset_x, axes_origin.y()+x_labels_offset_y, x_label);
        painter.drawText(axes_origin.x()+y_labels_offset_x, y_axis_ticks[i].y()+y_labels_offset_y, y_label);

    }

    // End draw scale bars

    update_mutex.lock();
    // scale coordinates

    std::vector<QPoint> scaled_target_locations;
    for(std::vector< pair<float,float> >::iterator it = target_locations[rover_to_display].begin(); it != target_locations[rover_to_display].end(); ++it) {
        pair<float,float> coordinate  = *it;
        QPoint point;
        point.setX(map_origin_x+coordinate.first*map_width);
        point.setY(map_origin_y+coordinate.second*map_height);
        scaled_target_locations.push_back(point);
    }

    std::vector<QPoint> scaled_collection_points;
    for(std::vector< pair<float,float> >::iterator it = collection_points[rover_to_display].begin(); it != collection_points[rover_to_display].end(); ++it) {
        pair<float,float> coordinate  = *it;
        QPoint point;
        point.setX(map_origin_x+coordinate.first*map_width);
        point.setY(map_origin_y+coordinate.second*map_height);
        scaled_collection_points.push_back(point);
    }

    std::vector<QPoint> scaled_gps_rover_points;
    for(std::vector< pair<float,float> >::iterator it = gps_rover_path[rover_to_display].begin(); it != gps_rover_path[rover_to_display].end(); ++it) {
        pair<float,float> coordinate  = *it;

        float x = map_origin_x+((coordinate.first-min_seen_x)/max_seen_width)*(map_width-map_origin_x);
        float y = map_origin_y+((coordinate.second-min_seen_y)/max_seen_height)*(map_height-map_origin_y);
        scaled_gps_rover_points.push_back( QPoint(x,y) );
    }


    QPainterPath scaled_ekf_rover_path;
    for(std::vector< pair<float,float> >::iterator it = ekf_rover_path[rover_to_display].begin(); it != ekf_rover_path[rover_to_display].end(); ++it) {
        pair<float,float> coordinate  = *it;
        QPoint point;
        float x = map_origin_x+((coordinate.first-min_seen_x)/max_seen_width)*(map_width-map_origin_x);
        float y = map_origin_y+((coordinate.second-min_seen_y)/max_seen_height)*(map_height-map_origin_y);

        if (it == ekf_rover_path[rover_to_display].begin()) scaled_ekf_rover_path.moveTo(x, y);
        scaled_ekf_rover_path.lineTo(x, y);
    }

    QPainterPath scaled_gps_rover_path;
    for(std::vector< pair<float,float> >::iterator it = gps_rover_path[rover_to_display].begin(); it != gps_rover_path[rover_to_display].end(); ++it) {
        pair<float,float> coordinate  = *it;
        QPoint point;
        float x = map_origin_x+((coordinate.first-min_seen_x)/max_seen_width)*(map_width-map_origin_x);
        float y = map_origin_y+((coordinate.second-min_seen_y)/max_seen_height)*(map_height-map_origin_y);

        scaled_gps_rover_path.lineTo(x, y);
    }

    QPainterPath scaled_encoder_rover_path;
    for(std::vector< pair<float,float> >::iterator it = encoder_rover_path[rover_to_display].begin(); it != encoder_rover_path[rover_to_display].end(); ++it) {
        pair<float,float> coordinate  = *it;
        QPoint point;
        float x = map_origin_x+((coordinate.first-min_seen_x)/max_seen_width)*(map_width-map_origin_x);
        float y = map_origin_y+((coordinate.second-min_seen_y)/max_seen_height)*(map_height-map_origin_y);

       if (it == encoder_rover_path[rover_to_display].begin()) scaled_encoder_rover_path.moveTo(x, y);

        scaled_encoder_rover_path.lineTo(x, y);
    }


    painter.setPen(red);
    if (display_gps_data) painter.drawPoints(&scaled_gps_rover_points[0], scaled_gps_rover_points.size());
   // if (display_gps_data) painter.drawPath(scaled_gps_rover_path);

    painter.setPen(Qt::white);
    if (display_ekf_data) painter.drawPath(scaled_ekf_rover_path);
    painter.setPen(green);
    if (display_encoder_data) painter.drawPath(scaled_encoder_rover_path);


    painter.setPen(red);
    QPoint* point_array = &scaled_collection_points[0];
    painter.drawPoints(point_array, scaled_collection_points.size());
    painter.setPen(green);
    point_array = &scaled_target_locations[0];
    painter.drawPoints(point_array, scaled_target_locations.size());

    update_mutex.unlock();

    painter.setPen(Qt::white);
}
Beispiel #10
0
// Utility function to test intersection between ray and mesh bounding box
bool test_ray_oobb(const glm::vec3 &origin, const glm::vec3 &direction, const glm::vec3 &aabb_min,
                   const glm::vec3 &aabb_max, const glm::mat4 &model, float &distance) {
  float t_min = 0.0f;
  float t_max = 100000.0f;

  // Calculate OOBB position in world space
  glm::vec3 OOBB_pos_world(model[3].x, model[3].y, model[3].z);

  // Calculate direction from ray origin to OOBB position
  glm::vec3 delta = OOBB_pos_world - origin;

  // Test intersection with the 2 planes perpendicular to the OOBB x-axis
  {
    // Get x-axis of transformed object
    glm::vec3 x_axis(model[0].x, model[0].y, model[0].z);
    x_axis = glm::normalize(x_axis);
    // Calculate the cosine of the x_axis and delta
    float e = glm::dot(x_axis, delta);
    // Calculate the cosine of the ray direction and x_axis
    float f = glm::dot(direction, x_axis);

    // Test if ray and x_axis are parallel
    if (fabs(f) > 0.001f) {
      // Calculate intersection distance with the left plane
      float t1 = (e + aabb_min.x) / f;
      // Calculate intersection distance with the right plane
      float t2 = (e + aabb_max.x) / f;

      // t1 needs to be the nearest intersection
      if (t1 > t2)
        std::swap(t1, t2);

      // t_max is the nearest far intersection
      t_max = glm::min(t_max, t2);
      // t_min is the farthese near intersection
      t_min = glm::max(t_min, t1);

      // If far is closer than near there is no intersection on this axis
      if (t_max < t_min)
        return false;
    }
    // ray and x_axis is almost parallel
    else if (-e + aabb_min.x > 0.0f || -e + aabb_max.x < 0.0f)
      return false;
  }

  // Test intersection with the 2 planes perpendicular to the OOBB y-axis
  {
    // Get y-axis of transformed object
    glm::vec3 y_axis(model[1].x, model[1].y, model[1].z);
    y_axis = glm::normalize(y_axis);
    // Calculate the cosine of the y_axis and delta
    float e = glm::dot(y_axis, delta);
    // Calculate the cosine of the ray direction and y_axis
    float f = glm::dot(direction, y_axis);

    // Test if ray and y_axis are parallel
    if (fabs(f) > 0.001f) {
      // Calculate intersection distance with the left plane
      float t1 = (e + aabb_min.y) / f;
      // Calculate intersection distance with the right plane
      float t2 = (e + aabb_max.y) / f;

      // t1 needs to be the nearest intersection
      if (t1 > t2)
        std::swap(t1, t2);

      // t_max is the nearest far intersection
      t_max = glm::min(t_max, t2);
      // t_min is the farthese near intersection
      t_min = glm::max(t_min, t1);

      // If far is closer than near there is no intersection on this axis
      if (t_max < t_min)
        return false;
    }
    // ray and y_axis is almost parallel
    else if (-e + aabb_min.y > 0.0f || -e + aabb_max.y < 0.0f)
      return false;
  }

  // Test intersection with the 2 planes perpendicular to the OOBB z-axis
  {
    // Get x-axis of transformed object
    glm::vec3 z_axis(model[2].x, model[2].y, model[2].z);
    z_axis = glm::normalize(z_axis);
    // Calculate the cosine of the z_axis and delta
    float e = glm::dot(z_axis, delta);
    // Calculate the cosine of the ray direction and z_axis
    float f = glm::dot(direction, z_axis);

    // Test if ray and z_axis are parallel
    if (fabs(f) > 0.001f) {
      // Calculate intersection distance with the left plane
      float t1 = (e + aabb_min.z) / f;
      // Calculate intersection distance with the right plane
      float t2 = (e + aabb_max.z) / f;

      // t1 needs to be the nearest intersection
      if (t1 > t2)
        std::swap(t1, t2);

      // t_max is the nearest far intersection
      t_max = glm::min(t_max, t2);
      // t_min is the farthese near intersection
      t_min = glm::max(t_min, t1);

      // If far is closer than near there is no intersection on this axis
      if (t_max < t_min)
        return false;
    }
    // ray and z_axis is almost parallel
    else if (-e + aabb_min.z > 0.0f || -e + aabb_max.z < 0.0f)
      return false;
  }

  // Set distance and return true
  distance = t_min;
  return true;
}
int main (int argc, char** argv)
{
    if (argc != 3)
        return (0);

    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ> ());
    if (pcl::io::loadPLYFile (argv[1], *cloud) == -1)
        return (-1);

    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud2 (new pcl::PointCloud<pcl::PointXYZ> ());
    pcl::io::loadPLYFile(argv[2], *cloud2);
    pcl::MomentOfInertiaEstimation <pcl::PointXYZ> feature_extractor;
    feature_extractor.setInputCloud (cloud);
    feature_extractor.compute ();

    std::vector <float> moment_of_inertia;
    std::vector <float> eccentricity;
    pcl::PointXYZ min_point_AABB;
    pcl::PointXYZ max_point_AABB;
    pcl::PointXYZ min_point_OBB;
    pcl::PointXYZ max_point_OBB;
    pcl::PointXYZ position_OBB;
    Eigen::Matrix3f rotational_matrix_OBB;
    float major_value, middle_value, minor_value;
    Eigen::Vector3f major_vector, middle_vector, minor_vector;
    Eigen::Vector3f mass_center;

    feature_extractor.getMomentOfInertia (moment_of_inertia);
    feature_extractor.getEccentricity (eccentricity);
    feature_extractor.getAABB (min_point_AABB, max_point_AABB);
    feature_extractor.getOBB (min_point_OBB, max_point_OBB, position_OBB, rotational_matrix_OBB);
    feature_extractor.getEigenValues (major_value, middle_value, minor_value);
    feature_extractor.getEigenVectors (major_vector, middle_vector, minor_vector);
    feature_extractor.getMassCenter (mass_center);

    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));
    viewer->setBackgroundColor (0, 0, 0);
    viewer->addCoordinateSystem (2.0, 0);
    viewer->initCameraParameters ();
    viewer->addPointCloud<pcl::PointXYZ> (cloud, "sample cloud");
    viewer->addPointCloud<pcl::PointXYZ> (cloud2, "object cloud");
    viewer->addCube (min_point_AABB.x, max_point_AABB.x, min_point_AABB.y, max_point_AABB.y, min_point_AABB.z, max_point_AABB.z, 1.0, 1.0, 0.0, "AABB");

    Eigen::Vector3f position (position_OBB.x, position_OBB.y, position_OBB.z);
    Eigen::Quaternionf quat (rotational_matrix_OBB);
    viewer->addCube (position, quat, max_point_OBB.x - min_point_OBB.x, max_point_OBB.y - min_point_OBB.y, max_point_OBB.z - min_point_OBB.z, "OBB");

    pcl::PointXYZ center (mass_center (0), mass_center (1), mass_center (2));
    pcl::PointXYZ x_axis (major_vector (0) + mass_center (0), major_vector (1) + mass_center (1), major_vector (2) + mass_center (2));
    pcl::PointXYZ y_axis (middle_vector (0) + mass_center (0), middle_vector (1) + mass_center (1), middle_vector (2) + mass_center (2));
    pcl::PointXYZ z_axis (minor_vector (0) + mass_center (0), minor_vector (1) + mass_center (1), minor_vector (2) + mass_center (2));
    viewer->addLine (center, x_axis, 1.0f, 0.0f, 0.0f, "major eigen vector");
    viewer->addLine (center, y_axis, 0.0f, 1.0f, 0.0f, "middle eigen vector");
    viewer->addLine (center, z_axis, 0.0f, 0.0f, 1.0f, "minor eigen vector");

    while(!viewer->wasStopped())
    {
        viewer->spinOnce (100);
        boost::this_thread::sleep (boost::posix_time::microseconds (100000));
    }

    return (0);
}
Beispiel #12
0
void Axes::set_domain(double x_min, double x_max)
{
  (*x_axis()->coords)[0] = QVector2D(x_min, 0.0);
  (*x_axis()->coords)[1] = QVector2D(x_max, 0.0);
}
Beispiel #13
0
void GraphManager::visualizeGraphNodes() const {
    std::clock_t starttime=std::clock();

    if (marker_pub_.getNumSubscribers() > 0) { //don't visualize, if nobody's looking
        visualization_msgs::Marker nodes_marker;
        nodes_marker.header.frame_id = "/openni_rgb_optical_frame"; //TODO: Should be a meaningfull fixed frame with known relative pose to the camera
        nodes_marker.header.stamp = ros::Time::now();
        nodes_marker.ns = "camera_pose_graph"; // Set the namespace and id for this marker.  This serves to create a unique ID
        nodes_marker.id = 1;    // Any marker sent with the same namespace and id will overwrite the old one


        nodes_marker.type = visualization_msgs::Marker::LINE_LIST;
        nodes_marker.action = visualization_msgs::Marker::ADD; // Set the marker action.  Options are ADD and DELETE
        nodes_marker.frame_locked = true; //rviz automatically retransforms the markers into the frame every update cycle
        // Set the scale of the marker -- 1x1x1 here means 1m on a side
        nodes_marker.scale.x = 0.002;
        //Global pose (used to transform all points) //TODO: is this the default pose anyway?
        nodes_marker.pose.position.x = 0;
        nodes_marker.pose.position.y = 0;
        nodes_marker.pose.position.z = 0;
        nodes_marker.pose.orientation.x = 0.0;
        nodes_marker.pose.orientation.y = 0.0;
        nodes_marker.pose.orientation.z = 0.0;
        nodes_marker.pose.orientation.w = 1.0;
        // Set the color -- be sure to set alpha to something non-zero!
        nodes_marker.color.r = 1.0f;
        nodes_marker.color.g = 0.0f;
        nodes_marker.color.b = 0.0f;
        nodes_marker.color.a = 1.0f;


        geometry_msgs::Point tail; //same startpoint for each line segment
        geometry_msgs::Point tip;  //different endpoint for each line segment
        std_msgs::ColorRGBA arrow_color_red  ;  //red x axis
        arrow_color_red.r = 1.0;
        arrow_color_red.a = 1.0;
        std_msgs::ColorRGBA arrow_color_green;  //green y axis
        arrow_color_green.g = 1.0;
        arrow_color_green.a = 1.0;
        std_msgs::ColorRGBA arrow_color_blue ;  //blue z axis
        arrow_color_blue.b = 1.0;
        arrow_color_blue.a = 1.0;
        Vector3f origin(0.0,0.0,0.0);
        Vector3f x_axis(0.2,0.0,0.0); //20cm long axis for the first (almost fixed) node
        Vector3f y_axis(0.0,0.2,0.0);
        Vector3f z_axis(0.0,0.0,0.2);
        Vector3f tmp; //the transformed endpoints
        int counter = 0;
        AISNavigation::PoseGraph3D::Vertex* v; //used in loop
        AISNavigation::PoseGraph3D::VertexIDMap::iterator vertex_iter = optimizer_->vertices().begin();
        for(/*see above*/; vertex_iter != optimizer_->vertices().end(); vertex_iter++, counter++) {
            v = static_cast<AISNavigation::PoseGraph3D::Vertex*>((*vertex_iter).second);
            //v->transformation.rotation().x()+ v->transformation.rotation().y()+ v->transformation.rotation().z()+ v->transformation.rotation().w();
            tmp = v->transformation * origin;
            tail.x = tmp.x();
            tail.y = tmp.y();
            tail.z = tmp.z();
            //Endpoints X-Axis
            nodes_marker.points.push_back(tail);
            nodes_marker.colors.push_back(arrow_color_red);
            tmp = v->transformation * x_axis;
            tip.x  = tmp.x();
            tip.y  = tmp.y();
            tip.z  = tmp.z();
            nodes_marker.points.push_back(tip);
            nodes_marker.colors.push_back(arrow_color_red);
            //Endpoints Y-Axis
            nodes_marker.points.push_back(tail);
            nodes_marker.colors.push_back(arrow_color_green);
            tmp = v->transformation * y_axis;
            tip.x  = tmp.x();
            tip.y  = tmp.y();
            tip.z  = tmp.z();
            nodes_marker.points.push_back(tip);
            nodes_marker.colors.push_back(arrow_color_green);
            //Endpoints Z-Axis
            nodes_marker.points.push_back(tail);
            nodes_marker.colors.push_back(arrow_color_blue);
            tmp = v->transformation * z_axis;
            tip.x  = tmp.x();
            tip.y  = tmp.y();
            tip.z  = tmp.z();
            nodes_marker.points.push_back(tip);
            nodes_marker.colors.push_back(arrow_color_blue);
            //shorten all nodes after the first one
            x_axis.x() = 0.1;
            y_axis.y() = 0.1;
            z_axis.z() = 0.1;
        }

        marker_pub_.publish (nodes_marker);
        ROS_INFO("published %d graph nodes", counter);
    }

    ROS_INFO_STREAM_COND_NAMED(( (std::clock()-starttime) / (double)CLOCKS_PER_SEC) > 0.01, "timings", "function runtime: "<< ( std::clock() - starttime ) / (double)CLOCKS_PER_SEC  <<"sec");
}
void soundManagerApp::myDraw()
{
   //std::cout << "\n--- myDraw() ---\n";

   //std::cout << "HeadPos:" << vrj::Coord(*mHead->getData()).pos << "\t"
   //          << "WandPos:" << vrj::Coord(*mWand->getData()).pos << std::endl;

   glMatrixMode(GL_MODELVIEW);

      // -- Draw box on wand --- //
   gmtl::Matrix44f wandMatrix = mWand->getData();      // Get the wand matrix

   glPushMatrix();
      // cout << "wand:\n" << *wandMatrix << endl;
      glPushMatrix();
         glMultMatrixf(wandMatrix.mData);  // Push the wand matrix on the stack
         //glColor3f(1.0f, 0.0f, 1.0f);
         float wand_color[3];
         wand_color[0] = wand_color[1] = wand_color[2] = 0.0f;
         if(mButton0->getData() == gadget::Digital::ON)
            wand_color[0] += 0.5f;
         if(mButton1->getData() == gadget::Digital::ON)
            wand_color[1] += 0.5f;
         if(mButton2->getData() == gadget::Digital::ON)
            wand_color[2] += 0.5f;
         if(mButton3->getData() == gadget::Digital::ON)
            wand_color[0] += 0.5f;
         if(mButton4->getData() == gadget::Digital::ON)
            wand_color[1] += 0.5f;
         if(mButton5->getData() == gadget::Digital::ON)
            wand_color[2] += 0.5f;
         glColor3fv(wand_color);
         glScalef(0.25f, 0.25f, 0.25f);
         drawCube();
      glPopMatrix();

         // A little laser pointer
      glLineWidth(5.0f);



      // Draw Axis
      glDisable(GL_LIGHTING);
      glPushMatrix();
         glMultMatrixf(wandMatrix.mData);    // Goto wand position

         gmtl::Vec3f x_axis(7.0f,0.0f,0.0f);
         gmtl::Vec3f y_axis(0.0f, 7.0f, 0.0f);
         gmtl::Vec3f z_axis(0.0f, 0.0f, 7.0f);
         gmtl::Vec3f origin(0.0f, 0.0f, 0.0f);

         glBegin(GL_LINES);
            glColor3f(1.0f, 0.0f, 0.0f);
            glVertex3fv(origin.mData);
            glVertex3fv(x_axis.mData);

            glColor3f(0.0f, 1.0f, 0.0f);
            glVertex3fv(origin.mData);
            glVertex3fv(y_axis.mData);

            glColor3f(0.0f, 0.0f, 1.0f);
            glVertex3fv(origin.mData);
            glVertex3fv(z_axis.mData);
         glEnd();
      glPopMatrix();
      glEnable(GL_LIGHTING);
   glPopMatrix();

}
void Transform::up(float degrees, vec3& eye, vec3& up) {
  // YOUR CODE FOR HW2 HERE 
	vec3 x_axis(1.0, 0.0, 0.0);
	eye = eye * rotate(degrees, x_axis);
	up = up * rotate(degrees, x_axis);
}
Beispiel #16
0
template <typename PointInT, typename PointNT, typename PointOutT> bool
pcl::ShapeContext3DEstimation<PointInT, PointNT, PointOutT>::computePoint (
    size_t index, const pcl::PointCloud<PointNT> &normals, float rf[9], std::vector<float> &desc)
{
  // The RF is formed as this x_axis | y_axis | normal
  Eigen::Map<Eigen::Vector3f> x_axis (rf);
  Eigen::Map<Eigen::Vector3f> y_axis (rf + 3);
  Eigen::Map<Eigen::Vector3f> normal (rf + 6);

  // Find every point within specified search_radius_
  std::vector<int> nn_indices;
  std::vector<float> nn_dists;
  const size_t neighb_cnt = searchForNeighbors ((*indices_)[index], search_radius_, nn_indices, nn_dists);
  if (neighb_cnt == 0)
  {
    for (size_t i = 0; i < desc.size (); ++i)
      desc[i] = std::numeric_limits<float>::quiet_NaN ();

    memset (rf, 0, sizeof (rf[0]) * 9);
    return (false);
  }

  float minDist = std::numeric_limits<float>::max ();
  int minIndex = -1;
  for (size_t i = 0; i < nn_indices.size (); i++)
  {
	  if (nn_dists[i] < minDist)
	  {
      minDist = nn_dists[i];
      minIndex = nn_indices[i];
	  }
  }
  
  // Get origin point
  Vector3fMapConst origin = input_->points[(*indices_)[index]].getVector3fMap ();
  // Get origin normal
  // Use pre-computed normals
  normal = normals[minIndex].getNormalVector3fMap ();

  // Compute and store the RF direction
  x_axis[0] = rnd ();
  x_axis[1] = rnd ();
  x_axis[2] = rnd ();
  if (!pcl::utils::equal (normal[2], 0.0f))
    x_axis[2] = - (normal[0]*x_axis[0] + normal[1]*x_axis[1]) / normal[2];
  else if (!pcl::utils::equal (normal[1], 0.0f))
    x_axis[1] = - (normal[0]*x_axis[0] + normal[2]*x_axis[2]) / normal[1];
  else if (!pcl::utils::equal (normal[0], 0.0f))
    x_axis[0] = - (normal[1]*x_axis[1] + normal[2]*x_axis[2]) / normal[0];

  x_axis.normalize ();

  // Check if the computed x axis is orthogonal to the normal
  assert (pcl::utils::equal (x_axis[0]*normal[0] + x_axis[1]*normal[1] + x_axis[2]*normal[2], 0.0f, 1E-6f));

  // Store the 3rd frame vector
  y_axis = normal.cross (x_axis);

  // For each point within radius
  for (size_t ne = 0; ne < neighb_cnt; ne++)
  {
    if (pcl::utils::equal (nn_dists[ne], 0.0f))
		  continue;
    // Get neighbours coordinates
    Eigen::Vector3f neighbour = surface_->points[nn_indices[ne]].getVector3fMap ();

    /// ----- Compute current neighbour polar coordinates -----
    /// Get distance between the neighbour and the origin
    float r = sqrt (nn_dists[ne]); 
    
    /// Project point into the tangent plane
    Eigen::Vector3f proj;
    pcl::geometry::project (neighbour, origin, normal, proj);
    proj -= origin;

    /// Normalize to compute the dot product
    proj.normalize ();
    
    /// Compute the angle between the projection and the x axis in the interval [0,360] 
    Eigen::Vector3f cross = x_axis.cross (proj);
    float phi = pcl::rad2deg (std::atan2 (cross.norm (), x_axis.dot (proj)));
    phi = cross.dot (normal) < 0.f ? (360.0 - phi) : phi;
    /// Compute the angle between the neighbour and the z axis (normal) in the interval [0, 180]
    Eigen::Vector3f no = neighbour - origin;
    no.normalize ();
    float theta = normal.dot (no);
    theta = pcl::rad2deg (acos (std::min (1.0f, std::max (-1.0f, theta))));

    // Bin (j, k, l)
    size_t j = 0;
    size_t k = 0;
    size_t l = 0;

    // Compute the Bin(j, k, l) coordinates of current neighbour
    for (size_t rad = 1; rad < radius_bins_+1; rad++) 
    {
      if (r <= radii_interval_[rad]) 
      {
        j = rad-1;
        break;
      }
    }

    for (size_t ang = 1; ang < elevation_bins_+1; ang++) 
    {
      if (theta <= theta_divisions_[ang]) 
      {
        k = ang-1;
        break;
      }
    }

    for (size_t ang = 1; ang < azimuth_bins_+1; ang++) 
    {
      if (phi <= phi_divisions_[ang]) 
      {
        l = ang-1;
        break;
      }
    }

    // Local point density = number of points in a sphere of radius "point_density_radius_" around the current neighbour
    std::vector<int> neighbour_indices;
    std::vector<float> neighbour_distances;
    int point_density = searchForNeighbors (*surface_, nn_indices[ne], point_density_radius_, neighbour_indices, neighbour_distances);
    // point_density is NOT always bigger than 0 (on error, searchForNeighbors returns 0), so we must check for that
    if (point_density == 0)
      continue;

    float w = (1.0f / point_density) * volume_lut_[(l*elevation_bins_*radius_bins_) + 
                                                  (k*radius_bins_) + 
                                                  j];
      
    assert (w >= 0.0);
    if (w == std::numeric_limits<float>::infinity ())
      PCL_ERROR ("Shape Context Error INF!\n");
    if (w != w)
      PCL_ERROR ("Shape Context Error IND!\n");
    /// Accumulate w into correspondant Bin(j,k,l)
    desc[(l*elevation_bins_*radius_bins_) + (k*radius_bins_) + j] += w;

    assert (desc[(l*elevation_bins_*radius_bins_) + (k*radius_bins_) + j] >= 0);
  } // end for each neighbour

  // 3DSC does not define a repeatable local RF, we set it to zero to signal it to the user 
  memset (rf, 0, sizeof (rf[0]) * 9);
  return (true);
}
Beispiel #17
0
void wandApp::draw()
{
   glClear(GL_DEPTH_BUFFER_BIT);
   //std::cout << "\n--- myDraw() ---\n";

//   std::cout << "HeadPos:" << gmtl::makeTrans<gmtl::Vec3f>(mHead->getData()) << "\t"
//             << "WandPos:" << gmtl::makeTrans<gmtl::Vec3f>(mWand->getData()) << std::endl;

   glMatrixMode(GL_MODELVIEW);

      // -- Draw box on wand --- //
   gmtl::Matrix44f wandMatrix = mWand->getData();      // Get the wand matrix

   glPushMatrix();
      // cout << "wand:\n" << *wandMatrix << endl;
      glPushMatrix();
         glMultMatrixf(wandMatrix.mData);  // Push the wand matrix on the stack

         //glColor3f(1.0f, 0.0f, 1.0f);
         float wand_color[3];
         wand_color[0] = wand_color[1] = wand_color[2] = 0.0f;
         if (mButton0->getData() == gadget::Digital::ON)
         {
            wand_color[0] += 0.5f;
         }
         if (mButton1->getData() == gadget::Digital::ON)
         {
            wand_color[1] += 0.5f;
         }
         if (mButton2->getData() == gadget::Digital::ON)
         {
            wand_color[2] += 0.5f;
         }
         if (mButton3->getData() == gadget::Digital::ON)
         {
            wand_color[0] += 0.5f;
         }
         if (mButton4->getData() == gadget::Digital::ON)
         {
            wand_color[1] += 0.5f;
         }
         if (mButton5->getData() == gadget::Digital::ON)
         {
            wand_color[2] += 0.5f;
         }
         glColor3fv(wand_color);
         glScalef(0.25f, 0.25f, 0.25f);
         drawCube();
      glPopMatrix();

         // A little laser pointer
      glLineWidth(5.0f);

      // Draw Axis
      glDisable(GL_LIGHTING);
      glPushMatrix();
         glMultMatrixf(wandMatrix.mData);    // Goto wand position

         gmtl::Vec3f x_axis(7.0f,0.0f,0.0f);
         gmtl::Vec3f y_axis(0.0f, 7.0f, 0.0f);
         gmtl::Vec3f z_axis(0.0f, 0.0f, 7.0f);
         gmtl::Vec3f origin(0.0f, 0.0f, 0.0f);

         glBegin(GL_LINES);
            glColor3f(1.0f, 0.0f, 0.0f);
            glVertex3fv(origin.mData);
            glVertex3fv(x_axis.mData);

            glColor3f(0.0f, 1.0f, 0.0f);
            glVertex3fv(origin.mData);
            glVertex3fv(y_axis.mData);

            glColor3f(0.0f, 0.0f, 1.0f);
            glVertex3fv(origin.mData);
            glVertex3fv(z_axis.mData);
         glEnd();
      glPopMatrix();
      glEnable(GL_LIGHTING);
   glPopMatrix();

   // Draw the head history
   glLineWidth(1.0f);
   glPushMatrix();
      glBegin(GL_LINES);
         glColor3f(0.0f, 0.8f, 0.8f);

         for(unsigned i=0; i<mHeadHistory.size(); ++i)
         {
            glVertex3fv(mHeadHistory[i].mData);
         }
      glEnd();
   glPopMatrix();

   mPerfProbe.draw();

}
/**
    take the input point
    move it 'distance' back along X
    determine the overall sketch length
    determine the distance along the sketch for this character based on the distance along the text as well as the justification
    find the point along the sketch for this character
    find the angle of rotation at this point along the sketch
    rotate the point about this character's origin
    translate the point to align it with the point along the sketch.
    return this adjusted point location.
 */
gp_Pnt & COrientationModifier::Transform(gp_Trsf existing_transformation, const double _distance, gp_Pnt & point, const float width )
{
	double tolerance = wxGetApp().m_geom_tol;
	gp_Pnt original_location(point);

    if (m_edges.size() == 0)
	{
		// No children so no modification of position.
		point = original_location;
		return(point);
	}

    // The text is formatted as though the characters start at the origin (0,0,0) and move
    // to the right by the character and/or word spacing as subsequent characters are rendered.
    // This step moves this character so that its origin is at the (0,0,0) location so that
    // we rotate just this character about its origin.  We can use the _distance value as
    // the offset along the X axis at which point this character's origin is located.
	gp_Pnt origin(0.0, 0.0, 0.0);
	gp_Trsf move_to_origin;
	move_to_origin.SetTranslation(origin, gp_Pnt(-1.0 * _distance, 0.0, 0.0));
	point.Transform(move_to_origin);

    // Make sure it's positive.
	double distance(_distance);
	if (distance < 0) distance *= -1.0;

	// Now adjust the distance based on a combination of the justification and how far through
	// the text string we are.
	double distance_remaining = distance;

	switch (m_params.m_justification)
	{
	case COrientationModifierParams::eLeftJustified:
        if (SketchIsClosed())
        {
            // Centre the text on the left edge.  NOTE: This assumes that it's a circle with
            // the starting location in the positive X axis.
            distance_remaining = (m_total_edge_length / 2.0) - (width / 2.0) + distance;
        }
        else
        {
            distance_remaining = distance;	// No special adjustment required.
        }
		break;

	case COrientationModifierParams::eRightJustified:
        if (SketchIsClosed())
        {
            distance_remaining = m_total_edge_length - (width / 2.0) + distance;
        }
        else
        {
            distance_remaining = m_total_edge_length - width + distance;
        }
		break;

	case COrientationModifierParams::eCentreJustified:
		distance_remaining = (m_total_edge_length / 2.0) - (width / 2.0) + distance;
		break;

    case COrientationModifierParams::eTopJustified:
        distance_remaining = (m_total_edge_length / 4.0) - (width / 2.0) + distance;
        break;

    case COrientationModifierParams::eBottomJustified:
        distance_remaining = (m_total_edge_length * 3.0 / 4.0) - (width / 2.0) + distance;
        break;
	} // End switch


    while ((distance_remaining > tolerance) && (m_edges.size() > 0))
    {
        for (Edges_t::iterator itEdge = m_edges.begin(); itEdge != m_edges.end(); itEdge++)
        {

			double edge_length = itEdge->second;
			if (edge_length < distance_remaining)
			{
				distance_remaining -= edge_length;
			}
			else
			{
				// The point we're after is along this edge somewhere.  Find the point and
				// the first derivative at that point.  The vector returned will allow us to
				// find out what angle the sketch is at this point.  We can use this angle
				// to rotate the character.

				BRepAdaptor_Curve curve(itEdge->first);
				gp_Pnt p;
				gp_Vec vec;
				double proportion = distance_remaining / edge_length;
				Standard_Real U = ((curve.LastParameter() - curve.FirstParameter()) * proportion) + curve.FirstParameter();
				curve.D1(U, p, vec);

				double angle = 0.0;
				if (m_params.m_sketch_rotates_text)
				{
				    // Measure the angle with respect to the positive X axis (when looking down from the top)
				    gp_Vec x_axis(gp_Pnt(0.0, 0.0, 0.0), gp_Pnt(1.0, 0.0, 0.0));
				    gp_Vec from_top_down( gp_Pnt(0.0, 0.0, 0.0), gp_Pnt(0.0, 0.0, 1.0));
				    angle = x_axis.AngleWithRef(vec, from_top_down);
				}

                // If the user wants each character given an extra quarter turn (or more)
                // then add this in now.
				if (m_params.m_number_of_rotations > 0)
				{
				    for (int i=0; i<m_params.m_number_of_rotations; i++)
				    {
				        angle += (PI / 2.0);
				    }
				}
				else
				{
				    for (int i=m_params.m_number_of_rotations; i<0; i++)
				    {
				        angle -= (PI / 2.0);
				    }
				}

                // Rotate the point around the origin.
				angle *= -1.0;
				gp_Trsf transformation;
				transformation.SetRotation( gp_Ax1(origin, gp_Vec(0,0,-1)), angle );
				point.Transform( transformation );

                // And then translate the point from the origin to the point along the sketch.
				gp_Trsf around;
				around.SetTranslation(origin, p);
				point.Transform(around);

				return(point);
			}
		} // End for
	} // End while

	return(point);
}
Beispiel #19
0
int main( void ) {


	//------------------------------------------------------

	//Ex 23 GLM:llä matriisi laskemista
	//glm::mat4 matrix1(1.0f, 0.0f, 2.0f, 2.0f,
	//				  0.0f, 1.0f, 0.0f, 0.0f,
	//				  1.0f, 1.0f, 1.0f, 2.0f,
	//				  0.0f, 0.0f, 1.0f, 0.0f);

	//glm::mat4 matrix2(0.0f, 0.0f, 0.0f, 2.0f,
	//				  1.0f, 1.0f, 0.0f, 0.0f,
	//				  1.0f, 1.0f, 0.0f, 2.0f,
	//				  0.0f, 0.0f, 1.0f, 0.0f);

	//glm::vec4 multiplyVector(3.0f, 4.0f, -2.0f, 1.0f);

	//glm::vec4 xTulos;

	//xTulos = (matrix1 * matrix2) * multiplyVector;

	///*for (int j = 0; j < sizeof(xTulos); j++)
	//{
	//	std::cout << xTulos[j] << std::endl;
	//}*/

	//std::cout << xTulos[0] << ", " << xTulos[1] << ", " << xTulos[2] << ", " << xTulos[3] << std::endl;

	//--------------------------------------------------------

	//Test
	//------------------------------------------------------
	
	std::cout << "Ex 25-------------------------------------------------------\n" << std::endl;
	glm::vec4 P1(0.0f, 0.0f, 0.0f, 1.0f);
	glm::vec4 P2(1.0f, 0.0f, 0.0f, 1.0f);
	glm::vec4 P3(0.0f, 1.0f, 0.0f, 1.0f);

	glm::vec3 x_axis(1.0f, 0.0f, 0.0f);
	glm::vec3 y_axis(0.0f, 1.0f, 0.0f);
	glm::vec3 z_axis(0.0f, 0.0f, 1.0f);

	glm::vec3 s(0.3f); //skaalaus
	glm::vec3 t(-2.0f, -1.0f, 0.0f); //siirto
	glm::vec3 r = z_axis; //kierto

	glm::mat4 R = rotate(3.14159265f / 6.0f, r);
	glm::mat4 S = scale(s);
	glm::mat4 T = translate(t);

	glm::mat4 T_total = T*S*R;
	PrintVec(T_total*P1);
	PrintVec(T_total*P2);
	PrintVec(T_total*P3);
	PrintMatrix(T_total);


	std::cout << "\nEx 26--------------------------------------------------------" << std::endl;
	glm::vec3 cam_pos(1.2f, 0.1f, 0.0);
	glm::vec3 cam_up = y_axis;
	glm::vec3 cam_right = x_axis;
	glm::vec3 cam_front = z_axis; //oikeakätinen koordinaatisto

	glm::mat4 C = lookAt(cam_pos, cam_pos + cam_front, cam_up);
	T_total = C*T_total;

	PrintVec(T_total*P1);
	PrintVec(T_total*P2);
	PrintVec(T_total*P3);
	PrintMatrix(T_total);

	std::cout << "\nEx 27---------------------------------------------------------" << std::endl;

	float v_width = 6.0; //viewport in camera coord
	float v_height = 6.0;

	glm::mat4  T_projection = glm::ortho(-0.5f*v_width, 0.5f*v_width,
		-0.5f*v_height, 0.5f*v_height);
	T_total = T_projection*T_total;
	PrintVec(T_total*P1);
	PrintVec(T_total*P2);
	PrintVec(T_total*P3);
	PrintMatrix(T_total);

	//------------------------------------------------------

	std::cout << "\n Ex 30--------------------------------------------------------" << std::endl;

	if (!glfwInit())
	{
		fprintf(stderr, "Failed to initialize GLFW\n");
		return -1;
	}

	glfwWindowHint(GLFW_SAMPLES, 4);
	glfwWindowHint(GLFW_CONTEXT_VERSION_MAJOR, 3);
	glfwWindowHint(GLFW_CONTEXT_VERSION_MINOR, 3);
	glfwWindowHint(GLFW_OPENGL_PROFILE, GLFW_OPENGL_CORE_PROFILE);

	int w = 1024;
	int h = 768;
	window = glfwCreateWindow(w, h,
		"Tutorial 02 - Red triangle", NULL, NULL);
	if (window == NULL){
		fprintf(stderr, "Failed to open GLFW window.");
		glfwTerminate();
		return -1;
	}
	glfwMakeContextCurrent(window);
	glfwSetFramebufferSizeCallback(window, renderer::FramebufferSizeCallback);
	renderer::FramebufferSizeCallback(window, w, h);
	glewExperimental = true; // Needed for core profile
	if (glewInit() != GLEW_OK) {
		fprintf(stderr, "Failed to initialize GLEW\n");
		return -1;
	}

	glfwSetInputMode(window, GLFW_STICKY_KEYS, GL_TRUE);

	renderer::Init(window);

	do{
		renderer::Render();
		glfwPollEvents();
	} while (glfwGetKey(window, GLFW_KEY_ESCAPE) != GLFW_PRESS &&
		glfwWindowShouldClose(window) == 0);


	renderer::Uninit();
	glfwTerminate();

	//------------------------------------------------------
	// TESTI KOLMIO

	//if (!glfwInit())
	//{
	//	fprintf(stderr, "Failed to initialize GLFW\n");
	//	return -1;
	//}

	//glfwWindowHint(GLFW_SAMPLES, 4);
	//glfwWindowHint(GLFW_CONTEXT_VERSION_MAJOR, 3);
	//glfwWindowHint(GLFW_CONTEXT_VERSION_MINOR, 3);
	//glfwWindowHint(GLFW_OPENGL_PROFILE,
	//	GLFW_OPENGL_CORE_PROFILE);

	//window = glfwCreateWindow(1024, 768,
	//	"Tutorial 02 - Red triangle", NULL, NULL);
	//if (window == NULL){
	//	fprintf(stderr, "Failed to open GLFW window.");
	//	glfwTerminate();
	//	return -1;
	//}
	//glfwMakeContextCurrent(window);
	//glewExperimental = true; // Needed for core profile
	//if (glewInit() != GLEW_OK) {
	//	fprintf(stderr, "Failed to initialize GLEW\n");
	//	return -1;
	//}

	//glfwSetInputMode(window, GLFW_STICKY_KEYS, GL_TRUE);
	//Init();

	//do{
	//	Render();
	//	glfwPollEvents();
	//} while (glfwGetKey(window, GLFW_KEY_ESCAPE) != GLFW_PRESS &&
	//	glfwWindowShouldClose(window) == 0);

	//glfwTerminate();
  
	//---------------------------------------------------------

	system("pause");

	return 0;
}