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
00037 #include <geometric_shapes/body_operations.h>
00038 #include <geometric_shapes/shape_operations.h>
00039 #include <console_bridge/console.h>
00040 #include <Eigen/Geometry>
00041
00042 bodies::Body* bodies::createBodyFromShape(const shapes::Shape *shape)
00043 {
00044 Body *body = NULL;
00045
00046 if (shape)
00047 switch (shape->type)
00048 {
00049 case shapes::BOX:
00050 body = new bodies::Box(shape);
00051 break;
00052 case shapes::SPHERE:
00053 body = new bodies::Sphere(shape);
00054 break;
00055 case shapes::CYLINDER:
00056 body = new bodies::Cylinder(shape);
00057 break;
00058 case shapes::MESH:
00059 body = new bodies::ConvexMesh(shape);
00060 break;
00061 default:
00062 logError("Creating body from shape: Unknown shape type %d", (int)shape->type);
00063 break;
00064 }
00065
00066 return body;
00067 }
00068
00069 void bodies::mergeBoundingSpheres(const std::vector<BoundingSphere> &spheres, BoundingSphere &mergedSphere)
00070 {
00071 if (spheres.empty())
00072 {
00073 mergedSphere.center = Eigen::Vector3d(0.0, 0.0, 0.0);
00074 mergedSphere.radius = 0.0;
00075 }
00076 else
00077 {
00078 mergedSphere = spheres[0];
00079 for (unsigned int i = 1 ; i < spheres.size() ; ++i)
00080 {
00081 if (spheres[i].radius <= 0.0)
00082 continue;
00083 Eigen::Vector3d diff = spheres[i].center-mergedSphere.center;
00084 double d = diff.norm();
00085 if (d + mergedSphere.radius <= spheres[i].radius)
00086 {
00087 mergedSphere.center = spheres[i].center;
00088 mergedSphere.radius = spheres[i].radius;
00089 }
00090 else if (d + spheres[i].radius > mergedSphere.radius)
00091 {
00092 Eigen::Vector3d delta = mergedSphere.center - spheres[i].center;
00093 mergedSphere.radius = (delta.norm() + spheres[i].radius + mergedSphere.radius)/2.0;
00094 mergedSphere.center = delta.normalized() * (mergedSphere.radius - spheres[i].radius) + spheres[i].center;
00095 }
00096 }
00097 }
00098 }
00099
00100 namespace bodies
00101 {
00102 template<typename T>
00103 Body* constructBodyFromMsgHelper(const T &shape_msg, const geometry_msgs::Pose &pose)
00104 {
00105 shapes::Shape *shape = shapes::constructShapeFromMsg(shape_msg);
00106
00107 if (shape)
00108 {
00109 Body *body = createBodyFromShape(shape);
00110 if (body)
00111 {
00112 Eigen::Quaterniond q(pose.orientation.w, pose.orientation.x, pose.orientation.y, pose.orientation.z);
00113 if (fabs(q.squaredNorm() - 1.0) > 1e-3)
00114 {
00115 logError("Quaternion is not normalized. Assuming identity.");
00116 q = Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0);
00117 }
00118 Eigen::Affine3d af(Eigen::Translation3d(pose.position.x, pose.position.y, pose.position.z) * q.toRotationMatrix());
00119 body->setPose(af);
00120 return body;
00121 }
00122 }
00123 return NULL;
00124 }
00125 }
00126
00127 bodies::Body* bodies::constructBodyFromMsg(const shapes::ShapeMsg &shape_msg, const geometry_msgs::Pose &pose)
00128 {
00129 return constructBodyFromMsgHelper(shape_msg, pose);
00130 }
00131
00132 bodies::Body* bodies::constructBodyFromMsg(const shape_msgs::Mesh &shape_msg, const geometry_msgs::Pose &pose)
00133 {
00134 return constructBodyFromMsgHelper(shape_msg, pose);
00135 }
00136
00137 bodies::Body* bodies::constructBodyFromMsg(const shape_msgs::SolidPrimitive &shape_msg, const geometry_msgs::Pose &pose)
00138 {
00139 return constructBodyFromMsgHelper(shape_msg, pose);
00140 }
00141
00142 void bodies::computeBoundingSphere(const std::vector<const bodies::Body*>& bodies, bodies::BoundingSphere& sphere) {
00143 Eigen::Vector3d sum(0.0,0.0,0.0);
00144
00145
00146 unsigned int vertex_count = 0;
00147 for(unsigned int i = 0; i < bodies.size(); i++) {
00148 const bodies::ConvexMesh* conv = dynamic_cast<const bodies::ConvexMesh*>(bodies[i]);
00149 if(!conv) continue;
00150 for(unsigned int j = 0; j < conv->getScaledVertices().size(); j++, vertex_count++) {
00151 sum += conv->getPose()*conv->getScaledVertices()[j];
00152 }
00153 }
00154
00155 sphere.center=sum/(double)vertex_count;
00156
00157 double max_dist_squared = 0.0;
00158 for(unsigned int i = 0; i < bodies.size(); i++) {
00159 const bodies::ConvexMesh* conv = dynamic_cast<const bodies::ConvexMesh*>(bodies[i]);
00160 if(!conv) continue;
00161 for(unsigned int j = 0; j < conv->getScaledVertices().size(); j++) {
00162 double dist = (conv->getPose()*conv->getScaledVertices()[j]-sphere.center).squaredNorm();
00163 if(dist > max_dist_squared) {
00164 max_dist_squared = dist;
00165 }
00166 }
00167 }
00168 sphere.radius = sqrt(max_dist_squared);
00169 }