Skip to content

Commit f0af91b

Browse files
th-skamth-skam
committed
Merge branch 'master' into test - this includes the modifications in operation classes
2 parents 37c2a59 + aacf5bd commit f0af91b

File tree

9 files changed

+194
-34
lines changed

9 files changed

+194
-34
lines changed

‎src/sofa/collisionAlgorithm/algorithm/InsertionAlgorithm.h‎

Lines changed: 12 additions & 34 deletions
Original file line numberDiff line numberDiff line change
@@ -6,7 +6,8 @@
66
#include <sofa/collisionAlgorithm/operations/CreateCenterProximity.h>
77
#include <sofa/collisionAlgorithm/operations/FindClosestProximity.h>
88
#include <sofa/collisionAlgorithm/operations/Project.h>
9-
#include <sofa/colliisonAlgorithm/operations/ContainsPoint.h>
9+
#include <sofa/collisionAlgorithm/operations/ContainsPoint.h>
10+
#include <sofa/collisionAlgorithm/operations/NeedleOperations.h>
1011
#include <sofa/collisionAlgorithm/proximity/EdgeProximity.h>
1112
#include <sofa/collisionAlgorithm/proximity/TetrahedronProximity.h>
1213
#include <sofa/component/constraint/lagrangian/solver/ConstraintSolverImpl.h>
@@ -225,6 +226,7 @@ class InsertionAlgorithm : public BaseAlgorithm
225226
// Only add a new coupling point if the needle tip has advanced far enough
226227
if (tipToLastCP.norm() > tipDistThreshold)
227228
{
229+
// Prepare the operations before entering the loop
228230
auto createShaftProximity =
229231
Operations::CreateCenterProximity::Operation::get(l_shaftGeom->getTypeInfo());
230232
auto projectOnShaft = Operations::Project::Operation::get(l_shaftGeom);
@@ -268,15 +270,18 @@ class InsertionAlgorithm : public BaseAlgorithm
268270
// Skip if candidate CP is outside current edge segment
269271
if (projPtOnEdge < 0_sreal || projPtOnEdge > edgeSegmentLength) break;
270272

271-
// Project candidate CP onto shaft geometry and then find nearest volume
272-
// proximity
273+
// Project candidate CP onto shaft geometry ...
273274
shaftProx = projectOnShaft(candidateCP, itShaft->element()).prox;
274275
if (!shaftProx) continue;
275276

277+
// ... then find nearest volume proximity
276278
const BaseProximity::SPtr volProx = findClosestProxOnVol(
277279
shaftProx, l_volGeom.get(), projectOnVol, getFilterFunc());
278280
if (!volProx) continue;
279281

282+
// Proximity can be detected before the tip enters the tetra (e.g. near a
283+
// boundary face) Only accept proximities if the tip is inside the tetra
284+
// during insertion
280285
if (containsPointInVol(shaftProx->getPosition(), volProx))
281286
{
282287
volProx->normalize();
@@ -288,38 +293,11 @@ class InsertionAlgorithm : public BaseAlgorithm
288293
}
289294
else // Don't bother with removing the point that was just added
290295
{
291-
// 2.2. Check whether coupling point should be removed
296+
// Remove coupling points that are ahead of the tip in the insertion direction
292297
ElementIterator::SPtr itShaft = l_shaftGeom->begin(l_shaftGeom->getSize() - 2);
293-
auto createShaftProximity =
294-
Operations::CreateCenterProximity::Operation::get(itShaft->getTypeInfo());
295-
const BaseProximity::SPtr shaftProx = createShaftProximity(itShaft->element());
296-
if (shaftProx)
297-
{
298-
const EdgeProximity::SPtr edgeProx =
299-
dynamic_pointer_cast<EdgeProximity>(shaftProx);
300-
if (edgeProx)
301-
{
302-
const type::Vec3 normal = (edgeProx->element()->getP1()->getPosition() -
303-
edgeProx->element()->getP0()->getPosition())
304-
.normalized();
305-
// If the (last) coupling point lies ahead of the tip (positive dot
306-
// product), the needle is retreating. Thus, that point is removed.
307-
if (dot(tipToLastCP, normal) > 0_sreal)
308-
{
309-
m_couplingPts.pop_back();
310-
}
311-
}
312-
else
313-
{
314-
msg_warning() << "shaftGeom: " << l_shaftGeom->getName()
315-
<< " is not an EdgeGeometry. Point removal is disabled";
316-
}
317-
}
318-
else
319-
{
320-
msg_warning() << "Cannot create proximity from shaftGeom: "
321-
<< l_shaftGeom->getName() << " - point removal is disabled";
322-
}
298+
auto prunePointsAheadOfTip =
299+
Operations::Needle::PrunePointsAheadOfTip::get(itShaft->getTypeInfo());
300+
prunePointsAheadOfTip(m_couplingPts, itShaft->element());
323301
}
324302
}
325303

