CollisionDetectionAlgorithm.h

Go to the documentation of this file.

#ifndef SimTK_SIMBODY_COLLISION_DETECTION_ALGORITHM_H_
#define SimTK_SIMBODY_COLLISION_DETECTION_ALGORITHM_H_

/* -------------------------------------------------------------------------- *
 *                      SimTK Core: SimTK Simbody(tm)                         *
 * -------------------------------------------------------------------------- *
 * This is part of the SimTK Core biosimulation toolkit originating from      *
 * Simbios, the NIH National Center for Physics-Based Simulation of           *
 * Biological Structures at Stanford, funded under the NIH Roadmap for        *
 * Medical Research, grant U54 GM072970. See https://simtk.org.               *
 *                                                                            *
 * Portions copyright (c) 2008 Stanford University and the Authors.           *
 * Authors: Peter Eastman                                                     *
 * Contributors:                                                              *
 *                                                                            *
 * Permission is hereby granted, free of charge, to any person obtaining a    *
 * copy of this software and associated documentation files (the "Software"), *
 * to deal in the Software without restriction, including without limitation  *
 * the rights to use, copy, modify, merge, publish, distribute, sublicense,   *
 * and/or sell copies of the Software, and to permit persons to whom the      *
 * Software is furnished to do so, subject to the following conditions:       *
 *                                                                            *
 * The above copyright notice and this permission notice shall be included in *
 * all copies or substantial portions of the Software.                        *
 *                                                                            *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,   *
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL    *
 * THE AUTHORS, CONTRIBUTORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,    *
 * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR      *
 * OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE  *
 * USE OR OTHER DEALINGS IN THE SOFTWARE.                                     *
 * -------------------------------------------------------------------------- */

#include "SimTKcommon.h"
#include "simbody/internal/common.h"
#include "simbody/internal/ContactGeometry.h"
#include "simbody/internal/Contact.h"
#include <map>

namespace SimTK {

class SimTK_SIMBODY_EXPORT CollisionDetectionAlgorithm {
public:
    class HalfSpaceSphere;
    class SphereSphere;
    class HalfSpaceEllipsoid;
    class HalfSpaceTriangleMesh;
    class SphereTriangleMesh;
    class TriangleMeshTriangleMesh;
    virtual ~CollisionDetectionAlgorithm() {}
    virtual void processObjects
       (ContactSurfaceIndex index1, const ContactGeometry& object1, 
        const Transform& transform1,
        ContactSurfaceIndex index2, const ContactGeometry& object2, 
        const Transform& transform2, 
        Array_<Contact>& contacts) const = 0;
    static void registerAlgorithm(const std::string& type1, const std::string& type2, CollisionDetectionAlgorithm* algorithm);
    static CollisionDetectionAlgorithm* getAlgorithm(int typeIndex1, int typeIndex2);
private:
    struct AlgorithmMap : public  std::map<std::pair<int, int>, CollisionDetectionAlgorithm*> {
        ~AlgorithmMap();
    };

