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
00099 struct CostSource
00100 {
00102 boost::array<double, 3> aabb_min;
00103
00105 boost::array<double, 3> aabb_max;
00106
00108 double cost;
00109
00111 double getVolume() const
00112 {
00113 return (aabb_max[0] - aabb_min[0]) * (aabb_max[1] - aabb_min[1]) * (aabb_max[2] - aabb_min[2]);
00114 }
00115
00117 bool operator<(const CostSource &other) const
00118 {
00119 double c1 = cost * getVolume();
00120 double c2 = other.cost * other.getVolume();
00121 if (c1 > c2)
00122 return true;
00123 if (c1 < c2)
00124 return false;
00125 if (cost < other.cost)
00126 return false;
00127 if (cost > other.cost)
00128 return true;
00129 return aabb_min < other.aabb_min;
00130 }
00131 };
00132
00134 struct CollisionResult
00135 {
00136 CollisionResult() : collision(false),
00137 distance(std::numeric_limits<double>::max()),
00138 contact_count(0)
00139 {
00140 }
00141 typedef std::map<std::pair<std::string, std::string>, std::vector<Contact> > ContactMap;
00142
00143 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00144
00146 void clear()
00147 {
00148 collision = false;
00149 distance = std::numeric_limits<double>::max();
00150 contact_count = 0;
00151 contacts.clear();
00152 cost_sources.clear();
00153 }
00154
00156 bool collision;
00157
00159 double distance;
00160
00162 std::size_t contact_count;
00163
00165 ContactMap contacts;
00166
00168 std::set<CostSource> cost_sources;
00169 };
00170
00172 struct CollisionRequest
00173 {
00174 CollisionRequest() : distance(false),
00175 cost(false),
00176 contacts(false),
00177 max_contacts(1),
00178 max_contacts_per_pair(1),
00179 max_cost_sources(1),
00180 min_cost_density(0.2),
00181 verbose(false)
00182 {
00183 }
00184 virtual ~CollisionRequest() {}
00185
00187 std::string group_name;
00188
00190 bool distance;
00191
00193 bool cost;
00194
00196 bool contacts;
00197
00199 std::size_t max_contacts;
00200
00202 std::size_t max_contacts_per_pair;
00203
00205 std::size_t max_cost_sources;
00206
00208 double min_cost_density;
00209
00211 boost::function<bool(const CollisionResult&)>
00212 is_done;
00213
00215 bool verbose;
00216 };
00217
00218 }
00219
00220 #endif