CollisionAlgorithm/src/sofa/collisionAlgorithm/toolbox/TetrahedronToolBox.cpp at 990551dd914285df82a05037b32a9331a10b5755 · InfinyTech3D/CollisionAlgorithm · GitHub

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
#include <sofa/collisionAlgorithm/toolbox/TetrahedronToolBox.h>
#include <sofa/collisionAlgorithm/elements/TetrahedronElement.h>
#include <sofa/collisionAlgorithm/proximity/TetrahedronProximity.h>
#include <sofa/collisionAlgorithm/operations/Project.h>
#include <sofa/collisionAlgorithm/operations/CreateCenterProximity.h>

namespace sofa::collisionAlgorithm::toolbox {

Operations::CreateCenterProximity::Result TetrahedronToolBox::createCenterProximity(const TetrahedronElement::SPtr & tetra) {
    return TetrahedronProximity::create(tetra, 1.0/4.0,1.0/4.0,1.0/4.0,1.0/4.0);
}

Operations::ContainsPointInElement::Result TetrahedronToolBox::containsPoint(const type::Vec3 & P, const TetrahedronElement::SPtr & tetra) {
    TetrahedronProximity::SPtr prox = TetrahedronProximity::create(tetra, 1.0/4.0,1.0/4.0,1.0/4.0,1.0/4.0);
    double f0(prox->f0()), f1(prox->f1()), f2(prox->f2()), f3(prox->f3());
    return isInTetra(P,tetra->getTetrahedronInfo(),f0,f1,f2,f3);
}

Operations::Project::Result TetrahedronToolBox::project(const type::Vec3 & P, const TetrahedronElement::SPtr & tetra) {
    double fact[4];
    projectOnTetra(P, tetra->getTetrahedronInfo(),fact[0],fact[1],fact[2],fact[3]);

    BaseProximity::SPtr prox = TetrahedronProximity::create(tetra, fact[0],fact[1],fact[2],fact[3]);
	BaseProximity::SPtr prox_normalized = prox->copy();
    prox_normalized->normalize();

    double dist = (prox_normalized->getPosition() - P).norm();

    return Operations::Project::Result(dist,prox);
}

void TetrahedronToolBox::projectOnTetra(const type::Vec3d projectP, const TetrahedronElement::TetraInfo & teinfo, double & fact_u, double & fact_v, double & fact_w,double & fact_x) {
    computeTetraBaryCoords(projectP, teinfo, fact_u,fact_v,fact_w,fact_x);
}

void TetrahedronToolBox::computeTetraBaryCoords(const type::Vec3d & P, const TetrahedronElement::TetraInfo & tinfo, double & fact_u,double & fact_v, double & fact_w, double & fact_x) {
    type::Vec3d e = P - tinfo.P0;

    double Va = 1.0/6.0 * dot(e,tinfo.ax2Cax3);
    double Vb = 1.0/6.0 * dot(tinfo.ax1,e.cross(tinfo.ax3));
    double Vc = 1.0/6.0 * dot(tinfo.ax1,tinfo.ax2.cross(e));

    fact_v = Va/tinfo.V0;
    fact_w = Vb/tinfo.V0;
    fact_x = Vc/tinfo.V0;
    fact_u = 1.0 - (fact_v + fact_w + fact_x);
}


bool TetrahedronToolBox::isInTetra(const type::Vec3d & P, const TetrahedronElement::TetraInfo & tinfo, double & fact_u,double & fact_v, double & fact_w, double & fact_x) {
    computeTetraBaryCoords(P, tinfo, fact_u, fact_v, fact_w, fact_x);
    if ((fact_u<0 || fact_u>1) || (fact_v<0 || fact_v>1) || (fact_w<0 || fact_w>1) || (fact_x<0 || fact_x>1)) return false;
    return true;
}


void TetrahedronToolBox::normalize(const type::Vec3d & P0, const type::Vec3d & P1, const type::Vec3d & P2, const type::Vec3d & P3, double & f0,double & f1, double & f2,double & f3) {
    if(f0<0) {
        type::Vec3 P = (P0*f0)+(P1*f1)+(P2*f2)+(P3*f3);

        TriangleElement::TriangleInfo tinfo;
        tinfo.update(P1,P2,P3);
        toolbox::TriangleToolBox::projectOnTriangle(P, tinfo, f1,f2,f3);

        f0 = 0;
        toolbox::TriangleToolBox::normalize(P1,P2,P3,
                                            f1,f2,f3);
    } else if(f1<0) {
        type::Vec3 P = (P0*f0)+(P1*f1)+(P2*f2)+(P3*f3);

        TriangleElement::TriangleInfo tinfo;
        tinfo.update(P0,P2,P3);

        toolbox::TriangleToolBox::projectOnTriangle(P, tinfo,f0,f2,f3);

        f1 = 0;
        toolbox::TriangleToolBox::normalize(P0,P2,P3,
                                            f0,f2,f3);
    } else if(f2<0) {
        type::Vec3 P = (P0*f0)+(P1*f1)+(P2*f2)+(P3*f3);

        TriangleElement::TriangleInfo tinfo;
        tinfo.update(P0,P1,P3);

        toolbox::TriangleToolBox::projectOnTriangle(P, tinfo,f0,f1,f3);

        f2 = 0;
        toolbox::TriangleToolBox::normalize(P0,P1,P3,
                                            f0,f1,f3);
    } else if(f3<0) {
        type::Vec3 P = (P0*f0)+(P1*f1)+(P2*f2)+(P3*f3);

        TriangleElement::TriangleInfo tinfo;
        tinfo.update(P0,P1,P2);

        toolbox::TriangleToolBox::projectOnTriangle(P, tinfo,f0,f1,f2);

        f3 = 0;
        toolbox::TriangleToolBox::normalize(P0,P1,P2,
                                            f0,f1,f2);
    }
}

}