Go to the documentation of this file.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 #include <shape_tools/shape_extents.h>
00036 #include <limits>
00037
00038 void shape_tools::getShapeExtents(const shape_msgs::SolidPrimitive& shape_msg, double& x_extent, double& y_extent, double& z_extent)
00039 {
00040 x_extent = y_extent = z_extent = 0.0;
00041
00042 if (shape_msg.type == shape_msgs::SolidPrimitive::SPHERE)
00043 {
00044 if (shape_msg.dimensions.size() > shape_msgs::SolidPrimitive::SPHERE_RADIUS)
00045 x_extent = y_extent = z_extent = shape_msg.dimensions[shape_msgs::SolidPrimitive::SPHERE_RADIUS] * 2.0;
00046 }
00047 else if (shape_msg.type == shape_msgs::SolidPrimitive::BOX)
00048 {
00049 if (shape_msg.dimensions.size() > shape_msgs::SolidPrimitive::BOX_X &&
00050 shape_msg.dimensions.size() > shape_msgs::SolidPrimitive::BOX_Y &&
00051 shape_msg.dimensions.size() > shape_msgs::SolidPrimitive::BOX_Z)
00052 {
00053 x_extent = shape_msg.dimensions[shape_msgs::SolidPrimitive::BOX_X];
00054 y_extent = shape_msg.dimensions[shape_msgs::SolidPrimitive::BOX_Y];
00055 z_extent = shape_msg.dimensions[shape_msgs::SolidPrimitive::BOX_Z];
00056 }
00057 }
00058 else if (shape_msg.type == shape_msgs::SolidPrimitive::CYLINDER)
00059 {
00060 if (shape_msg.dimensions.size() > shape_msgs::SolidPrimitive::CYLINDER_RADIUS &&
00061 shape_msg.dimensions.size() > shape_msgs::SolidPrimitive::CYLINDER_HEIGHT)
00062 {
00063 x_extent = y_extent = shape_msg.dimensions[shape_msgs::SolidPrimitive::CYLINDER_RADIUS] * 2.0;
00064 z_extent = shape_msg.dimensions[shape_msgs::SolidPrimitive::CYLINDER_HEIGHT];
00065 }
00066 }
00067 else if (shape_msg.type == shape_msgs::SolidPrimitive::CONE)
00068 {
00069 if (shape_msg.dimensions.size() > shape_msgs::SolidPrimitive::CONE_RADIUS &&
00070 shape_msg.dimensions.size() > shape_msgs::SolidPrimitive::CONE_HEIGHT)
00071 {
00072 x_extent = y_extent = shape_msg.dimensions[shape_msgs::SolidPrimitive::CONE_RADIUS] * 2.0;
00073 z_extent = shape_msg.dimensions[shape_msgs::SolidPrimitive::CONE_HEIGHT];
00074 }
00075 }
00076 }
00077
00078 void shape_tools::getShapeExtents(const shape_msgs::Mesh& shape_msg, double& x_extent, double& y_extent, double& z_extent)
00079 {
00080 x_extent = y_extent = z_extent = 0.0;
00081 if (shape_msg.vertices.size() > 0)
00082 {
00083 double xmin = std::numeric_limits<double>::max(), ymin = std::numeric_limits<double>::max(), zmin = std::numeric_limits<double>::max();
00084 double xmax = -std::numeric_limits<double>::max(), ymax = -std::numeric_limits<double>::max(), zmax = -std::numeric_limits<double>::max();
00085 for (std::size_t i = 0; i < shape_msg.vertices.size() ; ++i)
00086 {
00087 if (shape_msg.vertices[i].x > xmax)
00088 xmax = shape_msg.vertices[i].x;
00089 if (shape_msg.vertices[i].x < xmin)
00090 xmin = shape_msg.vertices[i].x;
00091 if (shape_msg.vertices[i].y > ymax)
00092 ymax = shape_msg.vertices[i].y;
00093 if (shape_msg.vertices[i].y < ymin)
00094 ymin = shape_msg.vertices[i].y;
00095 if (shape_msg.vertices[i].z > zmax)
00096 zmax = shape_msg.vertices[i].z;
00097 if (shape_msg.vertices[i].z < zmin)
00098 zmin = shape_msg.vertices[i].z;
00099 }
00100 x_extent = xmax-xmin;
00101 y_extent = ymax-ymin;
00102 z_extent = zmax-zmin;
00103 }
00104 }
00105