intersection.hpp 7.8 KB
Newer Older
Pierre Kraemer's avatar
Pierre Kraemer committed
1
2
3
/*******************************************************************************
 * CGoGN: Combinatorial and Geometric modeling with Generic N-dimensional Maps  *
 * version 0.1                                                                  *
4
 * Copyright (C) 2009-2011, IGG Team, LSIIT, University of Strasbourg           *
Pierre Kraemer's avatar
Pierre Kraemer committed
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
 *                                                                              *
 * This library 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 library 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 library; if not, write to the Free Software Foundation,      *
 * Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301 USA.           *
 *                                                                              *
20
 * Web site: http://cgogn.u-strasbg.fr/                                         *
Pierre Kraemer's avatar
Pierre Kraemer committed
21
22
23
24
25
26
 * Contact information: cgogn@unistra.fr                                        *
 *                                                                              *
 *******************************************************************************/

#include "Algo/Geometry/normal.h"
#include "Algo/Geometry/centroid.h"
untereiner's avatar
...    
untereiner committed
27
#include "intersection.h"
Pierre Kraemer's avatar
Pierre Kraemer committed
28
29
30
31
32
33
34
35
36
37
38
39
40

#include <limits>

namespace CGoGN
{

namespace Algo
{

namespace Geometry
{

template <typename PFP>
Pierre Kraemer's avatar
Pierre Kraemer committed
41
bool intersectionLineConvexFace(typename PFP::MAP& map, Dart d, const typename PFP::TVEC3& position, const typename PFP::VEC3& P, const typename PFP::VEC3& Dir, typename PFP::VEC3& Inter)
Pierre Kraemer's avatar
Pierre Kraemer committed
42
43
44
45
46
{
	typedef typename PFP::VEC3 VEC3 ;

	const float SMALL_NUM = std::numeric_limits<typename PFP::REAL>::min() * 5.0f;

Pierre Kraemer's avatar
Pierre Kraemer committed
47
48
	VEC3 p1 = position[d];
	VEC3 n = faceNormal<PFP>(map,d,position);
Pierre Kraemer's avatar
Pierre Kraemer committed
49
	VEC3 w0 = P - p1;
untereiner's avatar
...    
untereiner committed
50
51
    float a = -(n*w0);
    float b = n*Dir;
Pierre Kraemer's avatar
Pierre Kraemer committed
52
53
54
55
56
57
58
59

    if (fabs(b) < SMALL_NUM)
		return false;

	float r = a / b;
	Inter = P + r * Dir;           // intersect point of ray and plane

    // is I inside the face?
Pierre Kraemer's avatar
Pierre Kraemer committed
60
	VEC3 p2 = position[map.phi1(d)];
Pierre Kraemer's avatar
Pierre Kraemer committed
61
62
63
64
65
66
67
68
69
70
	VEC3 v = p2 - p1 ;
	VEC3 vInter = Inter - p1;
	float dirV = v * vInter;
	if(fabs(dirV) < SMALL_NUM) // on an edge
		return true;

	Dart it = map.phi1(d);
	while(it != d)
	{
		p1 = p2;
Pierre Kraemer's avatar
Pierre Kraemer committed
71
		p2 = position[map.phi1(it)];
Pierre Kraemer's avatar
Pierre Kraemer committed
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
		v = p2 - p1;
		vInter = Inter - p1;
		float dirD = v * vInter;

		if(fabs(dirD) < SMALL_NUM) // on an edge
			return true;
		if((dirV > SMALL_NUM && dirD < SMALL_NUM) || (dirV < SMALL_NUM && dirD > SMALL_NUM)) //exterior of the face
			return false;
		it = map.phi1(it) ;
	}

    return true;
}

template <typename PFP>
Pierre Kraemer's avatar
Pierre Kraemer committed
87
bool intersectionSegmentConvexFace(typename PFP::MAP& map, Dart d, const typename PFP::TVEC3& position, const typename PFP::VEC3& PA, const typename PFP::VEC3& PB, typename PFP::VEC3& Inter)
Pierre Kraemer's avatar
Pierre Kraemer committed
88
89
{
	typename PFP::VEC3 dir = PB - PA;
Pierre Kraemer's avatar
Pierre Kraemer committed
90
	if (intersectionLineConvexFace(map, d, position, PA, dir, Inter))
Pierre Kraemer's avatar
Pierre Kraemer committed
91
92
	{
		typename PFP::VEC3 dirA = PA - Inter;
Pierre Kraemer's avatar
Pierre Kraemer committed
93
		typename PFP::VEC3 dirB = PB - Inter;
Pierre Kraemer's avatar
Pierre Kraemer committed
94

Pierre Kraemer's avatar
Pierre Kraemer committed
95
		if (dirA * dirB < 0)
Pierre Kraemer's avatar
Pierre Kraemer committed
96
97
98
99
100
101
			return true;
	}
	return false;
}

template <typename PFP>
Pierre Kraemer's avatar
Pierre Kraemer committed
102
bool areTrianglesInIntersection(typename PFP::MAP& map, Dart tri1, Dart tri2, const typename PFP::TVEC3& position)
Pierre Kraemer's avatar
Pierre Kraemer committed
103
104
105
106
107
108
109
110
{
	typedef typename PFP::VEC3 VEC3 ;

	//get vertices position
	VEC3 tris1[3];
	VEC3 tris2[3];
	for (unsigned int i = 0; i < 3; ++i)
	{
Pierre Kraemer's avatar
Pierre Kraemer committed
111
112
		tris1[i] = position[tri1];
		tris2[i] = position[tri2];
Pierre Kraemer's avatar
Pierre Kraemer committed
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
160
161
162
163
164
165
166
167
168
		tri1 = map.phi1(tri1);
		tri2 = map.phi1(tri2);
	}

// 	gmtl::Vec3f nTri1,nTri2;
//	float offset1,offset2;
// 	CGoGN::Algo::Geometry::trianglePlane<PFP>(map,tri1,nTri1,offset1);
// 	CGoGN::Algo::Geometry::trianglePlane<PFP>(map,tri2,nTri2,offset2);
//
// 	Orientation3D oP[2][3];
// 	oP[0][0] = testOrientation3D(tris2[0],offset1,nTri1);
// 	oP[0][1] = testOrientation3D(tris2[1],offset1,nTri1);
// 	oP[0][2] = testOrientation3D(tris2[2],offset1,nTri1);
//
// 	if(oP[0][0]==oP[0][1] && oP[0][1]==oP[0][2]) {
// 		if(oP[0][0]==ON) { //coplanar triangles
// 			return isSegmentInTriangle2D(tris1[0],tris1[1],tris2[0],tris2[1],triS2[2],nTri2)
// 			||  isSegmentInTriangle2D(tris1[1],tris1[2],tris2[0],tris2[1],triS2[2],nTri2)
// 			|| isSegmentInTriangle2D(tris1[2],tris1[0],tris2[0],tris2[1],triS2[2],nTri2);
// 		}
// 		else
// 			return false;
// 	}
//
// 	oP[1][0] = testOrientation3D(tris1[0],offset2,nTri2);
// 	oP[1][1] = testOrientation3D(tris1[1],offset2,nTri2);
// 	oP[1][2] = testOrientation3D(tris1[2],offset2,nTri2);
//
// 	if(oP[1][0]==oP[1][1] && oP[1][1]==oP[1][2])
// 		return false;
//
// 	//search segment of tri 1 in plane of tri 2
// 	gmtl::Point3f inter1,inter2;
// 	bool found = false;
// 	for(unsigned int i=0;i<3 && !found;++i) {
// 		//test if the first point is the one opposite to the two others
// 		if(oP[0][i]!=oP[0][(1+i)%3] && oP[0][(1+i)%3]==oP[0][(2+i)%3]) {
// 			found=true;
// 			//search collision points with the two edges
// 			float offset= gmtl::dot(tris2[0],nTri2);
// 			gmtl::Planef pl(nTri2,offset);
//
// 			gmtl::Vec3f dir1(oP[0][(1+i)%3]);
// 			dir1 -= oP[0][i];
//
// 			gmtl::Vec3f dir2(oP[0][(2+i)%3]);
// 			dir2 -= oP[0][i];
//
// 			inter1 = gmtl::intersect(pl,gmtl::Ray(oP[0][i],dir1));
// 			inter2 = gmtl::intersect(pl,gmtl::Ray(oP[0][i],dir2));
// 		}
// 	}
//
// 	return isSegmentInTriangle2D(inter1,inter2,tris2[0],tris2[1],triS2[2],nTri2);

	//compute face normal
Pierre Kraemer's avatar
Pierre Kraemer committed
169
170
	VEC3 normale1 = faceNormal<PFP>(map, tri1, position);
	VEC3 bary1 = faceCentroid<PFP>(map, tri1, position);
Pierre Kraemer's avatar
Pierre Kraemer committed
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189

	int pos = 0;
	int neg = 0;
	//test position of points relative to first tri
	for (unsigned int i = 0; i < 3 ; ++i)
	{
		VEC3 nTest = bary1 - tris2[i];
		float scal = nTest * normale1;
		if (scal<0)
			++neg;
		if (scal>0)
			++pos;
	}

	//if all pos or neg then no intersection
	if (neg == 3 || pos == 3)
		return false;

	//same for the second triangle
Pierre Kraemer's avatar
Pierre Kraemer committed
190
191
	VEC3 normale2 = faceNormal<PFP>(map, tri2, position);
	VEC3 bary2 = faceCentroid<PFP>(map, tri2, position);
Pierre Kraemer's avatar
Pierre Kraemer committed
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
	pos = 0;
	neg = 0;
	for (unsigned int i = 0; i < 3 ; ++i)
	{
		VEC3 nTest = bary2 - tris1[i];
		float scal = nTest * normale2;
		if (scal<0)
			++neg;
		if (scal>0)
			++pos;
	}

	if (neg == 3 || pos == 3)
		return false;

	bool intersection = false;

	for (unsigned int i = 0; i < 3 && !intersection; ++i)
	{
		VEC3 inter;
Pierre Kraemer's avatar
Pierre Kraemer committed
212
		intersection = Geom::intersectionSegmentTriangle(tris1[i], tris1[(i+1)%3], tris2[0], tris2[1], tris2[2], inter);
Pierre Kraemer's avatar
Pierre Kraemer committed
213
214
215
216
217
218
219
220
	}

	if (intersection)
		return true;

	for (unsigned int i = 0; i < 3 && !intersection; ++i)
	{
		VEC3 inter;
Pierre Kraemer's avatar
Pierre Kraemer committed
221
		intersection = Geom::intersectionSegmentTriangle(tris2[i], tris2[(i+1)%3], tris1[0], tris1[1], tris1[2], inter);
Pierre Kraemer's avatar
Pierre Kraemer committed
222
223
224
225
226
	}

	return intersection;
}

Pierre Kraemer's avatar
Pierre Kraemer committed
227
228
229
230
231
template <typename PFP>
bool intersectionSphereEdge(typename PFP::MAP& map, typename PFP::VEC3& center, typename PFP::REAL radius, Dart d, const typename PFP::TVEC3& position, typename PFP::REAL& alpha)
{
	typedef typename PFP::VEC3 VEC3 ;
	typedef typename PFP::REAL REAL ;
Pierre Kraemer's avatar
Pierre Kraemer committed
232

Pierre Kraemer's avatar
Pierre Kraemer committed
233
234
235
236
237
238
239
240
241
242
243
244
	const typename PFP::VEC3& p1 = position[d];
	const typename PFP::VEC3& p2 = position[map.phi1(d)];
	if(Geom::isPointInSphere(p1, center, radius) && !Geom::isPointInSphere(p2, center, radius))
	{
		VEC3 p = p1 - center;
		VEC3 qminusp = p2 - center - p;
		REAL s = p * qminusp;
		REAL n2 = qminusp.norm2();
		alpha = (- s + sqrt(s*s + n2 * (radius*radius - p.norm2()))) / n2;
		return true ;
	}
	return false ;
untereiner's avatar
...    
untereiner committed
245
}
246

Pierre Kraemer's avatar
Pierre Kraemer committed
247
248
249
250
251
} // namespace Geometry

} // namespace Algo

} // namespace CGoGN