/opt/ros/diamondback/stacks/graspit_simulator/graspit/graspit_source/src/Collision/Graspit/collisionModel.cpp

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.cpp,v 1.8 2009/07/21 22:38:04 cmatei Exp $
//
//######################################################################

#include "collisionModel.h"

#include <algorithm>

//#define GRASPITDBG
#include "debug.h"
#ifdef MKL
#include "mkl_wrappers.h"
#else
#include "lapack_wrappers.h"
#endif

#include "collisionStructures.h"

namespace Collision {

const double Leaf::TOLERANCE = 1.0e-2;
const int Leaf::MAX_LEAF_TRIANGLES = 1;

void Jacobi(double a[3][3], double v[3][3]);
void print(const double a[3][3]);

position Leaf::getMeanVertex()
{
        position mean(0,0,0);
        if (mTriangles.empty()) return mean;

        std::list<Triangle>::iterator it;
        for (it=mTriangles.begin(); it!=mTriangles.end(); it++) {
                mean = mean + (*it).v1;
                mean = mean + (*it).v2;
                mean = mean + (*it).v3;
        }
        mean = ( 1.0 / ( 3.0 * (int)mTriangles.size() ) ) * mean;
        return mean;
}

double Leaf::getMedianProjection(const vec3 &axis)
{
        if (mTriangles.empty()) return 0.0;
        std::vector<double> projections;
        std::list<Triangle>::iterator it;
        for (it=mTriangles.begin(); it!=mTriangles.end(); it++) {
                position mean = (*it).v1 + (*it).v2 + (*it).v3;
                mean = (1.0 / 3.0) * mean;
                double projection = (mean - position::ORIGIN) % axis;
                projections.push_back(projection);
        }
        if (projections.size() == 1) {
                return projections.front();
        }
        std::nth_element(projections.begin(), projections.begin() + projections.size()/2, projections.end());
        return *(projections.begin() + projections.size()/2);
}

void boxSize(const position &p, vec3 &min, vec3 &max, 
                         const vec3 &x, const vec3 &y, const vec3 &z, double tolerance)
{
        vec3 d = p - position::ORIGIN;
        double dx = d % x;
        double dy = d % y;
        double dz = d % z;              
        if ( dx + tolerance > max.x() ) max.x() = dx + tolerance;
        if ( dy + tolerance > max.y() ) max.y() = dy + tolerance;
        if ( dz + tolerance > max.z() ) max.z() = dz + tolerance;
        if ( dx - tolerance < min.x() ) min.x() = dx - tolerance;
        if ( dy - tolerance < min.y() ) min.y() = dy - tolerance;
        if ( dz - tolerance < min.z() ) min.z() = dz - tolerance;
}

void Leaf::fitBox(const mat3 &R, vec3 &center, vec3 &halfSize)
{
        vec3 x = R.row(0);
        vec3 y = R.row(1);
        vec3 z = R.row(2);
        vec3 max(-1.0e10, -1.0e10, -1.0e10);
        vec3 min( 1.0e10,  1.0e10,  1.0e10);
        std::list<Triangle>::iterator it;
        for (it=mTriangles.begin(); it!=mTriangles.end(); it++) {
                boxSize( (*it).v1, min, max, x, y, z, TOLERANCE);
                boxSize( (*it).v2, min, max, x, y, z, TOLERANCE);
                boxSize( (*it).v3, min, max, x, y, z, TOLERANCE);
        }
        DBGP("Max: " << max);
        DBGP("Min: " << min);
        for (int i=0; i<3; i++) {
                halfSize[i] = 0.5 * (max[i] - min[i]);
        }
        DBGP("computed halfsize: " << halfSize);
        //halfSize = 0.5 * (max - min);
        center = min + halfSize;
        center = R.inverse() * center;
        //sanity check
        for (int i=0; i<3; i++) {
                if (halfSize[i] < TOLERANCE) {
                        if (halfSize[i] < 0.5 * TOLERANCE) {
                                DBGA("Warning: degenerate box computed");
                        }
                        halfSize[i] = TOLERANCE;
                }
        }
        DBGP("returned halfsize: " << halfSize);
}

void Leaf::areaWeightedCovarianceMatrix(double covMat[3][3])
{
        for (int i=0; i<3; i++) {
                for (int j=0; j<3; j++) {
                        covMat[i][j] = 0.0;
                }
        }
        double totalArea = 0;
        position areaWeightedMedian(0.0, 0.0, 0.0);
        std::list<Triangle>::iterator it;
        for (it=mTriangles.begin(); it!=mTriangles.end(); it++) {
                position m = it->centroid();
                double area = it->area();
                totalArea += area;
                areaWeightedMedian = areaWeightedMedian + area * m;
                for (int i=0; i<3; i++) {
                        for (int j=0; j<3; j++) {
                                //column major storage
                                covMat[i][j] += (area / 12.0) * (9*m[i]*m[j] + 
                                                                 it->v1[i]*it->v1[j] + 
                                                                 it->v2[i]*it->v2[j] +
                                                                 it->v3[i]*it->v3[j] );
                        }
                }
        }
        areaWeightedMedian = (1.0 / totalArea) * areaWeightedMedian;
        for (int i=0; i<3; i++) {
                for (int j=0; j<3; j++) {
                        covMat[i][j] = (1.0 / totalArea) * covMat[i][j] - 
                                                        areaWeightedMedian[i]*areaWeightedMedian[j];
                }
        }
}

void Leaf::computeBboxOO()
{
        //compute the covariance matrix
        double covMat[3][3], v[3][3];
        areaWeightedCovarianceMatrix(covMat);
        DBGP("Cov mat:"); DBGST(print(covMat));

        //perform singular value decomposition
        Jacobi(covMat, v);
        DBGP("eigenvalues:"); DBGST(print(covMat));
        DBGP("eigenVectors:"); DBGST(print(v));
        int first = 0, second = 1, third = 2;
        if (covMat[1][1] > covMat[0][0]) {
                std::swap(first, second);
        }
        if (covMat[2][2] > covMat[first][first]) {
                std::swap(first, third);
        }
        if (covMat[third][third] > covMat[second][second]) {
                std::swap(second, third);
        }
        DBGP("Eigenvalues: " << covMat[first][first] << " " << covMat[second][second] 
                 << " "  << covMat[third][third]);

        //set up rotation matrix
        vec3 xAxis(v[0][first], v[1][first], v[2][first]); 
        vec3 yAxis(v[0][second], v[1][second], v[2][second]);
        vec3 zAxis = normalise(xAxis) * normalise(yAxis);
        yAxis = zAxis * normalise(xAxis);
        xAxis = yAxis * zAxis;
        mat3 R(xAxis, yAxis, zAxis);

        DBGP("Matrix: " << R);

        //compute bbox extents
        vec3 halfSize, center;
        fitBox(R, center, halfSize);

        //rotate box so that x axis always points in the direction of largest extent
        first = 0;
        if (halfSize.y() > halfSize.x()) first = 1;
        if (halfSize.z() > halfSize[first]) first = 2;
        transf rotate = transf::IDENTITY;
        if (first == 1) {
                // y has the largest extent, rotate around z
                rotate = rotate_transf(M_PI/2.0, vec3(0,0,1));
        } else if (first == 2) {
                // z has the largest extent, rotate around y
                rotate = rotate_transf(M_PI/2.0, vec3(0,1,0));
        }
        halfSize = halfSize * rotate;
        for (int i=0; i<3; i++) {
                if (halfSize[i] < 0) halfSize[i] = -halfSize[i];
        }
        mat3 RR;
        rotate.rotation().ToRotationMatrix(RR);
        R = RR * R;

        mBbox.halfSize = halfSize;
        mBbox.setTran( transf(R, center ) );
}

void Leaf::computeBboxAA()
{
        mat3 R( vec3::X, vec3::Y, vec3::Z);
        vec3 halfSize, center;
        fitBox(R, center, halfSize);
        mBbox.halfSize = halfSize;
        mBbox.setTran( transf(R, center ) );
}

Branch* Leaf::split()
{       
        //if we have few enough triangles, we are done
        if ( (int)mTriangles.size() <= MAX_LEAF_TRIANGLES ) return NULL;

        //choose axis as dominant axis of bbox
        //the box is rotated after fitting so that the x axis always points in the direction
        //of the largest extent
        vec3 xAxis = vec3(1,0,0) * mBbox.getTran();
        vec3 yAxis = vec3(0,1,0) * mBbox.getTran();
        vec3 zAxis = vec3(0,0,1) * mBbox.getTran();
        if (mBbox.halfSize.y() > mBbox.halfSize.x()) {
                DBGA("Unexpected bounding box dominant axis. Extents: ");
                DBGA(mBbox.halfSize.x() << " " << mBbox.halfSize.y() << " " << mBbox.halfSize.z());
                //assert(0);
                std::swap(xAxis, yAxis);
                yAxis = - yAxis;
        }
        if (mBbox.halfSize.z() > mBbox.halfSize.y() && 
                mBbox.halfSize.z() > mBbox.halfSize.x() ) {
                DBGA("Unexpected bounding box dominant axis. Extents: ");
                DBGA(mBbox.halfSize.x() << " " << mBbox.halfSize.y() << " " << mBbox.halfSize.z());
                //assert(0);
                std::swap(xAxis, zAxis);
                zAxis = - zAxis;
        }


        //choose point as mean of triangle vertices
        double sepPoint = (getMeanVertex() - position::ORIGIN) % xAxis;

        //choose point as median of triangle centroids
        //double sepPoint = getMedianProjection(xAxis);

        //create children
        Leaf *child1 = new Leaf();
        Leaf *child2 = new Leaf();

        
        //split according to centroid
        balancedSplit(xAxis, sepPoint, child1, child2);

        //if unbalanced, try splitting based on median
        if (!child1->getNumTriangles() || !child2->getNumTriangles()) {
                child1->clearTriangles();
                child2->clearTriangles();
                sepPoint = getMedianProjection(xAxis);
                balancedSplit(xAxis, sepPoint, child1, child2);
        }
        

        //try optimal split
        //the dominant axis chosen earlier is always X
        //optimalSplit(xAxis, yAxis, zAxis, child1, child2);

        //if STILL unbalanced, split randomly
        if (!child1->getNumTriangles() || !child2->getNumTriangles()) {
                child1->clearTriangles();
                child2->clearTriangles();
                randomSplit(child1, child2);
        }

        //ask children to compute bboxes
        child1->computeBbox();
        child2->computeBbox();

        //create branch with new children
        Branch* b = new Branch(child1, child2, mBbox);
        return b;
}

void Leaf::balancedSplit(vec3 axis, double sepPoint, Leaf *child1, Leaf *child2)
{
        std::list<Triangle>::iterator it;
        for (it=mTriangles.begin(); it!=mTriangles.end(); it++) {
                position median(0,0,0);
                median = median + (*it).v1;
                median = median + (*it).v2;
                median = median + (*it).v3;
                median =  ( 1.0 / 3.0 ) * median;
                vec3 m = median - position::ORIGIN;
                double d = m % axis;
                if ( d < sepPoint ) {
                        child1->addTriangle(*it);
                } else {
                        child2->addTriangle(*it);
                }
        }
}

void Leaf::randomSplit(Leaf *child1, Leaf *child2)
{
        std::list<Triangle>::iterator it;
        bool one = true;
        for (it=mTriangles.begin(); it!=mTriangles.end(); it++) {
                if (one) {
                        child1->addTriangle(*it);
                } else {
                        child2->addTriangle(*it);
                }
                one = !one;
        }
}

bool 
compareProjections(const std::pair<Triangle, double> &p1, 
                                   const std::pair<Triangle, double> &p2) {
        return p1.second < p2.second;
}

void 
Leaf::optimalSplit(const vec3 &x, const vec3 &y, const vec3 &z, Leaf *child1, Leaf *child2)
{
        //sort triangles by centroid projections
        std::vector< std::pair<Triangle,double> > sortedTriangles;
        std::list<Triangle>::iterator it;
        for (it=mTriangles.begin(); it!=mTriangles.end(); it++) {
                position median = (*it).v1 + (*it).v2 + (*it).v3;
                median =  ( 1.0 / 3.0 ) * median;
                double projection = (median - position::ORIGIN) % x;
                sortedTriangles.push_back( std::pair<Triangle,double>(*it,projection) );
        }
        std::sort(sortedTriangles.begin(), sortedTriangles.end(), compareProjections);

        //compute bbox volumes going up
        std::vector<double> volumesUp;
        vec3 max(-1.0e10, -1.0e10, -1.0e10);
        vec3 min( 1.0e10,  1.0e10,  1.0e10);
        std::vector< std::pair<Triangle,double> >::iterator it2;
        for (it2 = sortedTriangles.begin(); it2!=sortedTriangles.end(); it2++) {
                boxSize((*it2).first.v1, min, max, x, y, z, Leaf::TOLERANCE);
                boxSize((*it2).first.v2, min, max, x, y, z, Leaf::TOLERANCE);
                boxSize((*it2).first.v3, min, max, x, y, z, Leaf::TOLERANCE);
                double volumeSq = (max[0] - min[0]) * (max[1] - min[1]) * (max[2] - min[2]);
                volumesUp.push_back(volumeSq);
        }
        //and bbox volumes going down
        std::vector<double> volumesDown;
        max.set(-1.0e10, -1.0e10, -1.0e10);
        min.set( 1.0e10,  1.0e10,  1.0e10);
        std::vector< std::pair<Triangle,double> >::reverse_iterator it3;
        for (it3 = sortedTriangles.rbegin(); it3!=sortedTriangles.rend(); it3++) {
                boxSize((*it3).first.v1, min, max, x, y, z, Leaf::TOLERANCE);
                boxSize((*it3).first.v2, min, max, x, y, z, Leaf::TOLERANCE);
                boxSize((*it3).first.v3, min, max, x, y, z, Leaf::TOLERANCE);
                double volumeSq = (max[0] - min[0]) * (max[1] - min[1]) * (max[2] - min[2]);
                volumesDown.push_back(volumeSq);
        }
        assert( volumesUp.size() == volumesDown.size() );

        //find optimal split point
        for (int i=0; i<(int)volumesUp.size(); i++) {
                volumesUp[i] += volumesDown[ volumesDown.size()-i-1 ];
        }
        std::vector<double>::iterator minVolIt = std::min_element(volumesUp.begin(), volumesUp.end());

        //assign triangles to children
        std::vector<double>::iterator it4;
        int index=0;
        for (it4 = volumesUp.begin(); it4 != minVolIt; it4++) {
                child1->addTriangle(sortedTriangles[index].first);
                index++;
        }
        for (it4 = minVolIt; it4 != volumesUp.end(); it4++) {
                child2->addTriangle(sortedTriangles[index].first);
                index++;
        }
}


void Node::getBVRecurse(int currentDepth, int desiredDepth, std::vector<BoundingBox> *bvs)
{
        if (currentDepth == desiredDepth || isLeaf() ) {
                bvs->push_back( BoundingBox(mBbox) );
                DBGP("BBox tran: " << mBbox.getTran());
                DBGP("BBox size: " << mBbox.halfSize);
                return;
        }
}

void Branch::getBVRecurse(int currentDepth, int desiredDepth, std::vector<BoundingBox> *bvs)
{
        Node::getBVRecurse(currentDepth, desiredDepth, bvs);
        if (currentDepth < desiredDepth) {
                mChild1->getBVRecurse(currentDepth+1, desiredDepth, bvs);
                mChild2->getBVRecurse(currentDepth+1, desiredDepth, bvs);
        }
}

void CollisionModel::cloneModel(CollisionModel *original)
{
        if (mClone || original->mClone) {
                DBGA("WARNING: cloning of clones! Not well tested!"); 
        }
        delete mRoot;
        mRoot = original->mRoot;
        mClone = true;
}

void CollisionModel::getBoundingVolumes(int depth, std::vector<BoundingBox> *bvs)
{
        mRoot->getBVRecurse(0, depth, bvs);
}

void 
CollisionModel::addTriangle(Triangle t) 
{
        if (mClone) {
                DBGA("Cannot add triangles to clones!");
                assert(0);
                return;
        } 
        if (!mRoot->isLeaf()) {
                DBGA("Reset model before adding triangles");
                return;
        }
        static_cast<Leaf*>(mRoot)->addTriangle(t);
}

void
CollisionModel::reset()
{
        if (mClone) {
                DBGA("Cannot reset a clone!");
                assert(0);
                return;
        } 
        delete mRoot; 
        mRoot = new Leaf();
}


void 
CollisionModel::build() 
{
        if (mClone) {
                DBGA("Cannot build a cloned model!");
                assert(0);
                return;
        }
        if (!mRoot->isLeaf()) {
                DBGA("Model already built. Reset first.");
                return;
        }
        DBGP("Buid bboxes from triangles: " << static_cast<Leaf*>(mRoot)->getNumTriangles());
        static_cast<Leaf*>(mRoot)->computeBbox();
        
        Branch *tmp = mRoot->split();
        if (tmp) {
                delete mRoot;
                mRoot = tmp;
                tmp->splitRecurse(0,-1);
        }
        DBGP("Split finished, hierarchy size: " << mRoot->countRecurse());              
}

//-------------------------------------

/* Some mathematical tools. I will try to find a better place for these, 
   maybe in matvec.cpp */

inline void multiply(double r[3][3], const double a[3][3], const double b[3][3]) {
        for (int i=0; i<3; i++) {
                for (int j=0; j<3; j++) {
                        r[i][j] = 0.0;
                        for (int k=0; k<3; k++) {
                                r[i][j] += a[i][k] * b[k][j];
                        }
                }
        }
}

inline void copy(double r[3][3], const double o[3][3]) {
        for (int i=0; i<3; i++) {
                for (int j=0; j<3; j++) {
                        r[i][j] = o[i][j];
                }
        }
}

inline void transpose(double r[3][3], const double o[3][3]) {
        for (int i=0; i<3; i++) {
                for (int j=0; j<3; j++) {
                        r[i][j] = o[j][i];
                }
        }
}

void print(const double a[3][3])
{
        DBGA(a[0][0] << " " << a[0][1] << " " << a[0][2]);
        DBGA(a[1][0] << " " << a[1][1] << " " << a[1][2]);
        DBGA(a[2][0] << " " << a[2][1] << " " << a[2][2]);
}

// 2-by-2 Symmetric Schur decomposition. Given an n-by-n symmetric matrix
// and indicies p, q such that 1 <= p < q <= n, computes a sine-cosine pair
// (s, c) that will serve to form a Jacobi rotation matrix.
//
// See Golub, Van Loan, Matrix Computations, 3rd ed, p428
void SymSchur2(double a[3][3], int p, int q, double &c, double &s)
{
    if (fabs(a[p][q]) > 0.0001f) {
        double r = (a[q][q] - a[p][p]) / (2.0f * a[p][q]);
        double t;
        if (r >= 0.0f)
            t = 1.0f / (r + sqrt(1.0f + r*r));
        else
            t = -1.0f / (-r + sqrt(1.0f + r*r));
        c = 1.0f / sqrt(1.0f + t*t);
        s = t * c;
    } else {
        c = 1.0f;
        s = 0.0f;
    }
}

// Computes the eigenvectors and eigenvalues of the symmetric matrix A using
// the classic Jacobi method of iteratively updating A as A = J^T * A * J,
// where J = J(p, q, theta) is the Jacobi rotation matrix.
//
// On exit, v will contain the eigenvectors, and the diagonal elements
// of a are the corresponding eigenvalues.
//
// See Golub, Van Loan, Matrix Computations, 3rd ed, p428
void Jacobi(double a[3][3], double v[3][3])
{
    int i, j, n, p, q;
    double prevoff, c, s;
    double J[3][3], tmp1[3][3], tmp2[3][3];

    // Initialize v to identity matrix
    for (i = 0; i < 3; i++) {
        v[i][0] = v[i][1] = v[i][2] = 0.0f;
        v[i][i] = 1.0f;
    }

    // Repeat for some maximum number of iterations
    const int MAX_ITERATIONS = 50;
    for (n = 0; n < MAX_ITERATIONS; n++) {
        // Find largest off-diagonal absolute element a[p][q]
        p = 0; q = 1;
        for (i = 0; i < 3; i++) {
            for (j = 0; j < 3; j++) {
                if (i == j) continue;
                if (fabs(a[i][j]) > fabs(a[p][q])) {
                    p = i;
                    q = j;
                }
            }
        }

        // Compute the Jacobi rotation matrix J(p, q, theta)
        // (This code can be optimized for the three different cases of rotation)
        SymSchur2(a, p, q, c, s);
        for (i = 0; i < 3; i++) {
            J[i][0] = J[i][1] = J[i][2] = 0.0f;
            J[i][i] = 1.0f;
        }
        J[p][p] =  c; J[p][q] = s;
        J[q][p] = -s; J[q][q] = c;

        // Cumulate rotations into what will contain the eigenvectors
        // v = v * J;
                multiply(tmp1, v, J);
                copy(v, tmp1);

        // Make 'a' more diagonal, until just eigenvalues remain on diagonal
        // a = (J.Transpose() * a) * J;
                transpose(tmp1, J);
                multiply(tmp2, tmp1, a);
                multiply(tmp1, tmp2, J);
                copy(a, tmp1);
    
        // Compute "norm" of off-diagonal elements
        double off = 0.0f;
        for (i = 0; i < 3; i++) {
            for (j = 0; j < 3; j++) {
                if (i == j) continue;
                off += a[i][j] * a[i][j];
            }
        }
        /* off = sqrt(off); not needed for norm comparison */

        // Stop when norm no longer decreasing
        if (n > 2 && off >= prevoff)
            return;
        
        prevoff = off;
    }
}


}

---

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