$search
00001 /********************************************************************* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2010, Willow Garage, Inc. 00005 * All rights reserved. 00006 * 00007 * Redistribution and use in source and binary forms, with or without 00008 * modification, are permitted provided that the following conditions 00009 * are met: 00010 * 00011 * * Redistributions of source code must retain the above copyright 00012 * notice, this list of conditions and the following disclaimer. 00013 * * Redistributions in binary form must reproduce the above 00014 * copyright notice, this list of conditions and the following 00015 * disclaimer in the documentation and/or other materials provided 00016 * with the distribution. 00017 * * Neither the name of the Willow Garage nor the names of its 00018 * contributors may be used to endorse or promote products derived 00019 * from this software without specific prior written permission. 00020 * 00021 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00022 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00023 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00024 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00025 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00026 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00027 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00028 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00029 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00030 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00031 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00032 * POSSIBILITY OF SUCH DAMAGE. 00033 *********************************************************************/ 00034 00037 #include <collision_proximity/collision_proximity_types.h> 00038 00039 std::vector<collision_proximity::CollisionSphere> collision_proximity::determineCollisionSpheres(const bodies::Body* body, btTransform& relativeTransform) 00040 { 00041 std::vector<collision_proximity::CollisionSphere> css; 00042 00043 bodies::BoundingCylinder cyl; 00044 body->computeBoundingCylinder(cyl); 00045 unsigned int num_points = ceil(cyl.length/(cyl.radius/2.0)); 00046 double spacing = cyl.length/((num_points*1.0)-1.0); 00047 btVector3 vec(0.0,0.0,0.0); 00048 for(unsigned int i = 1; i < num_points-1; i++) { 00049 vec.setZ((-cyl.length/2.0)+i*spacing); 00050 btVector3 p = cyl.pose*vec; 00051 collision_proximity::CollisionSphere cs(vec,cyl.radius); 00052 css.push_back(cs); 00053 } 00054 relativeTransform = body->getPose().inverse() * cyl.pose; 00055 return css; 00056 } 00057 00058 std::vector<btVector3> collision_proximity::determineCollisionPoints(const bodies::Body* body, double resolution) 00059 { 00060 std::vector<btVector3> ret_vec; 00061 bodies::BoundingSphere sphere; 00062 body->computeBoundingSphere(sphere); 00063 //ROS_INFO_STREAM("Radius is " << sphere.radius); 00064 //ROS_INFO_STREAM("Center is " << sphere.center.z() << " " << sphere.center.y() << " " << sphere.center.z()); 00065 for(double xval = sphere.center.x()-sphere.radius-resolution; xval < sphere.center.x()+sphere.radius+resolution; xval += resolution) { 00066 for(double yval = sphere.center.y()-sphere.radius-resolution; yval < sphere.center.y()+sphere.radius+resolution; yval += resolution) { 00067 for(double zval = sphere.center.z()-sphere.radius-resolution; zval < sphere.center.z()+sphere.radius+resolution; zval += resolution) { 00068 btVector3 rel_vec(xval, yval, zval); 00069 if(body->containsPoint(body->getPose()*rel_vec)) { 00070 ret_vec.push_back(rel_vec); 00071 } 00072 } 00073 } 00074 } 00075 return ret_vec; 00076 } 00077 00078 bool collision_proximity::getCollisionSphereGradients(const distance_field::DistanceField<distance_field::PropDistanceFieldVoxel>* distance_field, 00079 const std::vector<CollisionSphere>& sphere_list, 00080 GradientInfo& gradient, 00081 double tolerance, 00082 bool subtract_radii, 00083 double maximum_value, 00084 bool stop_at_first_collision) { 00085 //assumes gradient is properly initialized 00086 bool in_collision = false; 00087 for(unsigned int i = 0; i < sphere_list.size(); i++) { 00088 btVector3 p = sphere_list[i].center_; 00089 double gx, gy, gz; 00090 double dist = distance_field->getDistanceGradient(p.x(), p.y(), p.z(), gx, gy, gz); 00091 if(dist < maximum_value && subtract_radii) { 00092 dist -= sphere_list[i].radius_; 00093 if(dist <= tolerance) { 00094 if(stop_at_first_collision) { 00095 return true; 00096 } 00097 in_collision = true; 00098 } 00099 } 00100 if(dist < gradient.closest_distance) { 00101 gradient.closest_distance = dist; 00102 } 00103 gradient.distances[i] = dist; 00104 gradient.gradients[i] = btVector3(gx,gy,gz); 00105 } 00106 return in_collision; 00107 } 00108 00109 bool collision_proximity::getCollisionSphereCollision(const distance_field::DistanceField<distance_field::PropDistanceFieldVoxel>* distance_field, 00110 const std::vector<CollisionSphere>& sphere_list, 00111 double tolerance) 00112 { 00113 for(unsigned int i = 0; i < sphere_list.size(); i++) { 00114 btVector3 p = sphere_list[i].center_; 00115 double gx, gy, gz; 00116 double dist = distance_field->getDistanceGradient(p.x(), p.y(), p.z(), gx, gy, gz); 00117 if(dist - sphere_list[i].radius_ < tolerance) { 00118 return true; 00119 } 00120 } 00121 return false; 00122 00123 } 00124 00128 00129 collision_proximity::BodyDecomposition::BodyDecomposition(const std::string& object_name, const shapes::Shape* shape, double resolution, double padding) : 00130 object_name_(object_name) 00131 { 00132 body_ = bodies::createBodyFromShape(shape); //unpadded 00133 btTransform ident; 00134 ident.setIdentity(); 00135 body_->setPose(ident); 00136 body_->setPadding(padding); 00137 collision_spheres_ = determineCollisionSpheres(body_, relative_cylinder_pose_); 00138 relative_collision_points_ = determineCollisionPoints(body_, resolution); 00139 posed_collision_points_ = relative_collision_points_; 00140 ROS_DEBUG_STREAM("Object " << object_name << " has " << relative_collision_points_.size() << " collision points"); 00141 } 00142 00143 collision_proximity::BodyDecomposition::~BodyDecomposition() 00144 { 00145 delete body_; 00146 } 00147 00148 void collision_proximity::BodyDecomposition::updateSpheresPose(const btTransform& trans) 00149 { 00150 //body_->setPose(trans); 00151 btTransform cylTransform = trans * relative_cylinder_pose_; 00152 for(unsigned int i = 0; i < collision_spheres_.size(); i++) { 00153 collision_spheres_[i].center_ = cylTransform*collision_spheres_[i].relative_vec_; 00154 } 00155 } 00156 00157 void collision_proximity::BodyDecomposition::updatePointsPose(const btTransform& trans) { 00158 //body_->setPose(trans); 00159 posed_collision_points_.clear(); 00160 posed_collision_points_.resize(relative_collision_points_.size()); 00161 for(unsigned int i = 0; i < relative_collision_points_.size(); i++) { 00162 posed_collision_points_[i] = trans*relative_collision_points_[i]; 00163 } 00164 } 00165 00166 00167 void collision_proximity::BodyDecomposition::updatePose(const btTransform& trans) 00168 { 00169 updateSpheresPose(trans); 00170 updatePointsPose(trans); 00171 }