00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #include <ros/ros.h>
00038 #include <visualization_msgs/MarkerArray.h>
00039 #include <geometry_msgs/Quaternion.h>
00040
00041
00042 #include <moveit/semantic_world/semantic_world.h>
00043 #include <geometric_shapes/shape_operations.h>
00044 #include <moveit_msgs/PlanningScene.h>
00045
00046
00047 #include <opencv2/imgproc/imgproc.hpp>
00048
00049
00050 #include <eigen_conversions/eigen_msg.h>
00051 #include <Eigen/Geometry>
00052
00053 namespace moveit
00054 {
00055 namespace semantic_world
00056 {
00057
00058 SemanticWorld::SemanticWorld(const planning_scene::PlanningSceneConstPtr& planning_scene): planning_scene_(planning_scene)
00059 {
00060 table_subscriber_ = node_handle_.subscribe("table_array", 1, &SemanticWorld::tableCallback, this);
00061 visualization_publisher_ = node_handle_.advertise<visualization_msgs::MarkerArray>("visualize_place", 20, true);
00062 collision_object_publisher_ = node_handle_.advertise<moveit_msgs::CollisionObject>("/collision_object", 20);
00063 planning_scene_diff_publisher_ = node_handle_.advertise<moveit_msgs::PlanningScene>("planning_scene", 1);
00064 }
00065
00066 visualization_msgs::MarkerArray SemanticWorld::getPlaceLocationsMarker(const std::vector<geometry_msgs::PoseStamped> &poses) const
00067 {
00068 ROS_DEBUG("Visualizing: %d place poses", (int) poses.size());
00069 visualization_msgs::MarkerArray marker;
00070 for(std::size_t i=0; i < poses.size(); ++i)
00071 {
00072 visualization_msgs::Marker m;
00073 m.action = m.ADD;
00074 m.type = m.SPHERE;
00075 m.ns = "place_locations";
00076 m.id = i;
00077 m.pose = poses[i].pose;
00078 m.header = poses[i].header;
00079
00080 m.scale.x = 0.02;
00081 m.scale.y = 0.02;
00082 m.scale.z = 0.02;
00083
00084 m.color.r = 1.0;
00085 m.color.g = 0.0;
00086 m.color.b = 0.0;
00087 m.color.a = 1.0;
00088 marker.markers.push_back(m);
00089 }
00090 return marker;
00091 }
00092
00093 bool SemanticWorld::addTablesToCollisionWorld()
00094 {
00095 moveit_msgs::PlanningScene planning_scene;
00096 planning_scene.is_diff = true;
00097
00098
00099 std::map<std::string, object_recognition_msgs::Table>::iterator it;
00100 for(it = current_tables_in_collision_world_.begin(); it != current_tables_in_collision_world_.end(); ++it)
00101 {
00102 moveit_msgs::CollisionObject co;
00103 co.id = it->first;
00104 co.operation = moveit_msgs::CollisionObject::REMOVE;
00105 planning_scene.world.collision_objects.push_back(co);
00106
00107 }
00108
00109 planning_scene_diff_publisher_.publish(planning_scene);
00110 planning_scene.world.collision_objects.clear();
00111 current_tables_in_collision_world_.clear();
00112
00113 for(std::size_t i=0; i < table_array_.tables.size(); ++i)
00114 {
00115 moveit_msgs::CollisionObject co;
00116 std::stringstream ss;
00117 ss << "table_" << i;
00118 co.id = ss.str();
00119 current_tables_in_collision_world_[co.id] = table_array_.tables[i];
00120 co.operation = moveit_msgs::CollisionObject::ADD;
00121
00122 const std::vector<geometry_msgs::Point>& convex_hull = table_array_.tables[i].convex_hull;
00123
00124 EigenSTL::vector_Vector3d vertices(convex_hull.size());
00125 std::vector<unsigned int> triangles((vertices.size() - 2)* 3);
00126 for (unsigned int j = 0 ; j < convex_hull.size() ; ++j)
00127 vertices[j] = Eigen::Vector3d(convex_hull[j].x, convex_hull[j].y, convex_hull[j].z);
00128 for (unsigned int j = 1; j < triangles.size() - 1; ++j)
00129 {
00130 unsigned int i3 = j * 3;
00131 triangles[i3++] = 0;
00132 triangles[i3++] = j;
00133 triangles[i3] = j+1;
00134 }
00135
00136 shapes::Shape* table_shape = shapes::createMeshFromVertices(vertices, triangles);
00137 if(!table_shape)
00138 continue;
00139
00140 shapes::Mesh* table_mesh = static_cast<shapes::Mesh*>(table_shape);
00141 shapes::Mesh* table_mesh_solid = orientPlanarPolygon (*table_mesh);
00142 if(!table_mesh_solid)
00143 {
00144 delete table_shape;
00145 continue;
00146 }
00147
00148 shapes::ShapeMsg table_shape_msg;
00149 if(!shapes::constructMsgFromShape(table_mesh_solid, table_shape_msg))
00150 {
00151 delete table_shape;
00152 delete table_mesh_solid;
00153 continue;
00154 }
00155
00156 const shape_msgs::Mesh& table_shape_msg_mesh = boost::get<shape_msgs::Mesh> (table_shape_msg);
00157
00158 co.meshes.push_back(table_shape_msg_mesh);
00159 co.mesh_poses.push_back(table_array_.tables[i].pose);
00160 co.header = table_array_.tables[i].header;
00161 planning_scene.world.collision_objects.push_back(co);
00162
00163 delete table_shape;
00164 delete table_mesh_solid;
00165 }
00166 planning_scene_diff_publisher_.publish(planning_scene);
00167 return true;
00168 }
00169
00170 object_recognition_msgs::TableArray SemanticWorld::getTablesInROI(double minx, double miny, double minz,
00171 double maxx, double maxy, double maxz) const
00172 {
00173 object_recognition_msgs::TableArray tables_in_roi;
00174 std::map<std::string, object_recognition_msgs::Table>::const_iterator it;
00175 for(it = current_tables_in_collision_world_.begin(); it != current_tables_in_collision_world_.end(); ++it)
00176 {
00177 if(it->second.pose.position.x >= minx &&
00178 it->second.pose.position.x <= maxx &&
00179 it->second.pose.position.y >= miny &&
00180 it->second.pose.position.y <= maxy &&
00181 it->second.pose.position.z >= minz &&
00182 it->second.pose.position.z <= maxz)
00183 {
00184 tables_in_roi.tables.push_back(it->second);
00185 }
00186 }
00187 return tables_in_roi;
00188 }
00189
00190 std::vector<std::string> SemanticWorld::getTableNamesInROI(double minx, double miny, double minz,
00191 double maxx, double maxy, double maxz) const
00192 {
00193 std::vector<std::string> result;
00194 std::map<std::string, object_recognition_msgs::Table>::const_iterator it;
00195 for(it = current_tables_in_collision_world_.begin(); it != current_tables_in_collision_world_.end(); ++it)
00196 {
00197 if(it->second.pose.position.x >= minx &&
00198 it->second.pose.position.x <= maxx &&
00199 it->second.pose.position.y >= miny &&
00200 it->second.pose.position.y <= maxy &&
00201 it->second.pose.position.z >= minz &&
00202 it->second.pose.position.z <= maxz)
00203 {
00204 result.push_back(it->first);
00205 }
00206 }
00207 return result;
00208 }
00209
00210 void SemanticWorld::clear()
00211 {
00212 table_array_.tables.clear();
00213 current_tables_in_collision_world_.clear();
00214 }
00215
00216 std::vector<geometry_msgs::PoseStamped> SemanticWorld::generatePlacePoses(const std::string &table_name,
00217 const shapes::ShapeConstPtr& object_shape,
00218 const geometry_msgs::Quaternion &object_orientation,
00219 double resolution,
00220 double delta_height,
00221 unsigned int num_heights) const
00222 {
00223 object_recognition_msgs::Table chosen_table;
00224 std::map<std::string, object_recognition_msgs::Table>::const_iterator it = current_tables_in_collision_world_.find(table_name);
00225
00226 if(it != current_tables_in_collision_world_.end())
00227 {
00228 chosen_table = it->second;
00229 return generatePlacePoses(chosen_table, object_shape, object_orientation, resolution, delta_height, num_heights);
00230 }
00231
00232 std::vector<geometry_msgs::PoseStamped> place_poses;
00233 ROS_ERROR("Did not find table %s to place on", table_name.c_str());
00234 return place_poses;
00235 }
00236
00237 std::vector<geometry_msgs::PoseStamped> SemanticWorld::generatePlacePoses(const object_recognition_msgs::Table &chosen_table,
00238 const shapes::ShapeConstPtr& object_shape,
00239 const geometry_msgs::Quaternion &object_orientation,
00240 double resolution,
00241 double delta_height,
00242 unsigned int num_heights) const
00243 {
00244 std::vector<geometry_msgs::PoseStamped> place_poses;
00245 if(object_shape->type != shapes::MESH && object_shape->type != shapes::SPHERE
00246 && object_shape->type != shapes::BOX && object_shape->type != shapes::CONE)
00247 {
00248 return place_poses;
00249 }
00250
00251 double x_min(std::numeric_limits<double>::max()), x_max(-std::numeric_limits<double>::max());
00252 double y_min(std::numeric_limits<double>::max()), y_max(-std::numeric_limits<double>::max());
00253 double z_min(std::numeric_limits<double>::max()), z_max(-std::numeric_limits<double>::max());
00254
00255 Eigen::Quaterniond rotation(object_orientation.x, object_orientation.y, object_orientation.z, object_orientation.w);
00256 Eigen::Affine3d object_pose(rotation);
00257 double min_distance_from_edge;
00258 double height_above_table;
00259
00260 if(object_shape->type == shapes::MESH)
00261 {
00262 const shapes::Mesh* mesh = static_cast<const shapes::Mesh*>(object_shape.get());
00263
00264 for(std::size_t i=0; i < mesh->vertex_count; ++i)
00265 {
00266 Eigen::Vector3d position(mesh->vertices[3*i],
00267 mesh->vertices[3*i+1],
00268 mesh->vertices[3*i+2]);
00269 position = object_pose * position;
00270
00271 if(x_min > position.x())
00272 x_min = position.x();
00273 if(x_max < position.x())
00274 x_max = position.x();
00275 if(y_min > position.y())
00276 y_min = position.y();
00277 if(y_max < position.y())
00278 y_max = position.y();
00279 if(z_min > position.z())
00280 z_min = position.z();
00281 if(z_max < position.z())
00282 z_max = position.z();
00283 }
00284 min_distance_from_edge = 0.5 * std::max<double>(fabs(x_max-x_min), fabs(y_max-y_min));
00285 height_above_table = -z_min;
00286 }
00287 else if(object_shape->type == shapes::BOX)
00288 {
00289 const shapes::Box* box = static_cast<const shapes::Box*>(object_shape.get());
00290 min_distance_from_edge = std::max<double>(fabs(box->size[0]), fabs(box->size[1]))/2.0;
00291 height_above_table = fabs(box->size[2])/2.0;
00292 }
00293 else if(object_shape->type == shapes::SPHERE)
00294 {
00295 const shapes::Sphere* sphere = static_cast<const shapes::Sphere*>(object_shape.get());
00296 min_distance_from_edge = sphere->radius;
00297 height_above_table = -sphere->radius;
00298 }
00299 else if(object_shape->type == shapes::CYLINDER)
00300 {
00301 const shapes::Cylinder* cylinder = static_cast<const shapes::Cylinder*>(object_shape.get());
00302 min_distance_from_edge = cylinder->radius;
00303 height_above_table = cylinder->length/2.0;
00304 }
00305 else if(object_shape->type == shapes::CONE)
00306 {
00307 const shapes::Cone* cone = static_cast<const shapes::Cone*>(object_shape.get());
00308 min_distance_from_edge = cone->radius;
00309 height_above_table = cone->length/2.0;
00310 }
00311
00312 return generatePlacePoses(chosen_table, resolution, height_above_table, delta_height, num_heights, min_distance_from_edge);
00313 }
00314
00315 std::vector<geometry_msgs::PoseStamped> SemanticWorld::generatePlacePoses(const object_recognition_msgs::Table &table,
00316 double resolution,
00317 double height_above_table,
00318 double delta_height,
00319 unsigned int num_heights,
00320 double min_distance_from_edge) const
00321 {
00322 std::vector<geometry_msgs::PoseStamped> place_poses;
00323
00324 if(table.convex_hull.empty())
00325 return place_poses;
00326 const int scale_factor = 100;
00327 std::vector<cv::Point2f> table_contour;
00328 float x_min=table.convex_hull[0].x, x_max=x_min, y_min=table.convex_hull[0].y, y_max=y_min;
00329 for(std::size_t j=1; j < table.convex_hull.size(); ++j)
00330 {
00331 if (table.convex_hull[j].x < x_min)
00332 x_min=table.convex_hull[j].x;
00333 else if (table.convex_hull[j].x > x_max)
00334 x_max=table.convex_hull[j].x;
00335 if (table.convex_hull[j].y < y_min)
00336 y_min=table.convex_hull[j].y;
00337 else if (table.convex_hull[j].y > y_max)
00338 y_max=table.convex_hull[j].y;
00339 }
00340 for(std::size_t j=0; j < table.convex_hull.size(); ++j)
00341 table_contour.push_back(cv::Point((table.convex_hull[j].x-x_min)*scale_factor,
00342 (table.convex_hull[j].y-y_min)*scale_factor));
00343
00344 double x_range = fabs(x_max-x_min);
00345 double y_range = fabs(y_max-y_min);
00346 int max_range = (int) x_range + 1;
00347 if(max_range < (int) y_range + 1)
00348 max_range = (int) y_range + 1;
00349
00350 int image_scale = std::max<int>(max_range, 4);
00351 cv::Mat src = cv::Mat::zeros(image_scale*scale_factor, image_scale*scale_factor, CV_8UC1);
00352
00353 for(std::size_t j = 0; j < table.convex_hull.size(); ++j )
00354 {
00355 cv::line(src, table_contour[j], table_contour[(j+1)%table.convex_hull.size()],
00356 cv::Scalar( 255 ), 3, 8 );
00357 }
00358
00359 unsigned int num_x = fabs(x_max-x_min)/resolution + 1;
00360 unsigned int num_y = fabs(y_max-y_min)/resolution + 1;
00361
00362 ROS_DEBUG("Num points for possible place operations: %d %d", num_x, num_y);
00363
00364 std::vector<std::vector<cv::Point> > contours;
00365 std::vector<cv::Vec4i> hierarchy;
00366 cv::findContours(src, contours, hierarchy, cv::RETR_TREE, cv::CHAIN_APPROX_SIMPLE);
00367
00368 for(std::size_t j=0; j < num_x; ++j)
00369 {
00370 int point_x = j * resolution * scale_factor;
00371 for(std::size_t k=0; k < num_y; ++k)
00372 {
00373 for(std::size_t mm=0; mm < num_heights; ++mm)
00374 {
00375 int point_y = k * resolution * scale_factor;
00376 cv::Point2f point2f(point_x, point_y);
00377 double result = cv::pointPolygonTest(contours[0], point2f, true);
00378 if((int) result >= (int) (min_distance_from_edge*scale_factor))
00379 {
00380 Eigen::Vector3d point((double) (point_x)/scale_factor + x_min,
00381 (double) (point_y)/scale_factor + y_min,
00382 height_above_table + mm * delta_height);
00383 Eigen::Affine3d pose;
00384 tf::poseMsgToEigen(table.pose, pose);
00385 point = pose * point;
00386 geometry_msgs::PoseStamped place_pose;
00387 place_pose.pose.orientation.w = 1.0;
00388 place_pose.pose.position.x = point.x();
00389 place_pose.pose.position.y = point.y();
00390 place_pose.pose.position.z = point.z();
00391 place_pose.header = table.header;
00392 place_poses.push_back(place_pose);
00393 }
00394 }
00395 }
00396 }
00397 return place_poses;
00398 }
00399
00400 bool SemanticWorld::isInsideTableContour(const geometry_msgs::Pose &pose,
00401 const object_recognition_msgs::Table &table,
00402 double min_distance_from_edge,
00403 double min_vertical_offset) const
00404 {
00405
00406 if(table.convex_hull.empty())
00407 return false;
00408 float x_min=table.convex_hull[0].x, x_max=x_min, y_min=table.convex_hull[0].y, y_max=y_min;
00409 for(std::size_t j=1; j < table.convex_hull.size(); ++j)
00410 {
00411 if (table.convex_hull[j].x < x_min)
00412 x_min=table.convex_hull[j].x;
00413 else if (table.convex_hull[j].x > x_max)
00414 x_max=table.convex_hull[j].x;
00415 if (table.convex_hull[j].y < y_min)
00416 y_min=table.convex_hull[j].y;
00417 else if (table.convex_hull[j].y > y_max)
00418 y_max=table.convex_hull[j].y;
00419 }
00420 const int scale_factor = 100;
00421 std::vector<cv::Point2f> table_contour;
00422 for(std::size_t j=0; j < table.convex_hull.size(); ++j)
00423 table_contour.push_back(cv::Point((table.convex_hull[j].x-x_min)*scale_factor,
00424 (table.convex_hull[j].y-y_min)*scale_factor));
00425
00426 double x_range = fabs(x_max-x_min);
00427 double y_range = fabs(y_max-y_min);
00428 int max_range = (int) x_range + 1;
00429 if(max_range < (int) y_range + 1)
00430 max_range = (int) y_range + 1;
00431
00432 int image_scale = std::max<int>(max_range, 4);
00433 cv::Mat src = cv::Mat::zeros(image_scale*scale_factor, image_scale*scale_factor, CV_8UC1);
00434
00435 for(std::size_t j = 0; j < table.convex_hull.size(); ++j )
00436 {
00437 cv::line(src, table_contour[j], table_contour[(j+1)%table.convex_hull.size()],
00438 cv::Scalar( 255 ), 3, 8 );
00439 }
00440
00441 std::vector<std::vector<cv::Point> > contours;
00442 std::vector<cv::Vec4i> hierarchy;
00443 cv::findContours(src, contours, hierarchy, cv::RETR_TREE, cv::CHAIN_APPROX_SIMPLE);
00444
00445 Eigen::Vector3d point(pose.position.x, pose.position.y, pose.position.z);
00446 Eigen::Affine3d pose_table;
00447 tf::poseMsgToEigen(table.pose, pose_table);
00448
00449
00450 point = pose_table.inverse() * point;
00451
00452 if(point.z() < -fabs(min_vertical_offset))
00453 {
00454 ROS_ERROR("Object is not above table");
00455 return false;
00456 }
00457
00458 int point_x = (point.x() -x_min) * scale_factor;
00459 int point_y = (point.y() -y_min) * scale_factor;
00460 cv::Point2f point2f(point_x, point_y);
00461 double result = cv::pointPolygonTest(contours[0], point2f, true);
00462 ROS_DEBUG("table distance: %f", result);
00463
00464 if((int) result >= (int) (min_distance_from_edge*scale_factor))
00465 return true;
00466
00467 return false;
00468 }
00469
00470 std::string SemanticWorld::findObjectTable(const geometry_msgs::Pose &pose,
00471 double min_distance_from_edge,
00472 double min_vertical_offset) const
00473 {
00474 std::map<std::string, object_recognition_msgs::Table>::const_iterator it;
00475 for(it = current_tables_in_collision_world_.begin(); it != current_tables_in_collision_world_.end(); ++it)
00476 {
00477 ROS_DEBUG("Testing table: %s", it->first.c_str());
00478 if(isInsideTableContour(pose, it->second, min_distance_from_edge, min_vertical_offset))
00479 return it->first;
00480 }
00481 return std::string();
00482 }
00483
00484
00485 void SemanticWorld::tableCallback(const object_recognition_msgs::TableArrayPtr &msg)
00486 {
00487 table_array_ = *msg;
00488 ROS_INFO("Table callback with %d tables", (int) table_array_.tables.size());
00489 transformTableArray(table_array_);
00490
00491 if(table_callback_)
00492 {
00493 ROS_INFO("Calling table callback");
00494 table_callback_();
00495 }
00496 }
00497
00498 void SemanticWorld::transformTableArray(object_recognition_msgs::TableArray &table_array) const
00499 {
00500 for(std::size_t i=0; i < table_array.tables.size(); ++i)
00501 {
00502 std::string original_frame = table_array.tables[i].header.frame_id;
00503 if(table_array.tables[i].convex_hull.empty())
00504 continue;
00505 ROS_INFO_STREAM("Original pose: " << table_array.tables[i].pose.position.x << ","
00506 << table_array.tables[i].pose.position.y << ","
00507 << table_array.tables[i].pose.position.z);
00508 std::string error_text;
00509 const Eigen::Affine3d& original_transform = planning_scene_->getTransforms().getTransform(original_frame);
00510 Eigen::Affine3d original_pose;
00511 tf::poseMsgToEigen(table_array.tables[i].pose, original_pose);
00512 original_pose = original_transform * original_pose;
00513 tf::poseEigenToMsg(original_pose, table_array.tables[i].pose);
00514 table_array.tables[i].header.frame_id = planning_scene_->getTransforms().getTargetFrame();
00515 ROS_INFO_STREAM("Successfully transformed table array from " << original_frame <<
00516 "to " << table_array.tables[i].header.frame_id);
00517 ROS_INFO_STREAM("Transformed pose: " << table_array.tables[i].pose.position.x << ","
00518 << table_array.tables[i].pose.position.y << ","
00519 << table_array.tables[i].pose.position.z);
00520 }
00521 }
00522
00523 shapes::Mesh* SemanticWorld::orientPlanarPolygon (const shapes::Mesh& polygon) const
00524 {
00525 if (polygon.vertex_count < 3 || polygon.triangle_count < 1)
00526 return 0;
00527
00528 Eigen::Vector3d vec1, vec2, vec3, normal;
00529
00530 int vIdx1 = polygon.triangles [0];
00531 int vIdx2 = polygon.triangles [1];
00532 int vIdx3 = polygon.triangles [2];
00533 vec1 = Eigen::Vector3d (polygon.vertices [vIdx1 * 3], polygon.vertices [vIdx1 * 3 + 1], polygon.vertices [vIdx1* 3 + 2]);
00534 vec2 = Eigen::Vector3d (polygon.vertices [vIdx2 * 3], polygon.vertices [vIdx2 * 3 + 1], polygon.vertices [vIdx2* 3 + 2]);
00535 vec3 = Eigen::Vector3d (polygon.vertices [vIdx3 * 3], polygon.vertices [vIdx3 * 3 + 1], polygon.vertices [vIdx3* 3 + 2]);
00536 vec2 -= vec1;
00537 vec3 -= vec1;
00538 normal = vec3.cross(vec2);
00539
00540 if (normal [2] < 0.0)
00541 normal *= -1.0;
00542
00543 normal.normalize();
00544
00545 shapes::Mesh* solid = new shapes::Mesh(polygon.vertex_count, polygon.triangle_count);
00546 solid->type = shapes::MESH;
00547
00548
00549 memcpy (solid->vertices, polygon.vertices, polygon.vertex_count * 3 * sizeof(double));
00550
00551 memcpy (solid->triangles, polygon.triangles, polygon.triangle_count * 3 * sizeof(unsigned int));
00552
00553 for (unsigned tIdx = 0; tIdx < polygon.triangle_count; ++tIdx)
00554 {
00555 int vIdx1 = polygon.triangles [tIdx*3];
00556 int vIdx2 = polygon.triangles [tIdx*3+1];
00557 int vIdx3 = polygon.triangles [tIdx*3+2];
00558
00559 vec1 = Eigen::Vector3d (polygon.vertices [vIdx1 * 3], polygon.vertices [vIdx1 * 3 + 1], polygon.vertices [vIdx1* 3 + 2]);
00560 vec2 = Eigen::Vector3d (polygon.vertices [vIdx2 * 3], polygon.vertices [vIdx2 * 3 + 1], polygon.vertices [vIdx2* 3 + 2]);
00561 vec3 = Eigen::Vector3d (polygon.vertices [vIdx3 * 3], polygon.vertices [vIdx3 * 3 + 1], polygon.vertices [vIdx3* 3 + 2]);
00562
00563 vec2 -= vec1;
00564 vec3 -= vec1;
00565
00566 Eigen::Vector3d triangle_normal = vec2.cross(vec1);
00567
00568 if (triangle_normal.dot (normal) < 0.0)
00569 std::swap (solid->triangles [tIdx*3 + 1], solid->triangles [tIdx*3 + 2]);
00570
00571 }
00572 return solid;
00573 }
00574
00575 shapes::Mesh* SemanticWorld::createSolidMeshFromPlanarPolygon (const shapes::Mesh& polygon, double thickness) const
00576 {
00577 if (polygon.vertex_count < 3 || polygon.triangle_count < 1 || thickness <= 0)
00578 return 0;
00579
00580 Eigen::Vector3d vec1, vec2, vec3, normal;
00581
00582 int vIdx1 = polygon.triangles [0];
00583 int vIdx2 = polygon.triangles [1];
00584 int vIdx3 = polygon.triangles [2];
00585 vec1 = Eigen::Vector3d (polygon.vertices [vIdx1 * 3], polygon.vertices [vIdx1 * 3 + 1], polygon.vertices [vIdx1* 3 + 2]);
00586 vec2 = Eigen::Vector3d (polygon.vertices [vIdx2 * 3], polygon.vertices [vIdx2 * 3 + 1], polygon.vertices [vIdx2* 3 + 2]);
00587 vec3 = Eigen::Vector3d (polygon.vertices [vIdx3 * 3], polygon.vertices [vIdx3 * 3 + 1], polygon.vertices [vIdx3* 3 + 2]);
00588 vec2 -= vec1;
00589 vec3 -= vec1;
00590 normal = vec3.cross(vec2);
00591
00592 if (normal [2] < 0.0)
00593 normal *= -1.0;
00594
00595 normal.normalize();
00596
00597
00598
00599 shapes::Mesh* solid = new shapes::Mesh(polygon.vertex_count * 2, polygon.triangle_count * 2);
00600 solid->type = shapes::MESH;
00601
00602
00603 memcpy (solid->vertices, polygon.vertices, polygon.vertex_count * 3 * sizeof(double));
00604
00605 memcpy (solid->triangles, polygon.triangles, polygon.triangle_count * 3 * sizeof(unsigned int));
00606
00607 for (unsigned tIdx = 0; tIdx < polygon.triangle_count; ++tIdx)
00608 {
00609 solid->triangles [(tIdx + polygon.triangle_count) * 3 + 0] = solid->triangles [tIdx * 3 + 0] + polygon.vertex_count;
00610 solid->triangles [(tIdx + polygon.triangle_count) * 3 + 1] = solid->triangles [tIdx * 3 + 1] + polygon.vertex_count;
00611 solid->triangles [(tIdx + polygon.triangle_count) * 3 + 2] = solid->triangles [tIdx * 3 + 2] + polygon.vertex_count;
00612
00613 int vIdx1 = polygon.triangles [tIdx*3];
00614 int vIdx2 = polygon.triangles [tIdx*3+1];
00615 int vIdx3 = polygon.triangles [tIdx*3+2];
00616
00617 vec1 = Eigen::Vector3d (polygon.vertices [vIdx1 * 3], polygon.vertices [vIdx1 * 3 + 1], polygon.vertices [vIdx1* 3 + 2]);
00618 vec2 = Eigen::Vector3d (polygon.vertices [vIdx2 * 3], polygon.vertices [vIdx2 * 3 + 1], polygon.vertices [vIdx2* 3 + 2]);
00619 vec3 = Eigen::Vector3d (polygon.vertices [vIdx3 * 3], polygon.vertices [vIdx3 * 3 + 1], polygon.vertices [vIdx3* 3 + 2]);
00620
00621 vec2 -= vec1;
00622 vec3 -= vec1;
00623
00624 Eigen::Vector3d triangle_normal = vec2.cross(vec1);
00625
00626 if (triangle_normal.dot (normal) < 0.0)
00627 std::swap (solid->triangles [tIdx*3 + 1], solid->triangles [tIdx*3 + 2]);
00628 else
00629 std::swap (solid->triangles [(tIdx + polygon.triangle_count) * 3 + 1], solid->triangles [(tIdx + polygon.triangle_count) * 3 + 2]);
00630 }
00631
00632 for (unsigned vIdx = 0; vIdx < polygon.vertex_count; ++vIdx)
00633 {
00634 solid->vertices [(vIdx + polygon.vertex_count) * 3 + 0] = solid->vertices [vIdx * 3 + 0] - thickness * normal [0];
00635 solid->vertices [(vIdx + polygon.vertex_count) * 3 + 1] = solid->vertices [vIdx * 3 + 1] - thickness * normal [1];
00636 solid->vertices [(vIdx + polygon.vertex_count) * 3 + 2] = solid->vertices [vIdx * 3 + 2] - thickness * normal [2];
00637 }
00638
00639 return solid;
00640 }
00641 }
00642
00643 }