-
Notifications
You must be signed in to change notification settings - Fork 298
/
NarrowPhaseDetection.cpp
125 lines (107 loc) · 4.45 KB
/
NarrowPhaseDetection.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
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
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
/******************************************************************************
* SOFA, Simulation Open-Framework Architecture *
* (c) 2006 INRIA, USTL, UJF, CNRS, MGH *
* *
* This program is free software; you can redistribute it and/or modify it *
* under the terms of the GNU Lesser General Public License as published by *
* the Free Software Foundation; either version 2.1 of the License, or (at *
* your option) any later version. *
* *
* This program 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 Lesser General Public License *
* for more details. *
* *
* You should have received a copy of the GNU Lesser General Public License *
* along with this program. If not, see <http://www.gnu.org/licenses/>. *
*******************************************************************************
* Authors: The SOFA Team and external contributors (see Authors.txt) *
* *
* Contact information: contact@sofa-framework.org *
******************************************************************************/
#include <sofa/core/collision/NarrowPhaseDetection.h>
#include <sofa/core/visual/VisualParams.h>
namespace sofa::core::collision
{
NarrowPhaseDetection::~NarrowPhaseDetection()
{
for (const auto& it : m_outputsMap)
{
DetectionOutputVector* do_vec = it.second;
if (do_vec != nullptr)
{
do_vec->clear();
do_vec->release();
}
}
}
void NarrowPhaseDetection::beginNarrowPhase()
{
for (DetectionOutputMap::iterator it = m_outputsMap.begin(); it != m_outputsMap.end(); ++it)
{
DetectionOutputVector *do_vec = (it->second);
if (do_vec != nullptr)
do_vec->clear();
}
}
void NarrowPhaseDetection::draw(const core::visual::VisualParams* vparams)
{
if(! vparams->displayFlags().getShowDetectionOutputs()) return;
std::vector<type::Vec3> points;
for (auto mapIt = m_outputsMap.begin(); mapIt!=m_outputsMap.end() ; ++mapIt)
{
for (unsigned idx = 0; idx != (*mapIt).second->size(); ++idx)
{
points.push_back((*mapIt).second->getFirstPosition(idx));
points.push_back((*mapIt).second->getSecondPosition(idx));
}
}
vparams->drawTool()->drawLines(points,5, type::g_red);
vparams->drawTool()->drawPoints(points, 10, type::g_blue);
}
void NarrowPhaseDetection::addCollisionPairs(const sofa::type::vector< std::pair<core::CollisionModel*, core::CollisionModel*> >& v)
{
for (sofa::type::vector< std::pair<core::CollisionModel*, core::CollisionModel*> >::const_iterator it = v.begin(); it!=v.end(); ++it)
addCollisionPair(*it);
// m_outputsMap should just be filled in addCollisionPair function
m_primitiveTestCount = m_outputsMap.size();
}
void NarrowPhaseDetection::endNarrowPhase()
{
for (auto it = m_outputsMap.begin(); it != m_outputsMap.end();)
{
DetectionOutputVector *do_vec = (it->second);
if (!do_vec || do_vec->empty())
{
if (do_vec)
{
do_vec->release();
}
m_outputsMap.erase(it++);
}
else
{
++it;
}
}
}
size_t NarrowPhaseDetection::getPrimitiveTestCount() const
{
return m_primitiveTestCount;
}
auto NarrowPhaseDetection::getDetectionOutputs() const -> const DetectionOutputMap&
{
return m_outputsMap;
}
DetectionOutputVector*& NarrowPhaseDetection::getDetectionOutputs(CollisionModel *cm1, CollisionModel *cm2)
{
std::pair< CollisionModel*, CollisionModel* > cm_pair = std::make_pair(cm1, cm2);
const auto res = m_outputsMap.insert(m_outputsMap.end(), {cm_pair, nullptr});
return res->second;
}
void NarrowPhaseDetection::changeInstanceNP(Instance inst)
{
m_storedOutputsMap[instance].swap(m_outputsMap);
m_outputsMap.swap(m_storedOutputsMap[inst]);
}
} // namespace sofa::core::collision