imstkPBDCollisionHandling.cpp 4.72 KB
Newer Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
/*=========================================================================

   Library: iMSTK

   Copyright (c) Kitware, Inc. & Center for Modeling, Simulation,
   & Imaging in Medicine, Rensselaer Polytechnic Institute.

   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.txt

   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.

=========================================================================*/

#include "imstkPBDCollisionHandling.h"
#include "imstkCollisionData.h"
#include "imstkDeformableObject.h"
25
#include "imstkPbdModel.h"
26
27
28
29
#include "imstkPbdObject.h"
#include "imstkPbdEdgeEdgeCollisionConstraint.h"
#include "imstkPbdPointTriCollisionConstraint.h"

30
#include "imstkPbdSolver.h"
31
32
33
34
35
36
37
38
39
40
41
#include <memory>
#include <g3log/g3log.hpp>

namespace imstk
{
void
PBDCollisionHandling::processCollisionData()
{
    /*if (auto deformableObj = std::dynamic_pointer_cast<DeformableObject>(m_object))
    {
        this->computeContactForcesDiscreteDeformable(deformableObj);
Sreekanth Arikatla's avatar
Sreekanth Arikatla committed
42
    }
43
44
45
46
47
48
49
50
51
52
    else if (auto analyticObj = std::dynamic_pointer_cast<CollidingObject>(m_object))
    {
        this->computeContactForcesAnalyticRigid(analyticObj);
    }
    else
    {
        LOG(WARNING) << "PenaltyRigidCH::computeContactForces error: "
                     << "no penalty collision handling available for " << m_object->getName()
                     << " (rigid mesh not yet supported).";
    }*/
53
    this->generatePBDConstraints();
54
    if (m_PBDSolver)
Sreekanth Arikatla's avatar
Sreekanth Arikatla committed
55
56
57
58
59
60
61
    {
        m_PBDSolver->addCollisionConstraints(&m_PBDConstraints);
    }
    else
    {
        LOG(WARNING) << "Error: PBDCollisionHandling: no PbdSolver found to handle the Collision constraints...";
    }
62
63
}

Sreekanth Arikatla's avatar
Sreekanth Arikatla committed
64
void
65
66
PBDCollisionHandling::generatePBDConstraints()
{
67
68
69
    // clear the constraints before populating wit new ones
    m_PBDConstraints.clear();

70
71
72
73
74
75
76
77
    const auto dynaModel1 = std::static_pointer_cast<PbdModel>(m_pbdObject1->getDynamicalModel());
    const auto dynaModel2 = std::static_pointer_cast<PbdModel>(m_pbdObject2->getDynamicalModel());

    const auto colGeo2 = std::static_pointer_cast<SurfaceMesh>(m_pbdObject2->getCollidingGeometry());

    const auto map1 = m_pbdObject1->getPhysicsToCollidingMap();
    const auto map2 = m_pbdObject2->getPhysicsToCollidingMap();

78
//    std::cout << "EE: " << m_colData->EEColData.getSize() << "TV: " << m_colData->VTColData.getSize() << std::endl;
79

80
    // Generate edge-edge pbd constraints
81
    for (size_t i = 0; i < m_colData->EEColData.getSize(); ++i)
82
    {
83
84
        const auto& colData = m_colData->EEColData[i];
        auto        c       = std::make_shared<PbdEdgeEdgeConstraint>();
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109

        size_t edgeA1, edgeA2;
        if (map1)
        {
            edgeA1 = map1->getMapIdx(colData.edgeIdA.first);
            edgeA2 = map1->getMapIdx(colData.edgeIdA.second);
        }
        else
        {
            edgeA1 = colData.edgeIdA.first;
            edgeA2 = colData.edgeIdA.second;
        }

        size_t edgeB1, edgeB2;
        if (map2)
        {
            edgeB1 = map2->getMapIdx(colData.edgeIdB.first);
            edgeB2 = map2->getMapIdx(colData.edgeIdB.second);
        }
        else
        {
            edgeB1 = colData.edgeIdB.first;
            edgeB2 = colData.edgeIdB.second;
        }

110
111
        c->initConstraint(dynaModel1, edgeA1, edgeA2,
                          dynaModel2, edgeB1, edgeB2);
Sreekanth Arikatla's avatar
Sreekanth Arikatla committed
112

113
114
115
116
        m_PBDConstraints.push_back(c);
    }

    // Generate triangle-vertex pbd constraints
117
    for (size_t i = 0; i < m_colData->TVColData.getSize(); ++i)
118
    {
119
120
        const auto& colData  = m_colData->TVColData[i];
        const auto& triVerts = colGeo2->getTrianglesVertices()[colData.triIdx];
121
122

        const auto c = std::make_shared<PbdPointTriangleConstraint>();
Sreekanth Arikatla's avatar
Sreekanth Arikatla committed
123

124
125
126
127
128
129
130
131
132
133
134
135
136
        size_t v1, v2, v3;
        if (map2)
        {
            v1 = map2->getMapIdx(triVerts[0]);
            v2 = map2->getMapIdx(triVerts[1]);
            v3 = map2->getMapIdx(triVerts[2]);
        }
        else
        {
            v1 = triVerts[0];
            v2 = triVerts[1];
            v3 = triVerts[2];
        }
137

Sreekanth Arikatla's avatar
Sreekanth Arikatla committed
138
        c->initConstraint(dynaModel1,
139
                          colData.vertexIdx,
Sreekanth Arikatla's avatar
Sreekanth Arikatla committed
140
                          dynaModel2,
141
142
143
                          v1,
                          v2,
                          v3);
144
145
146

        m_PBDConstraints.push_back(c);
    }
Sreekanth Arikatla's avatar
Sreekanth Arikatla committed
147
    //TODO: generating PbdPointTriangleConstraint from the VTColData should be added
148
149
}
}