openrave.org

 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
collisionchecker.h
Go to the documentation of this file.
1 // -*- coding: utf-8 -*-
2 // Copyright (C) 2006-2011 Rosen Diankov <rosen.diankov@gmail.com>
3 //
4 // This file is part of OpenRAVE.
5 // OpenRAVE is free software: you can redistribute it and/or modify
6 // it under the terms of the GNU Lesser General Public License as published by
7 // the Free Software Foundation, either version 3 of the License, or
8 // at your option) any later version.
9 //
10 // This program is distributed in the hope that it will be useful,
11 // but WITHOUT ANY WARRANTY; without even the implied warranty of
12 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13 // GNU Lesser General Public License for more details.
14 //
15 // You should have received a copy of the GNU Lesser General Public License
16 // along with this program. If not, see <http://www.gnu.org/licenses/>.
22 #ifndef OPENRAVE_COLLISIONCHECKER_H
23 #define OPENRAVE_COLLISIONCHECKER_H
24 
25 namespace OpenRAVE {
26 
29 {
34 
42 };
43 
46 {
48  CA_Ignore = 1,
49 };
50 
53 {
54 public:
56  Reset();
57  }
58 
60  {
61 public:
62  CONTACT() : depth(0) {
63  }
64  CONTACT(const Vector &p, const Vector &n, dReal d) : pos(p), norm(n) {
65  depth = d;
66  }
67 
71  };
72 
73  int options;
74 
76 
77  //KinBody::Link::GeomConstPtr pgeom1, pgeom2; ///< the specified geometries hit for the given links
78  int numCols;
79  std::vector<KinBody::LinkConstPtr> vLinkColliding;
80 
83 
84  std::vector<CONTACT> contacts;
85 
86  virtual void Reset(int coloptions = 0);
87  virtual std::string __str__() const;
88 };
89 
90 typedef CollisionReport COLLISIONREPORT RAVE_DEPRECATED;
91 
96 {
97 public:
98  CollisionCheckerBase(EnvironmentBasePtr penv) : InterfaceBase(PT_CollisionChecker, penv) {
99  }
101  }
102 
104  static inline InterfaceType GetInterfaceTypeStatic() {
105  return PT_CollisionChecker;
106  }
107 
109  virtual bool SetCollisionOptions(int collisionoptions) = 0;
110 
112  virtual int GetCollisionOptions() const = 0;
113 
114  virtual void SetTolerance(dReal tolerance) = 0;
115 
117  virtual bool InitEnvironment() = 0;
118 
120  virtual void DestroyEnvironment() = 0;
121 
123  virtual bool InitKinBody(KinBodyPtr pbody) = 0;
124 
126  virtual void RemoveKinBody(KinBodyPtr pbody) = 0;
127 
131 
132 
134  virtual bool CheckCollision(KinBodyConstPtr pbody1, CollisionReportPtr report = CollisionReportPtr())=0;
135 
137  virtual bool CheckCollision(KinBodyConstPtr pbody1, KinBodyConstPtr pbody2, CollisionReportPtr report = CollisionReportPtr())=0;
138 
140  virtual bool CheckCollision(KinBody::LinkConstPtr plink, CollisionReportPtr report = CollisionReportPtr())=0;
141 
143  virtual bool CheckCollision(KinBody::LinkConstPtr plink1, KinBody::LinkConstPtr plink2, CollisionReportPtr report = CollisionReportPtr())=0;
144 
146  virtual bool CheckCollision(KinBody::LinkConstPtr plink, KinBodyConstPtr pbody, CollisionReportPtr report = CollisionReportPtr())=0;
147 
149  virtual bool CheckCollision(KinBody::LinkConstPtr plink, const std::vector<KinBodyConstPtr>& vbodyexcluded, const std::vector<KinBody::LinkConstPtr>& vlinkexcluded, CollisionReportPtr report = CollisionReportPtr())=0;
150 
152  virtual bool CheckCollision(KinBodyConstPtr pbody, const std::vector<KinBodyConstPtr>& vbodyexcluded, const std::vector<KinBody::LinkConstPtr>& vlinkexcluded, CollisionReportPtr report = CollisionReportPtr())=0;
153 
159  virtual bool CheckCollision(const RAY& ray, KinBody::LinkConstPtr plink, CollisionReportPtr report = CollisionReportPtr()) = 0;
160 
166  virtual bool CheckCollision(const RAY& ray, KinBodyConstPtr pbody, CollisionReportPtr report = CollisionReportPtr()) = 0;
167 
173  virtual bool CheckCollision(const RAY& ray, CollisionReportPtr report = CollisionReportPtr()) = 0;
174 
178  virtual bool CheckSelfCollision(KinBodyConstPtr pbody, CollisionReportPtr report = CollisionReportPtr()) = 0;
179 
183  virtual bool CheckSelfCollision(KinBody::LinkConstPtr plink, CollisionReportPtr report = CollisionReportPtr()) = 0;
184 
185 protected:
188  pbody->SetUserData(GetXMLId(), data);
189  }
190 
192  return boost::static_pointer_cast<CollisionCheckerBase>(shared_from_this());
193  }
195  return boost::static_pointer_cast<CollisionCheckerBase const>(shared_from_this());
196  }
197 
198 private:
199  virtual const char* GetHash() const {
201  }
202 
203 #ifdef RAVE_PRIVATE
204 #ifdef _MSC_VER
205  friend class Environment;
206 #else
207  friend class ::Environment;
208 #endif
209 #endif
210  friend class KinBody;
211 };
212 
215 {
216 public:
217  CollisionOptionsStateSaver(CollisionCheckerBasePtr p, int newoptions, bool required=true);
218  virtual ~CollisionOptionsStateSaver();
219 private:
220  int _oldoptions;
222 };
223 
224 typedef boost::shared_ptr<CollisionOptionsStateSaver> CollisionOptionsStateSaverPtr;
225 
226 } // end namespace OpenRAVE
227 
228 #endif