Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00017 #ifndef OPENHRP_COLISIONDETECTOR_IMPL_H_INCLUDED
00018 #define OPENHRP_COLISIONDETECTOR_IMPL_H_INCLUDED
00019
00020 #include <map>
00021 #include <vector>
00022 #include <hrpCorba/ORBwrap.h>
00023 #include <hrpCorba/CollisionDetector.hh>
00024 #include <hrpCorba/ModelLoader.hh>
00025 #include <hrpCollision/ColdetModelPair.h>
00026 #include "ColdetBody.h"
00027
00028 using namespace std;
00029 using namespace OpenHRP;
00030
00031
00032 class CollisionDetector_impl : virtual public POA_OpenHRP::CollisionDetector,
00033 virtual public PortableServer::RefCountServantBase
00034 {
00035 public:
00036
00037 CollisionDetector_impl(CORBA_ORB_ptr orb);
00038
00039 ~CollisionDetector_impl();
00040
00041 virtual void destroy();
00042
00043 virtual void registerCharacter(const char* name, BodyInfo_ptr bodyInfo);
00044
00045 virtual void addCollisionPair(const LinkPair& colPair);
00046
00047
00048 virtual CORBA::Boolean queryIntersectionForDefinedPairs(
00049 CORBA::Boolean checkAll,
00050 const CharacterPositionSequence& characterPositions,
00051 LinkPairSequence_out collidedPairs
00052 );
00053
00054
00055 virtual CORBA::Boolean queryIntersectionForGivenPairs(
00056 CORBA::Boolean checkAll,
00057 const LinkPairSequence& checkPairs,
00058 const CharacterPositionSequence& characterPositions,
00059 LinkPairSequence_out collidedPairs
00060 );
00061
00062 virtual CORBA::Boolean queryContactDeterminationForDefinedPairs(
00063 const CharacterPositionSequence& characterPositions,
00064 CollisionSequence_out collisions
00065 );
00066
00067 virtual CORBA::Boolean queryContactDeterminationForGivenPairs(
00068 const LinkPairSequence& checkPairs,
00069 const CharacterPositionSequence& characterPositions,
00070 CollisionSequence_out collisions
00071 );
00072
00073 virtual void queryDistanceForDefinedPairs(
00074 const CharacterPositionSequence& characterPositions,
00075 DistanceSequence_out distances
00076 );
00077
00078
00079 virtual void queryDistanceForGivenPairs(
00080 const LinkPairSequence& checkPairs,
00081 const CharacterPositionSequence& characterPositions,
00082 DistanceSequence_out distances
00083 );
00084
00085 virtual CORBA::Double queryDistanceWithRay(
00086 const DblArray3 point,
00087 const DblArray3 dir
00088 );
00089
00090 virtual DblSequence* scanDistanceWithRay(const DblArray3 p, const DblArray9 R, CORBA::Double step, CORBA::Double range);
00091
00092 private:
00093
00094 CORBA_ORB_var orb;
00095
00096 typedef map<string, ColdetBodyPtr> StringToColdetBodyMap;
00097
00098
00099 StringToColdetBodyMap bodyInfoToColdetBodyMap;
00100
00101 StringToColdetBodyMap nameToColdetBodyMap;
00102
00103 class ColdetModelPairEx : public ColdetModelPair
00104 {
00105 public:
00106 ColdetModelPairEx(
00107 ColdetBodyPtr& body0, ColdetModelPtr& link0, ColdetBodyPtr& body1, ColdetModelPtr& link1, double tolerance=0)
00108 : ColdetModelPair(link0, link1, tolerance),
00109 body0(body0),
00110 body1(body1)
00111 { }
00112 ColdetBodyPtr body0;
00113 ColdetBodyPtr body1;
00114 double tolerance;
00115 };
00116 typedef intrusive_ptr<ColdetModelPairEx> ColdetModelPairExPtr;
00117
00118 vector<ColdetModelPairExPtr> coldetModelPairs;
00119
00120 void addCollisionPairSub(const LinkPair& linkPair, vector<ColdetModelPairExPtr>& io_coldetPairs);
00121 void updateAllLinkPositions(const CharacterPositionSequence& characterPositions);
00122 bool detectAllCollisions(vector<ColdetModelPairExPtr>& coldetPairs, CollisionSequence_out& out_collisions);
00123 bool detectCollisionsOfLinkPair(
00124 ColdetModelPairEx& coldetPair, CollisionPointSequence& out_collisionPoints, const bool addCollisionPoints);
00125 bool detectIntersectionOfLinkPair(ColdetModelPairExPtr& coldetPair);
00126 bool detectCollidedLinkPairs(
00127 vector<ColdetModelPairExPtr>& coldetPairs, LinkPairSequence_out& out_collidedPairs, const bool checkAll);
00128 bool detectIntersectingLinkPairs(
00129 vector<ColdetModelPairExPtr>& coldetPairs, LinkPairSequence_out& out_collidedPairs, const bool checkAll);
00130 void computeDistances(
00131 vector<ColdetModelPairExPtr>& coldetPairs, DistanceSequence_out& out_distances);
00132 };
00133
00134
00135 class CollisionDetectorFactory_impl
00136 : virtual public POA_OpenHRP::CollisionDetectorFactory,
00137 virtual public PortableServer::RefCountServantBase
00138 {
00139 public:
00140
00141 CollisionDetectorFactory_impl(CORBA_ORB_ptr orb);
00142
00143 ~CollisionDetectorFactory_impl();
00144
00145 CollisionDetector_ptr create();
00146
00147 void shutdown();
00148
00149 private:
00150 CORBA_ORB_var orb;
00151 };
00152
00153 #endif