/opt/ros/diamondback/stacks/graspit_simulator/graspit/graspit_source/src/Collision/Graspit/collisionAlgorithms.h

Go to the documentation of this file.

//######################################################################
//
// GraspIt!
// Copyright (C) 2002-2009  Columbia University in the City of New York.
// All rights reserved.
//
// GraspIt! is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// GraspIt! is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with GraspIt!.  If not, see <http://www.gnu.org/licenses/>.
//
// Author(s): Matei T. Ciocarlie
//
// $Id: collisionAlgorithms.h,v 1.6 2009/04/21 14:53:08 cmatei Exp $
//
//######################################################################

#ifndef _collisionalgorithms_h_
#define _collisionalgorithms_h_

#include <vector>

#include "mytools.h"
#include "collisionStructures.h"

namespace Collision {

class Node;
class Leaf;
class Branch;
class CollisionModel;

class RecursionCallback
{
protected:
        const CollisionModel *mModel1;
        const CollisionModel *mModel2;
        transf mTran2To1;
        transf mTran1To2;

        int mNumLeafTests, mNumTriangleTests, mNumQuickTests;
public:
        RecursionCallback(const CollisionModel *m1, const CollisionModel *m2);
        virtual void reset() {
                mNumLeafTests = 0;
                mNumTriangleTests = 0;
                mNumQuickTests = 0;
        }
        virtual ~RecursionCallback(){}
        virtual void leafTest(const Leaf *l1, const Leaf *l2) = 0;
        virtual double quickTest(const Node *n1, const Node *n2) = 0;
        virtual bool distanceTest(double d) = 0;
        virtual void printStatistics();
};

void startRecursion(const CollisionModel *model1, const CollisionModel *model2, 
                                        RecursionCallback *rc);

class CollisionCallback : public RecursionCallback
{
private:
        bool mCollision;
public:
        CollisionCallback(const CollisionModel *m1, const CollisionModel *m2) : 
                                          RecursionCallback(m1,m2){reset();}
        void reset(){
                RecursionCallback::reset();
                mCollision = false;
        }
        bool isCollision() const {return mCollision;}
        virtual void leafTest(const Leaf *l1, const Leaf *l2);
        virtual double quickTest(const Node *n1, const Node *n2);
        virtual bool distanceTest(double d) {
                if (mCollision) return false;
                if (d<0) return true;
                return false;
        }
        void printStatistics();
};

class ContactCallback : public RecursionCallback
{
private:
        double mThreshold;
        ContactReport mReport;

        void insertContactNoDuplicates(const position &p1, const position &p2, 
                                                                   const vec3 &n1, const vec3 &n2, 
                                                                   double distSq, double thresh);
public:
        ContactCallback(double threshold, const CollisionModel *m1, const CollisionModel *m2) : 
                        RecursionCallback(m1,m2), mThreshold(threshold) {reset();}

        virtual void reset() {
                RecursionCallback::reset();
                mReport.clear();
        }
        virtual void leafTest(const Leaf *l1, const Leaf *l2);
        virtual double quickTest(const Node *n1, const Node *n2);
        virtual bool distanceTest(double dSq) {
                if (dSq <= mThreshold * mThreshold) return true;
                return false;
        }
        const ContactReport& getReport(){return mReport;}
        void printStatistics();
};

class DistanceCallback : public RecursionCallback
{
private:
        double mMinDistSq;
        position mP1, mP2;
public:
        DistanceCallback(const CollisionModel *m1, const CollisionModel *m2) : 
                                         RecursionCallback(m1,m2) {reset();}
        virtual void reset() {
                RecursionCallback::reset();
                mMinDistSq = 1.0e10;
                mP1 = position(0,0,0); mP2 = position(0,0,0);
        }
        virtual void leafTest(const Leaf *l1, const Leaf *l2);
        virtual double quickTest(const Node *n1, const Node *n2);
        virtual bool distanceTest(double dSq) {
                if (mMinDistSq < 0.0) return false;
                if (dSq<0.0 || dSq <= mMinDistSq) return true;
                return false;
        }
        double getMin() const {if (mMinDistSq<0.0) return -1.0; return sqrt(mMinDistSq);}
        void getClosestPoints(position &p1, position &p2) const {p1 = mP1; p2 = mP2;}
        void printStatistics();
};

class ClosestPtCallback : public RecursionCallback
{
private:
        double mMinDistSq;
        position mRefPosition, mClosestPt;
public:
        ClosestPtCallback(const CollisionModel *m1, const position &p) : 
                                         RecursionCallback(m1, NULL), mRefPosition(p) {reset();}
        virtual void reset() {
                RecursionCallback::reset();
                mMinDistSq = 1.0e10;
        }
        virtual void leafTest(const Leaf *l1, const Leaf *l2);
        virtual double quickTest(const Node *n1, const Node *n2);
        virtual bool distanceTest(double dSq) {
                if (mMinDistSq < 0.0) return false;
                if (dSq<0.0 || dSq <= mMinDistSq) return true;
                return false;
        }
        double getMin() {if (mMinDistSq<0.0) return -1.0; return sqrt(mMinDistSq);}
        position getClosestPt() {return mClosestPt;}
        void printStatistics();
};

class RegionCallback : public RecursionCallback
{
private:
        Neighborhood mNeighborhood;
        position mRefPosition;
        vec3 mNormal;
        double mRadiusSq;

        void insertPoint(const position &point);
public:
        RegionCallback(const CollisionModel *m1, const position &p, const vec3 &n, double r) : 
                                   RecursionCallback(m1, NULL), mRefPosition(p), 
                                   mNormal(n), mRadiusSq(r * r) {reset();}
        virtual void reset() {
                RecursionCallback::reset();
                mNeighborhood.clear();
        }
        virtual void leafTest(const Leaf *l1, const Leaf *l2);
        virtual double quickTest(const Node *n1, const Node *n2);
        virtual bool distanceTest(double dSq) {
                if (dSq<0.0 || dSq <= mRadiusSq) return true;
                return false;
        }
        Neighborhood& getRegion() {return mNeighborhood;}
        void printStatistics();
};

} //namespace Collision

#endif

---

graspit
Author(s):
autogenerated on Wed Jan 25 10:58:52 2012