CollisionDetector_impl.h
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2008, AIST, the University of Tokyo and General Robotix Inc.
00003  * All rights reserved. This program is made available under the terms of the
00004  * Eclipse Public License v1.0 which accompanies this distribution, and is
00005  * available at http://www.eclipse.org/legal/epl-v10.html
00006  * Contributors:
00007  * National Institute of Advanced Industrial Science and Technology (AIST)
00008  * General Robotix Inc. 
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     // Existing ColdetBodies are used for sharing their ColdetModels
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


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Sun Apr 2 2017 03:43:53