/opt/ros/diamondback/stacks/graspit_simulator/graspit/graspit_source/src/Collision/Graspit/collisionModel.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: collisionModel.h,v 1.5 2009/04/21 14:53:08 cmatei Exp $
//
//######################################################################

#ifndef _collisionmodel_h_
#define _collisionmodel_h_

#include <list>
#include <vector>

#include "matvec3D.h"
#include "bBox.h"
#include "triangle.h"

class BoundingBox;

namespace Collision{

class Branch;
class CollisionModel;

class Node {
protected:
        BoundingBox mBbox;
public:
        Node() {}
        virtual ~Node(){}
        virtual bool isLeaf() const = 0;
        virtual Branch* split(){return NULL;}
        virtual void getBVRecurse(int currentDepth, int desiredDepth, std::vector<BoundingBox> *bvs);
        virtual int countRecurse(){return 1;}
        const BoundingBox& getBox() const {return mBbox;}
        double getBoxVolume() const {return mBbox.halfSize.x() * 
                                                                 mBbox.halfSize.y() * mBbox.halfSize.z();}
        //for debugging
        virtual void markRecurse(bool m) const {mBbox.mMark = m;}
        virtual void mark(bool m) const {mBbox.mMark = m;}
};


class Leaf : public Node {
private:
        static const double TOLERANCE;
        static const int MAX_LEAF_TRIANGLES;

        std::list<Triangle> mTriangles;
        void computeBboxAA();
        void computeBboxOO();
        position getMeanVertex();
        double getMedianProjection(const vec3 &axis);
        void fitBox(const mat3 &R, vec3 &center, vec3 &halfSize);
        void balancedSplit(vec3 axis, double sepPoint, Leaf *child1, Leaf *child2);
        void randomSplit(Leaf *child1, Leaf *child2);
        void optimalSplit(const vec3 &x, const vec3 &y, const vec3 &z, Leaf *child1, Leaf *child2);
        void areaWeightedCovarianceMatrix(double covMat[3][3]);
public:
        Leaf() : Node() {}
        bool isLeaf() const {return true;}
        Branch* split();
        void addTriangle(Triangle t){mTriangles.push_back(t);}
        void computeBbox(){computeBboxOO();}
        int getNumTriangles() const {return mTriangles.size();}
        void clearTriangles() {mTriangles.clear();}
        const std::list<Triangle>* getTriangles() const {return &mTriangles;}
};


class Branch : public Node {
private:
        Node *mChild1, *mChild2;
        Branch(Leaf *c1, Leaf *c2, const BoundingBox &box) : Node(), mChild1(c1), mChild2(c2) {
                mBbox.setTran( box.getTran() );
                mBbox.halfSize = box.halfSize;
        }
        friend Branch* Leaf::split();
public:
        ~Branch() {if(mChild1) delete mChild1; if (mChild2) delete mChild2;}
        bool isLeaf() const {return false;}
        void splitRecurse(int currentDepth, int desiredDepth) {
                if (desiredDepth >= 0 && currentDepth >= desiredDepth) return;
                Branch *tmp = mChild1->split();
                if (tmp) {
                        delete mChild1;
                        mChild1 = tmp;
                        tmp->splitRecurse(currentDepth+1, desiredDepth);
                }
                tmp = mChild2->split();
                if (tmp) {
                        delete mChild2;
                        mChild2 = tmp;
                        tmp->splitRecurse(currentDepth+1, desiredDepth);
                }
        }
        const Node* child1() const {return mChild1;}
        const Node* child2() const {return mChild2;}
        virtual void getBVRecurse(int currentDepth, int desiredDepth, std::vector<BoundingBox> *bvs);
        virtual int countRecurse(){return 1 + mChild1->countRecurse() + mChild2->countRecurse();}
        virtual void markRecurse(bool m) const {
                Node::markRecurse(m);
                //only negative marks get percolated down
                if (!m) {
                        mChild1->markRecurse(m);
                        mChild2->markRecurse(m);
                }
        }
};

class CollisionModel {
private:
        Node *mRoot;
        bool mActive;
        transf mTran;
        bool mClone;

        int mThreadId;
public:
        CollisionModel(int threadId) {
                mRoot = new Leaf();
                mActive = true;
                mClone = false;
                mThreadId = threadId;
        }
        ~CollisionModel(){if(!mClone) delete mRoot;}
        void cloneModel(CollisionModel *original);
        void addTriangle(Triangle t);
        const Node* getRoot() const {return mRoot;}
        void reset();
        void build();
        void getBoundingVolumes(int depth, std::vector<BoundingBox> *bvs);
        void setTran(const transf &t){mTran = t;}
        const transf& getTran() const {return mTran;}
        bool isActive() const {return mActive;}
        void setActive(bool act){mActive = act;}
        int getThreadId() const {return mThreadId;}
};

}

#endif

---

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