CollisionDetector_impl.h
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2008, AIST, the University of Tokyo and General Robotix Inc.
3  * All rights reserved. This program is made available under the terms of the
4  * Eclipse Public License v1.0 which accompanies this distribution, and is
5  * available at http://www.eclipse.org/legal/epl-v10.html
6  * Contributors:
7  * National Institute of Advanced Industrial Science and Technology (AIST)
8  * General Robotix Inc.
9  */
10 
17 #ifndef OPENHRP_COLISIONDETECTOR_IMPL_H_INCLUDED
18 #define OPENHRP_COLISIONDETECTOR_IMPL_H_INCLUDED
19 
20 #include <map>
21 #include <vector>
22 #include <hrpCorba/ORBwrap.h>
23 #include <hrpCorba/CollisionDetector.hh>
24 #include <hrpCorba/ModelLoader.hh>
26 #include "ColdetBody.h"
27 
28 using namespace std;
29 using namespace OpenHRP;
30 
31 
32 class CollisionDetector_impl : virtual public POA_OpenHRP::CollisionDetector,
33  virtual public PortableServer::RefCountServantBase
34 {
35 public:
36 
38 
40 
41  virtual void destroy();
42 
43  virtual void registerCharacter(const char* name, BodyInfo_ptr bodyInfo);
44 
45  virtual void addCollisionPair(const LinkPair& colPair);
46 
47 
48  virtual CORBA::Boolean queryIntersectionForDefinedPairs(
49  CORBA::Boolean checkAll,
50  const CharacterPositionSequence& characterPositions,
51  LinkPairSequence_out collidedPairs
52  );
53 
54 
55  virtual CORBA::Boolean queryIntersectionForGivenPairs(
56  CORBA::Boolean checkAll,
57  const LinkPairSequence& checkPairs,
58  const CharacterPositionSequence& characterPositions,
59  LinkPairSequence_out collidedPairs
60  );
61 
62  virtual CORBA::Boolean queryContactDeterminationForDefinedPairs(
63  const CharacterPositionSequence& characterPositions,
64  CollisionSequence_out collisions
65  );
66 
67  virtual CORBA::Boolean queryContactDeterminationForGivenPairs(
68  const LinkPairSequence& checkPairs,
69  const CharacterPositionSequence& characterPositions,
70  CollisionSequence_out collisions
71  );
72 
73  virtual void queryDistanceForDefinedPairs(
74  const CharacterPositionSequence& characterPositions,
75  DistanceSequence_out distances
76  );
77 
78 
79  virtual void queryDistanceForGivenPairs(
80  const LinkPairSequence& checkPairs,
81  const CharacterPositionSequence& characterPositions,
82  DistanceSequence_out distances
83  );
84 
85  virtual CORBA::Double queryDistanceWithRay(
86  const DblArray3 point,
87  const DblArray3 dir
88  );
89 
90  virtual DblSequence* scanDistanceWithRay(const DblArray3 p, const DblArray9 R, CORBA::Double step, CORBA::Double range);
91 
92 private:
93 
95 
96  typedef map<string, ColdetBodyPtr> StringToColdetBodyMap;
97 
98  // Existing ColdetBodies are used for sharing their ColdetModels
99  StringToColdetBodyMap bodyInfoToColdetBodyMap;
100 
101  StringToColdetBodyMap nameToColdetBodyMap;
102 
104  {
105  public:
107  ColdetBodyPtr& body0, ColdetModelPtr& link0, ColdetBodyPtr& body1, ColdetModelPtr& link1, double tolerance=0)
108  : ColdetModelPair(link0, link1, tolerance),
109  body0(body0),
110  body1(body1)
111  { }
114  double tolerance;
115  };
116  typedef intrusive_ptr<ColdetModelPairEx> ColdetModelPairExPtr;
117 
118  vector<ColdetModelPairExPtr> coldetModelPairs;
119 
120  void addCollisionPairSub(const LinkPair& linkPair, vector<ColdetModelPairExPtr>& io_coldetPairs);
121  void updateAllLinkPositions(const CharacterPositionSequence& characterPositions);
122  bool detectAllCollisions(vector<ColdetModelPairExPtr>& coldetPairs, CollisionSequence_out& out_collisions);
123  bool detectCollisionsOfLinkPair(
124  ColdetModelPairEx& coldetPair, CollisionPointSequence& out_collisionPoints, const bool addCollisionPoints);
125  bool detectIntersectionOfLinkPair(ColdetModelPairExPtr& coldetPair);
126  bool detectCollidedLinkPairs(
127  vector<ColdetModelPairExPtr>& coldetPairs, LinkPairSequence_out& out_collidedPairs, const bool checkAll);
128  bool detectIntersectingLinkPairs(
129  vector<ColdetModelPairExPtr>& coldetPairs, LinkPairSequence_out& out_collidedPairs, const bool checkAll);
130  void computeDistances(
131  vector<ColdetModelPairExPtr>& coldetPairs, DistanceSequence_out& out_distances);
132 };
133 
134 
136  : virtual public POA_OpenHRP::CollisionDetectorFactory,
137  virtual public PortableServer::RefCountServantBase
138 {
139 public:
140 
142 
144 
145  CollisionDetector_ptr create();
146 
147  void shutdown();
148 
149 private:
151 };
152 
153 #endif
map< string, ColdetBodyPtr > StringToColdetBodyMap
#define CORBA_ORB_ptr
Definition: ORBwrap.h:16
boost::intrusive_ptr< ColdetBody > ColdetBodyPtr
Definition: ColdetBody.h:72
png_infop png_charpp name
Definition: png.h:2382
StringToColdetBodyMap bodyInfoToColdetBodyMap
StringToColdetBodyMap nameToColdetBodyMap
#define CORBA_ORB_var
Definition: ORBwrap.h:17
vector< ColdetModelPairExPtr > coldetModelPairs
local int destroy(gz_stream *s)
Definition: gzio.c:355
ColdetModelPairEx(ColdetBodyPtr &body0, ColdetModelPtr &link0, ColdetBodyPtr &body1, ColdetModelPtr &link1, double tolerance=0)
static BodyCustomizerHandle create(BodyHandle bodyHandle, const char *modelName)
boost::intrusive_ptr< ColdetModel > ColdetModelPtr
Definition: ColdetModel.h:268
intrusive_ptr< ColdetModelPairEx > ColdetModelPairExPtr


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Sat Apr 13 2019 02:14:20