TriangleTriangleContactCalculation-inl.h

Go to the documentation of this file.

// This file is a part of the OpenSurgSim project.
// Copyright 2013, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
 
#ifndef SURGSIM_MATH_TRIANGLETRIANGLECONTACTCALCULATION_INL_H
#define SURGSIM_MATH_TRIANGLETRIANGLECONTACTCALCULATION_INL_H
 
namespace SurgSim
{
 
namespace Math
{
 
template <class T, int MOpt>
class TriangleHelper
{
    static const size_t CAPACITY = 10;
    typedef Eigen::Matrix<T, 3, 1, MOpt> Vector3;
    typedef boost::container::static_vector<Vector3, CAPACITY> Vertices;
 
public:
    TriangleHelper(const Vector3& v0, const Vector3& v1, const Vector3& v2, const Vector3& n)
        : m_normal(n), m_receiverBufferIndex(0)
    {
        m_vertices[0] = &v0;
        m_vertices[1] = &v1;
        m_vertices[2] = &v2;
        m_planeD = -m_vertices[0]->dot(m_normal);
    }
 
    void findDeepestPenetrationWithTriangle(const TriangleHelper& triangle, T* penetrationDepth,
                                            Vector3* penetrationPoint0, Vector3* penetrationPoint1)
    {
        m_clippedVerticesBuffer[0].push_back(*m_vertices[0]);
        m_clippedVerticesBuffer[0].push_back(*m_vertices[1]);
        m_clippedVerticesBuffer[0].push_back(*m_vertices[2]);
        m_receiverBufferIndex = 1;
 
        Vector3 clipPlaneNormal;
        T clipPlaneD;
 
        for (size_t i = 0; i < 3; ++i)
        {
            triangle.getPrismPlane(i, &clipPlaneNormal, &clipPlaneD);
            clipAgainstPlane(clipPlaneNormal, clipPlaneD);
        }
 
        findDeepestVertexUnderPlane(triangle.m_normal, triangle.m_planeD, penetrationDepth, penetrationPoint0);
 
        SURGSIM_ASSERT(*penetrationDepth <= T(0))
                << "The distance from triangle is calculated as " << *penetrationDepth << ". At this point in the"
                << " algorithm, the depth is expected to be negative.";
 
        *penetrationPoint1 = *penetrationPoint0 - (triangle.m_normal * (*penetrationDepth));
        *penetrationDepth = -(*penetrationDepth);
    }
 
private:
    void getPrismPlane(size_t index, Vector3* planeNormal, T* planeD) const
    {
        *planeNormal = *m_vertices[(index + 1) % 3] - *m_vertices[index];
        *planeNormal = planeNormal->cross(m_normal);
        planeNormal->normalize();
        *planeD = -m_vertices[index]->dot(*planeNormal);
    }
 
    void clipAgainstPlane(const Vector3& planeN, T planeD)
    {
        // Loop through the edges starting from (m_vertices[0]->m_vertices[1]) to
        // (m_vertices[m_numVertices - 1]->m_vertices[0]).
        // The start vertex and end vertex can be either under/on/over the clipping plane.
        //      start   |   end     |   action
        // ----------------------------------------------
        //      under   |   under   |   add start to clipped vertices
        //      under   |   on      |   add start to clipped vertices
        //      under   |   over    |   add start to clipped vertices, clip the edge to the plane (creating a vertex)
        //      on      |   under   |   add start to clipped vertices
        //      on      |   on      |   add start to clipped vertices
        //      on      |   over    |   add start to clipped vertices
        //      over    |   under   |   clip the edge to the plane (creating a vertex)