‎src/sofa/collisionAlgorithm/operations/ContainsPoint.cpp‎

Lines changed: 25 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,25 @@
1+
#include <sofa/collisionAlgorithm/operations/ContainsPoint.h>
2+
#include <sofa/collisionAlgorithm/proximity/TetrahedronProximity.h>
3+
#include <sofa/collisionAlgorithm/proximity/TriangleProximity.h>
4+
5+
namespace sofa::collisionAlgorithm::Operations::ContainsPointInElement
6+
{
7+
8+
int register_ContainsPoint_Triangle =
9+
Operation::register_func<TriangleElement>(&toolbox::TriangleToolBox::containsPoint);
10+
11+
int register_ContainsPoint_Tetrahedron =
12+
Operation::register_func<TetrahedronElement>(&toolbox::TetrahedronToolBox::containsPoint);
13+
14+
} // namespace sofa::collisionAlgorithm::Operations::ContainsPointInElement
15+
16+
namespace sofa::collisionAlgorithm::Operations::ContainsPointInProximity
17+
{
18+
19+
int register_ContainsPointInProximity_Triangle =
20+
Operation::register_func<TriangleElement>(&containsPoint<TriangleProximity>);
21+
22+
int register_ContainsPointInProximity_Tetrahedron =
23+
Operation::register_func<TetrahedronElement>(&containsPoint<TetrahedronProximity>);
24+
25+
} // namespace sofa::collisionAlgorithm::Operations::ContainsPointInProximity

‎src/sofa/collisionAlgorithm/operations/ContainsPoint.h‎

Lines changed: 74 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,74 @@
1+
#pragma once
2+
3+
#include <sofa/collisionAlgorithm/BaseElement.h>
4+
#include <sofa/collisionAlgorithm/BaseOperation.h>
5+
6+
namespace sofa::collisionAlgorithm::Operations::ContainsPointInElement
7+
{
8+
9+
typedef bool Result;
10+
11+
class Operation : public GenericOperation<Operation, // Type of the operation
12+
Result, // Default return type
13+
const type::Vec3&, const BaseElement::SPtr& // Parameters
14+
>
15+
{
16+
public:
17+
Result defaultFunc(const type::Vec3&, const BaseElement::SPtr&) const override { return false; }
18+
19+
void notFound(const std::type_info& id) const override
20+
{
21+
msg_error("ContainsPointInElement")
22+
<< "The operation ContainsPointInElementOperation is not registered with for type = "
23+
<< sofa::helper::NameDecoder::decodeFullName(id);
24+
}
25+
};
26+
27+
typedef Operation::FUNC FUNC;
28+
29+
} // namespace sofa::collisionAlgorithm::Operations::ContainsPointInElement
30+
31+
namespace sofa::collisionAlgorithm::Operations::ContainsPointInProximity
32+
{
33+
34+
typedef bool Result;
35+
36+
class Operation
37+
: public GenericOperation<Operation, // Type of the operation
38+
Result, // Default return type
39+
const type::Vec3&, const BaseProximity::SPtr& // Parameters
40+
>
41+
{
42+
public:
43+
Result defaultFunc(const type::Vec3&, const BaseProximity::SPtr&) const override
44+
{
45+
return false;
46+
}
47+
48+
void notFound(const std::type_info& id) const override
49+
{
50+
msg_error("ContainsPointInProximity")
51+
<< "The operation ContainsPointInProximityOperation is not registered with for type = "
52+
<< sofa::helper::NameDecoder::decodeFullName(id);
53+
}
54+
};
55+
56+
template <typename PROX>
57+
Result containsPoint(const type::Vec3& P, const typename std::shared_ptr<PROX>& prox)
58+
{
59+
if (!prox)
60+
{
61+
msg_warning("ContainsPointInProximity") << "Null proximity pointer in containsPoint"
62+
<< "Operation is disabled; returning false";
63+
return false;
64+
}
65+
66+
auto elem = prox->element();
67+
auto containsPointInElem =
68+
sofa::collisionAlgorithm::Operations::ContainsPointInElement::Operation::get(elem);
69+
return containsPointInElem(P, elem);
70+
}
71+
72+
typedef Operation::FUNC FUNC;
73+
74+
} // namespace sofa::collisionAlgorithm::Operations::ContainsPointInProximity

