/
MeshDiscreteIntersection.inl
159 lines (132 loc) · 6.44 KB
/
MeshDiscreteIntersection.inl
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
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
/******************************************************************************
* 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 *
******************************************************************************/
#pragma once
#include <sofa/component/collision/detection/intersection/MeshDiscreteIntersection.h>
#include <sofa/component/collision/detection/intersection/DiscreteIntersection.h>
namespace sofa::component::collision::detection::intersection
{
template<class T>
bool MeshDiscreteIntersection::testIntersection(collision::geometry::TSphere<T>& sph, collision::geometry::Triangle& triangle, const core::collision::Intersection* currentIntersection)
{
SOFA_UNUSED(currentIntersection);
const double EPSILON = 0.00001;
//Vertices of the triangle:
const type::Vec3 p0 = triangle.p1();
const type::Vec3 p1 = triangle.p2();
const type::Vec3 p2 = triangle.p3();
// Center of the sphere
const type::Vec3 sphCenter(sph.center());
// Radius of the sphere
const double r = sph.r();
//Normal to the plane (plane spanned by tree points of the triangle)
type::Vec3 normal = cross( (p1 - p0), (p2 - p0) );
normal.normalize();
//Distance from the center of the sphere to the plane.
double distance = sphCenter*normal - normal*p0;
//Projection of the center of the sphere onto the plane
const type::Vec3 projPoint = sphCenter - normal*distance;
//Distance correction in case is negative.
if (distance < 0.0)
distance = -distance;
//Distance to the sphere:
distance -= r;
//If the distance is positive, the point has been proyected outside
//the sphere and hence the plane does not intersect the sphere
//and so the triangle (that spanned the plane) cannot be inside the sphere.
if (distance > EPSILON)
{
return false;
}
//However, if the plane has intersected the sphere, then it is
//necessary to check if the projected point "projPoint" is inside
//the triangle.
#define SAMESIDE(ap1,ap2,ap3,ap4) (((cross((ap4-ap3),(ap1-ap3))) * (cross((ap4-ap3),(ap2-ap3)))) >= 0)
if ( (SAMESIDE(projPoint,p0,p1,p2) && SAMESIDE(projPoint,p1,p0,p2) && SAMESIDE(projPoint,p2,p0,p1)))
{
return true;
}
#undef SAMESIDE
return false;
}
template<class T>
int MeshDiscreteIntersection::computeIntersection(collision::geometry::TSphere<T>& sph, collision::geometry::Triangle& triangle, OutputVector* contacts, const core::collision::Intersection* currentIntersection)
{
SOFA_UNUSED(currentIntersection);
const double EPSILON = 0.00001;
//Vertices of the triangle:
const type::Vec3 p0 = triangle.p1();
const type::Vec3 p1 = triangle.p2();
const type::Vec3 p2 = triangle.p3();
// Center of the sphere
const type::Vec3 sphCenter(sph.center());
// Radius of the sphere
const double r = sph.r();
//Normal to the plane (plane spanned by tree points of the triangle)
type::Vec3 normal = cross( (p1 - p0), (p2 - p0) );
normal.normalize();
//Distance from the center of the sphere to the plane.
double distance = sphCenter*normal - normal*p0;
//Projection of the center of the sphere onto the plane
const type::Vec3 projPoint = sphCenter - normal*distance;
//Distance correction in case is negative.
if (distance < 0.0)
distance = -distance;
//Distance to the sphere:
distance -= r;
//If the distance is positive, the point has been proyected outside
//the sphere and hence the plane does not intersect the sphere
//and so the triangle (that spanned the plane) cannot be inside the sphere.
if (distance > EPSILON)
{
return 0;
}
//However, if the plane has intersected the sphere, then it is
//neccesary to check if the proyected point "projPoint" is inside
//the triangle.
#define SAMESIDE(ap1,ap2,ap3,ap4) (((cross((ap4-ap3),(ap1-ap3))) * (cross((ap4-ap3),(ap2-ap3)))) >= 0)
if ( (SAMESIDE(projPoint,p0,p1,p2) && SAMESIDE(projPoint,p1,p0,p2) && SAMESIDE(projPoint,p2,p0,p1)))
{
contacts->resize(contacts->size()+1);
core::collision::DetectionOutput *detection = &*(contacts->end()-1);
detection->normal = -normal;
detection->point[1] = projPoint;
detection->point[0] = sph.getContactPointByNormal( detection->normal );
detection->value = -distance;
detection->elem.first = sph;
detection->elem.second = triangle;
detection->id = sph.getIndex();
return 1;
}
#undef SAMESIDE
return 0; // No intersection: passed all tests for intersections !
}
template<class T>
bool MeshDiscreteIntersection::testIntersection(collision::geometry::TSphere<T>& sph, collision::geometry::Triangle& triangle)
{
return testIntersection(sph, triangle, {});
}
template<class T>
int MeshDiscreteIntersection::computeIntersection(collision::geometry::TSphere<T>& sph, collision::geometry::Triangle& triangle, OutputVector* contacts)
{
return computeIntersection(sph, triangle, contacts, {});
}
} // namespace sofa::component::collision::detection::intersection