    // static std::map<std::pair<int, int>, CollisionDetectionAlgorithm*> algorithmMap;
    static AlgorithmMap algorithmMap;
};

class SimTK_SIMBODY_EXPORT CollisionDetectionAlgorithm::HalfSpaceSphere : public CollisionDetectionAlgorithm {
public:
    virtual ~HalfSpaceSphere() {}
    void processObjects
       (ContactSurfaceIndex index1, const ContactGeometry& object1, 
        const Transform& transform1,
        ContactSurfaceIndex index2, const ContactGeometry& object2, 
        const Transform& transform2, 
        Array_<Contact>& contacts) const;
};

class SimTK_SIMBODY_EXPORT CollisionDetectionAlgorithm::HalfSpaceEllipsoid : public CollisionDetectionAlgorithm {
public:
    virtual ~HalfSpaceEllipsoid() {}
    void processObjects
       (ContactSurfaceIndex index1, const ContactGeometry& object1,
        const Transform& transform1,
        ContactSurfaceIndex index2, const ContactGeometry& object2,
        const Transform& transform2,
        Array_<Contact>& contacts) const;
};

class SimTK_SIMBODY_EXPORT CollisionDetectionAlgorithm::SphereSphere : public CollisionDetectionAlgorithm {
public:
    virtual ~SphereSphere() {}
    void processObjects
       (ContactSurfaceIndex index1, const ContactGeometry& object1, 
        const Transform& transform1,
        ContactSurfaceIndex index2, const ContactGeometry& object2, 
        const Transform& transform2, 
        Array_<Contact>& contacts) const;
};

class SimTK_SIMBODY_EXPORT CollisionDetectionAlgorithm::HalfSpaceTriangleMesh : public CollisionDetectionAlgorithm {
public:
    virtual ~HalfSpaceTriangleMesh() {}
    void processObjects
       (ContactSurfaceIndex index1, const ContactGeometry& object1, 
        const Transform& transform1,
        ContactSurfaceIndex index2, const ContactGeometry& object2, 
        const Transform& transform2, 
        Array_<Contact>& contacts) const;
private:
    void processBox(const ContactGeometry::TriangleMesh& mesh, const ContactGeometry::TriangleMesh::OBBTreeNode& node,
            const Transform& transform, const Vec3& axisDir, Real xoffset, std::set<int>& insideFaces) const;
    void addAllTriangles(const ContactGeometry::TriangleMesh::OBBTreeNode& node, std::set<int>& insideFaces) const;
};

class SimTK_SIMBODY_EXPORT CollisionDetectionAlgorithm::SphereTriangleMesh : public CollisionDetectionAlgorithm {
public:
    virtual ~SphereTriangleMesh() {}
    void processObjects
       (ContactSurfaceIndex index1, const ContactGeometry& object1, 
        const Transform& transform1,
        ContactSurfaceIndex index2, const ContactGeometry& object2, 
        const Transform& transform2, 
        Array_<Contact>& contacts) const;
private:
    void processBox(const Vec3& center, Real radius2, const ContactGeometry::TriangleMesh& mesh, const ContactGeometry::TriangleMesh::OBBTreeNode& node,
            std::set<int>& insideFaces) const;
};

class SimTK_SIMBODY_EXPORT CollisionDetectionAlgorithm::TriangleMeshTriangleMesh : public CollisionDetectionAlgorithm {
public:
    virtual ~TriangleMeshTriangleMesh() {}
    void processObjects
       (ContactSurfaceIndex index1, const ContactGeometry& object1, 
        const Transform& transform1,
        ContactSurfaceIndex index2, const ContactGeometry& object2, 
        const Transform& transform2, 
        Array_<Contact>& contacts) const;
private:
    void processNodes(const ContactGeometry::TriangleMesh& mesh1, const ContactGeometry::TriangleMesh& mesh2,
            const ContactGeometry::TriangleMesh::OBBTreeNode& node1, const ContactGeometry::TriangleMesh::OBBTreeNode& node2,
            const OrientedBoundingBox& node2Bounds, const Transform& transform, std::set<int>& triangles1, std::set<int>& triangles2) const;
    void findInsideTriangles(const ContactGeometry::TriangleMesh& mesh, const ContactGeometry::TriangleMesh& otherMesh,
        const Transform& transform, std::set<int>& triangles) const;
    void tagFaces(const ContactGeometry::TriangleMesh& mesh, Array_<int>& faceType, std::set<int>& triangles, int index, int depth) const;
    static const int OUTSIDE = -1;
    static const int UNKNOWN = 0;
    static const int BOUNDARY = 1;
    static const int INSIDE = 2;
};

} // namespace SimTK

#endif // SimTK_SIMBODY_COLLISION_DETECTION_ALGORITHM_H_