‎src/sofa/collisionAlgorithm/operations/NeedleOperations.cpp‎

Lines changed: 31 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,31 @@
1+
#include <sofa/collisionAlgorithm/operations/NeedleOperations.h>
2+
3+
namespace sofa::collisionAlgorithm::Operations::Needle
4+
{
5+
6+
bool prunePointsUsingEdges(std::vector<BaseProximity::SPtr>& couplingPts,
7+
const EdgeElement::SPtr& edge)
8+
{
9+
if (!edge)
10+
{
11+
msg_warning("Needle::PrunePointsAheadOfTip")
12+
<< "Null element pointer in prunePointsUsingEdges; returning false";
13+
return false;
14+
}
15+
const type::Vec3 edgeBase(edge->getP0()->getPosition());
16+
const type::Vec3 tip(edge->getP1()->getPosition());
17+
18+
const type::Vec3 edgeDirection = tip - edgeBase;
19+
20+
if (couplingPts.empty()) return true;
21+
const type::Vec3 tip2Pt = couplingPts.back()->getPosition() - tip;
22+
23+
// Positive dot product means the point is ahead of the tip
24+
if (dot(tip2Pt, edgeDirection) > 0_sreal) couplingPts.pop_back();
25+
26+
return true;
27+
}
28+
29+
int register_PrunePointsAheadOfTip_Edge =
30+
PrunePointsAheadOfTip::register_func<EdgeElement>(&prunePointsUsingEdges);
31+
} // namespace sofa::collisionAlgorithm::Operations::Needle

‎src/sofa/collisionAlgorithm/operations/NeedleOperations.h‎

Lines changed: 34 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,34 @@
1+
#pragma once
2+
3+
#include <sofa/collisionAlgorithm/BaseOperation.h>
4+
#include <sofa/collisionAlgorithm/BaseProximity.h>
5+
#include <sofa/collisionAlgorithm/elements/EdgeElement.h>
6+
7+
namespace sofa::collisionAlgorithm::Operations::Needle
8+
{
9+
10+
class PrunePointsAheadOfTip
11+
: public GenericOperation<PrunePointsAheadOfTip, // Type of the operation
12+
bool, // Default return type
13+
std::vector<BaseProximity::SPtr>&,
14+
const BaseElement::SPtr& // Parameters
15+
>
16+
{
17+
public:
18+
bool defaultFunc(std::vector<BaseProximity::SPtr>&, const BaseElement::SPtr&) const override
19+
{
20+
return false;
21+
}
22+
23+
void notFound(const std::type_info& id) const override
24+
{
25+
msg_error("Needle::PrunePointsAheadOfTip")
26+
<< "The operation PrunePointsAheadOfTipOperation is not registered with for type = "
27+
<< sofa::helper::NameDecoder::decodeFullName(id);
28+
}
29+
};
30+
31+
bool prunePointsUsingEdges(std::vector<BaseProximity::SPtr>& couplingPts,
32+
const EdgeElement::SPtr& edgeProx);
33+
34+
} // namespace sofa::collisionAlgorithm::Operations::Needle

‎src/sofa/collisionAlgorithm/toolbox/TetrahedronToolBox.cpp‎

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -10,6 +10,12 @@ Operations::CreateCenterProximity::Result TetrahedronToolBox::createCenterProxim
1010
return TetrahedronProximity::create(tetra, 1.0/4.0,1.0/4.0,1.0/4.0,1.0/4.0);
1111
}
1212

