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;
}
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;
}
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;
}
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;
}
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();
}
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);
}
// 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);
}
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);
}
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);
}
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);
}
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);
}
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;
}