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
00036
00037 #ifndef MOVEIT_COLLISION_DETECTION_COLLISION_COMMON_
00038 #define MOVEIT_COLLISION_DETECTION_COLLISION_COMMON_
00039
00040 #include <boost/array.hpp>
00041 #include <boost/function.hpp>
00042 #include <vector>
00043 #include <string>
00044 #include <map>
00045 #include <set>
00046 #include <Eigen/Core>
00047 #include <console_bridge/console.h>
00048
00049 namespace collision_detection
00050 {
00052 namespace BodyTypes
00053 {
00055 enum Type
00056 {
00058 ROBOT_LINK,
00059
00061 ROBOT_ATTACHED,
00062
00064 WORLD_OBJECT
00065 };
00066 }
00067
00069 typedef BodyTypes::Type BodyType;
00070
00072 struct Contact
00073 {
00074 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00075
00077 Eigen::Vector3d pos;
00078
00080 Eigen::Vector3d normal;
00081
00083 double depth;
00084
00086 std::string body_name_1;
00087
00089 BodyType body_type_1;
00090
00092 std::string body_name_2;
00093
00095 BodyType body_type_2;
00096 };
00097
00100 struct CostSource
00101 {
00103 boost::array<double, 3> aabb_min;
00104
00106 boost::array<double, 3> aabb_max;
00107
00109 double cost;
00110
00112 double getVolume() const
00113 {
00114 return (aabb_max[0] - aabb_min[0]) * (aabb_max[1] - aabb_min[1]) * (aabb_max[2] - aabb_min[2]);
00115 }
00116
00118 bool operator<(const CostSource& other) const
00119 {
00120 double c1 = cost * getVolume();
00121 double c2 = other.cost * other.getVolume();
00122 if (c1 > c2)
00123 return true;
00124 if (c1 < c2)
00125 return false;
00126 if (cost < other.cost)
00127 return false;
00128 if (cost > other.cost)
00129 return true;
00130 return aabb_min < other.aabb_min;
00131 }
00132 };
00133
00135 struct CollisionResult
00136 {
00137 CollisionResult() : collision(false), distance(std::numeric_limits<double>::max()), contact_count(0)
00138 {
00139 }
00140 typedef std::map<std::pair<std::string, std::string>, std::vector<Contact> > ContactMap;
00141
00142 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00143
00145 void clear()
00146 {
00147 collision = false;
00148 distance = std::numeric_limits<double>::max();
00149 contact_count = 0;
00150 contacts.clear();
00151 cost_sources.clear();
00152 }
00153
00155 bool collision;
00156
00158 double distance;
00159
00161 std::size_t contact_count;
00162
00165 ContactMap contacts;
00166
00168 std::set<CostSource> cost_sources;
00169 };
00170
00172 struct CollisionRequest
00173 {
00174 CollisionRequest()
00175 : distance(false)
00176 , cost(false)
00177 , contacts(false)
00178 , max_contacts(1)
00179 , max_contacts_per_pair(1)
00180 , max_cost_sources(1)
00181 , min_cost_density(0.2)
00182 , verbose(false)
00183 {
00184 }
00185 virtual ~CollisionRequest()
00186 {
00187 }
00188
00190 std::string group_name;
00191
00193 bool distance;
00194
00196 bool cost;
00197
00199 bool contacts;
00200
00202 std::size_t max_contacts;
00203
00206 std::size_t max_contacts_per_pair;
00207
00209 std::size_t max_cost_sources;
00210
00212 double min_cost_density;
00213
00215 boost::function<bool(const CollisionResult&)> is_done;
00216
00218 bool verbose;
00219 };
00220 }
00221
00222 #endif