13+
Operations::ContainsPointInElement::Result TetrahedronToolBox::containsPoint(const type::Vec3 & P, const TetrahedronElement::SPtr & tetra) {
14+
TetrahedronProximity::SPtr prox = TetrahedronProximity::create(tetra, 1.0/4.0,1.0/4.0,1.0/4.0,1.0/4.0);
15+
double f0(prox->f0()), f1(prox->f1()), f2(prox->f2()), f3(prox->f3());
16+
return isInTetra(P,tetra->getTetrahedronInfo(),f0,f1,f2,f3);
17+
}
18+
1319
Operations::Project::Result TetrahedronToolBox::project(const type::Vec3 & P, const TetrahedronElement::SPtr & tetra) {
1420
double fact[4];
1521
projectOnTetra(P, tetra->getTetrahedronInfo(),fact[0],fact[1],fact[2],fact[3]);

‎src/sofa/collisionAlgorithm/toolbox/TetrahedronToolBox.h‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -4,6 +4,7 @@
44
#include <sofa/collisionAlgorithm/elements/TetrahedronElement.h>
55
#include <sofa/collisionAlgorithm/operations/Project.h>
66
#include <sofa/collisionAlgorithm/operations/CreateCenterProximity.h>
7+
#include <sofa/collisionAlgorithm/operations/ContainsPoint.h>
78

89
namespace sofa::collisionAlgorithm::toolbox {
910

@@ -12,6 +13,8 @@ class TetrahedronToolBox {
1213

1314
static Operations::CreateCenterProximity::Result createCenterProximity(const TetrahedronElement::SPtr & tetra);
1415

16+
static Operations::ContainsPointInElement::Result containsPoint(const type::Vec3 & P, const TetrahedronElement::SPtr & tetra);
17+
1518
static Operations::Project::Result project(const type::Vec3 & P, const TetrahedronElement::SPtr & tetra);
1619

1720
static void projectOnTetra(const type::Vec3d projectP, const TetrahedronElement::TetraInfo & teinfo, double & fact_u, double & fact_v, double & fact_w,double & fact_x);

‎src/sofa/collisionAlgorithm/toolbox/TriangleToolBox.cpp‎

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -8,6 +8,12 @@ Operations::CreateCenterProximity::Result TriangleToolBox::createCenterProximity
88
return TriangleProximity::create(tri, 1.0/3.0,1.0/3.0,1.0/3.0);
99
}
1010

11+
Operations::ContainsPointInElement::Result TriangleToolBox::containsPoint(const type::Vec3 & P, const TriangleElement::SPtr & tri) {
12+
TriangleProximity::SPtr prox = TriangleProximity::create(tri, 1.0 / 3.0, 1.0 / 3.0, 1.0 / 3.0);
13+
double f0(prox->f0()), f1(prox->f1()), f2(prox->f2());
14+
return isInTriangle(P,tri->getTriangleInfo(),f0,f1,f1);
15+
}
16+
1117
//Barycentric coordinates are computed according to
1218
//http://gamedev.stackexchange.com/questions/23743/whats-the-most-efficient-way-to-find-barycentric-coordinates
1319
Operations::Project::Result TriangleToolBox::project(const type::Vec3 & P, const TriangleElement::SPtr & tri) {

‎src/sofa/collisionAlgorithm/toolbox/TriangleToolBox.h‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -4,6 +4,7 @@
44
#include <sofa/collisionAlgorithm/elements/TriangleElement.h>
55
#include <sofa/collisionAlgorithm/operations/Project.h>
66
#include <sofa/collisionAlgorithm/operations/CreateCenterProximity.h>
7+
#include <sofa/collisionAlgorithm/operations/ContainsPoint.h>
78

89
namespace sofa::collisionAlgorithm::toolbox {
910

@@ -12,6 +13,8 @@ class TriangleToolBox {
1213

1314
static Operations::CreateCenterProximity::Result createCenterProximity(const TriangleElement::SPtr & tri);
1415

16+
static Operations::ContainsPointInElement::Result containsPoint(const type::Vec3 & P, const TriangleElement::SPtr & tri);
17+
1518
static Operations::Project::Result project(const type::Vec3 & P, const TriangleElement::SPtr & tri);
1619

1720
static void computeTriangleBaryCoords(const type::Vec3d & proj_P, const TriangleElement::TriangleInfo & tinfo, double & fact_u, double & fact_v, double & fact_w);

0 commit comments

Comments
 (0)