intersection.hpp 16.9 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-2012, 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.unistra.fr/                                           *
Pierre Kraemer's avatar
Pierre Kraemer committed
21 22 23 24
 * Contact information: cgogn@unistra.fr                                        *
 *                                                                              *
 *******************************************************************************/

25 26
#include "Geometry/distances.h"

Pierre Kraemer's avatar
Pierre Kraemer committed
27 28 29 30 31 32
namespace CGoGN
{

namespace Geom
{

33
template <typename VEC3>
Pierre Kraemer's avatar
Pierre Kraemer committed
34
Intersection intersectionLinePlane(const VEC3& P, const VEC3& Dir, const VEC3& PlaneP, const VEC3& NormP, VEC3& Inter)
35
{
Sylvain Thery's avatar
Sylvain Thery committed
36
	double b = NormP * Dir ;
37

38
#define PRECISION 1e-6
Pierre Kraemer's avatar
Pierre Kraemer committed
39
	if (fabs(b) < PRECISION)		//ray parallel to triangle
40
	{
41
		VEC3 v = PlaneP - P;
Sylvain Thery's avatar
Sylvain Thery committed
42
		double c = NormP * v;
Pierre Kraemer's avatar
Pierre Kraemer committed
43
		if (fabs(c) < PRECISION )
44 45 46
			return EDGE_INTERSECTION;

		return NO_INTERSECTION;
47
	}
Pierre Kraemer's avatar
Pierre Kraemer committed
48
#undef PRECISION
49

Sylvain Thery's avatar
Sylvain Thery committed
50
	double a = NormP * (PlaneP - P);
51 52

	Inter = P + (a / b) * Dir;
53

54 55 56 57 58 59 60
	return FACE_INTERSECTION;
}

template <typename VEC3, typename PLANE>
Intersection intersectionLinePlane(const VEC3& P, const VEC3& Dir, const PLANE& Plane, VEC3& Inter)
{
	return intersectionLinePlane(P, Dir, Plane.normal()*Plane.d(), Plane.normal(), Inter);
61 62
}

63
template <typename VEC3>
Pierre Kraemer's avatar
Pierre Kraemer committed
64
Intersection intersectionRayTriangleOpt(const VEC3& P, const VEC3& Dir, const VEC3& Ta, const VEC3& Tb, const VEC3& Tc, VEC3& Inter)
65 66 67 68 69 70 71 72 73 74 75
{
	typedef typename VEC3::DATA_TYPE T ;

	VEC3 u = Ta - P ;
	VEC3 v = Tb - P ;
	VEC3 w = Tc - P ;

	T x = tripleProduct(Dir, u, v) ;
	T y = tripleProduct(Dir, v, w) ;
	T z = tripleProduct(Dir, w, u) ;

Pierre Kraemer's avatar
Pierre Kraemer committed
76 77 78
	unsigned int np = 0 ;
	unsigned int nn = 0 ;
	unsigned int nz = 0 ;
79

Pierre Kraemer's avatar
Pierre Kraemer committed
80 81 82 83
	if (x > T(0))
		++np ;
	else if (x < T(0))
		++nn ;
84
	else
Pierre Kraemer's avatar
Pierre Kraemer committed
85
		++nz ;
86

Pierre Kraemer's avatar
Pierre Kraemer committed
87 88 89 90
	if (y > T(0))
		++np ;
	else if (y < T(0))
		++nn ;
91
	else
Pierre Kraemer's avatar
Pierre Kraemer committed
92
		++nz ;
93

Pierre Kraemer's avatar
Pierre Kraemer committed
94 95 96 97
	if (z > T(0))
		++np ;
	else if (z < T(0))
		++nn ;
98
	else
Pierre Kraemer's avatar
Pierre Kraemer committed
99
		++nz ;
100

Pierre Kraemer's avatar
Pierre Kraemer committed
101
	if ((np != 0) && (nn != 0)) return NO_INTERSECTION ;
102 103 104 105 106 107 108

	T sum = x + y + z ;
	T alpha = y / sum ;
	T beta = z / sum ;
	T gamma = T(1) - alpha - beta ;
	Inter = Ta * alpha + Tb * beta + Tc * gamma ;

Pierre Kraemer's avatar
Pierre Kraemer committed
109
	return Intersection(FACE_INTERSECTION - nz) ;
110 111 112
}

template <typename VEC3>
Pierre Kraemer's avatar
Pierre Kraemer committed
113
Intersection intersectionRayTriangleOpt(const VEC3& P, const VEC3& Dir, const VEC3& Ta, const VEC3& Tb, const VEC3& Tc)
114 115 116 117 118 119 120 121 122 123 124
{
	typedef typename VEC3::DATA_TYPE T ;

	VEC3 u = Ta - P ;
	VEC3 v = Tb - P ;
	VEC3 w = Tc - P ;

	T x = tripleProduct(Dir, u, v) ;
	T y = tripleProduct(Dir, v, w) ;
	T z = tripleProduct(Dir, w, u) ;

Pierre Kraemer's avatar
Pierre Kraemer committed
125 126 127
	unsigned int np = 0 ;
	unsigned int nn = 0 ;
	unsigned int nz = 0 ;
128

Pierre Kraemer's avatar
Pierre Kraemer committed
129 130 131 132
	if (x > T(0))
		++np ;
	else if (x < T(0))
		++nn ;
133
	else
Pierre Kraemer's avatar
Pierre Kraemer committed
134
		++nz ;
135

Pierre Kraemer's avatar
Pierre Kraemer committed
136 137 138 139
	if (y > T(0))
		++np ;
	else if (y < T(0))
		++nn ;
140
	else
Pierre Kraemer's avatar
Pierre Kraemer committed
141
		++nz ;
142

Pierre Kraemer's avatar
Pierre Kraemer committed
143 144 145 146
	if (z > T(0))
		++np ;
	else if (z < T(0))
		++nn ;
147
	else
Pierre Kraemer's avatar
Pierre Kraemer committed
148
		++nz ;
149

Pierre Kraemer's avatar
Pierre Kraemer committed
150
	if ((np != 0) && (nn != 0)) return NO_INTERSECTION ;
151

Pierre Kraemer's avatar
Pierre Kraemer committed
152
	return Intersection(FACE_INTERSECTION - nz) ;
153 154
}

Pierre Kraemer's avatar
Pierre Kraemer committed
155
template <typename VEC3>
Pierre Kraemer's avatar
Pierre Kraemer committed
156
Intersection intersectionRayTriangle(const VEC3& P, const VEC3& Dir, const VEC3& Ta, const VEC3& Tb, const VEC3& Tc, VEC3& Inter)
Pierre Kraemer's avatar
Pierre Kraemer committed
157 158 159 160 161 162 163 164 165
{
	typedef typename VEC3::DATA_TYPE T ;

	VEC3 u = Ta - P ;
	VEC3 v = Tb - P ;
	VEC3 w = Tc - P ;
	T x = tripleProduct(Dir, u, v) ;
	T y = tripleProduct(Dir, v, w) ;
	T z = tripleProduct(Dir, w, u) ;
Sylvain Thery's avatar
MAJ MC  
Sylvain Thery committed
166
//	std::cout << x << " / "<< y << " / "<< z << std::endl;
Pierre Kraemer's avatar
Pierre Kraemer committed
167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190
	if((x < T(0) && y < T(0) && z < T(0)) || (x > T(0) && y > T(0) && z > T(0)))
	{
		T sum = x + y + z ;
		T alpha = y / sum ;
		T beta = z / sum ;
		T gamma = T(1) - alpha - beta ;
		Inter = Ta * alpha + Tb * beta + Tc * gamma ;
		return FACE_INTERSECTION ;
	}
	if(
		( x == T(0) && y > T(0) && z > T(0) ) || ( x == T(0) && y < T(0) && z < T(0) ) || // intersection on [Ta,Tb]
		( x > T(0) && y == T(0) && z > T(0) ) || ( x < T(0) && y == T(0) && z < T(0) ) || // intersection on [Tb,Tc]
		( x > T(0) && y > T(0) && z == T(0) ) || ( x < T(0) && y < T(0) && z == T(0) )	  // intersection on [Tc,Ta]
	)
		return EDGE_INTERSECTION ;
	if(
		( x == T(0) && y == T(0) && z != T(0) ) || // intersection on Tb
		( x == T(0) && y != T(0) && z == T(0) ) || // intersection on Ta
		( x != T(0) && y == T(0) && z == T(0) )	   // intersection on Tc
	)
		return VERTEX_INTERSECTION ;
	return NO_INTERSECTION ;
}

Sylvain Thery's avatar
MAJ MC  
Sylvain Thery committed
191

Pierre Kraemer's avatar
Pierre Kraemer committed
192
template <typename VEC3>
Pierre Kraemer's avatar
Pierre Kraemer committed
193
Intersection intersectionLineTriangle(const VEC3& P, const VEC3& Dir, const VEC3& Ta, const VEC3& Tb, const VEC3& Tc, VEC3& Inter)
Pierre Kraemer's avatar
Pierre Kraemer committed
194 195 196 197 198 199 200 201 202 203 204 205 206
{
	typedef typename VEC3::DATA_TYPE T ;

	VEC3 u = Tb - Ta ;
	VEC3 v = Tc - Ta ;
	VEC3 n = u ^ v ;

	VEC3 w0 = P - Ta ;
    T a = -(n * w0) ;
    T b = (n * Dir) ;

#define PRECISION 1e-20
    if(fabs(b) < PRECISION)			//ray parallel to triangle
untereiner's avatar
untereiner committed
207
			return NO_INTERSECTION ;
Pierre Kraemer's avatar
Pierre Kraemer committed
208 209 210 211 212 213 214 215 216 217 218 219 220 221 222 223 224 225 226 227 228 229 230 231 232 233 234 235 236 237 238 239 240 241 242 243 244 245 246 247 248 249 250 251 252 253 254 255 256 257 258 259 260 261 262 263 264 265 266 267 268 269 270 271 272 273 274 275 276 277
#undef PRECISION

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

    // is I inside T?
	T uu = u.norm2() ;
	T uv = u * v ;
	T vv = v.norm2() ;
	VEC3 w = Inter - Ta ;
	T wu = w * u ;
	T wv = w * v ;
	T D = (uv * uv) - (uu * vv) ;

    // get and test parametric coords
	T s = ((uv * wv) - (vv * wu)) / D ;
	if(s < T(0) || s > T(1))
		return NO_INTERSECTION ;
	T t = ((uv * wu) - (uu * wv)) / D ;
	if(t < T(0) || (s + t) > T(1))
        return NO_INTERSECTION ;

	if((s == T(0) || s == T(1)))
		if(t == T(0) || t == T(1))
			return VERTEX_INTERSECTION ;
		else
			return EDGE_INTERSECTION ;
	else if(t == T(0) || t == T(1))
			return EDGE_INTERSECTION ;

    return FACE_INTERSECTION ;
}

template <typename VEC3>
Intersection intersectionLineTriangle2D(const VEC3& P, const VEC3& Dir, const VEC3& Ta,  const VEC3& Tb, const VEC3& Tc, VEC3& Inter)
{
	Inclusion inc = isPointInTriangle(P,Ta,Tb,Tc) ;
	VEC3 N = Ta ^ Tb ;

	switch(inc)
	{
		case FACE_INCLUSION :
			Inter = P ;
			return FACE_INTERSECTION ;
		case EDGE_INCLUSION :
			Inter = P ;
			return EDGE_INTERSECTION ;
		case VERTEX_INCLUSION :
			Inter = P ;
			return VERTEX_INTERSECTION ;
		default : //NO_INCLUSION : test if ray enters the triangle
			VEC3 P2 = P + Dir ;
			Orientation2D oA = testOrientation2D(Ta, P, P2, N) ;
			Orientation2D oB = testOrientation2D(Tb, P, P2, N) ;
			Orientation2D oC = testOrientation2D(Tc, P, P2, N) ;

			if(oA == oB && oB == oC)
				return NO_INTERSECTION ;

			Orientation2D oPBC = testOrientation2D(P,Tb,Tc,N) ;
			if(oPBC == LEFT)  // same side of A, test edge AC and AB
			{
				if(oA == LEFT)
				{
					if(oB == ALIGNED)
					{
						Inter = Tb ;
						return VERTEX_INTERSECTION ;
					}
					//inter with AB
278
//					CGoGNout << __FILE__ << " TODO compute edge coplanar intersection AB" << CGoGNendl ;
Pierre Kraemer's avatar
Pierre Kraemer committed
279 280 281 282 283 284 285 286 287 288 289 290 291
					return EDGE_INTERSECTION ;
				}
				if(oA == ALIGNED)
				{
					Inter = Ta ;
					return VERTEX_INTERSECTION ;
				}
				if(oC == ALIGNED)
				{
					Inter = Tc ;
					return VERTEX_INTERSECTION ;
				}
				//inter with AC
292
//				CGoGNout << __FILE__ << " TODO compute edge coplanar intersection AC" << CGoGNendl ;
Pierre Kraemer's avatar
Pierre Kraemer committed
293 294 295 296 297 298 299 300 301 302 303 304 305 306 307
				return EDGE_INTERSECTION ;
			}
			if(oPBC == RIGHT) // same side of BC, test this edge
			{
				if(oB == ALIGNED)
				{
					Inter = Tb ;
					return VERTEX_INTERSECTION ;
				}
				if(oC == ALIGNED)
				{
					Inter = Tc ;
					return VERTEX_INTERSECTION ;
				}
				//inter with BC
308
//				CGoGNout << __FILE__ << " TODO compute edge coplanar intersection BC" << CGoGNendl ;
Pierre Kraemer's avatar
Pierre Kraemer committed
309 310 311 312 313 314 315 316 317 318 319 320 321 322
				return EDGE_INTERSECTION ;
			}

			//aligned with BC
			//possibly colliding with edge AB or AC
			Orientation2D oPAB = testOrientation2D(P,Ta,Tb,N) ;
			if(oPAB == RIGHT) //possibly colliding with edge AB
			{
				if(oA == ALIGNED)
				{
					Inter = Ta ;
					return VERTEX_INTERSECTION ;
				}
				//inter with AB
323
//				CGoGNout << __FILE__ << " TODO compute edge coplanar intersection AB" << CGoGNendl ;
Pierre Kraemer's avatar
Pierre Kraemer committed
324 325 326 327 328 329 330 331 332 333 334 335 336 337 338 339 340 341 342
				return EDGE_INTERSECTION ;
			}
			if(oPAB == ALIGNED)
			{
				Inter = Tb ;
				return VERTEX_INTERSECTION ;
			}
			//possibly colliding with edge AC
			else if(oC == ALIGNED)
			{
				Inter = Tc ;
				return VERTEX_INTERSECTION ;
			}
			else if(oA == ALIGNED)
			{
				Inter = Ta ;
				return VERTEX_INTERSECTION ;
			}
			//inter with AC
343
//			CGoGNout << __FILE__ << " TODO compute edge coplanar intersection AC" << CGoGNendl ;
Pierre Kraemer's avatar
Pierre Kraemer committed
344 345 346 347 348 349 350
			return EDGE_INTERSECTION ;
	}
}

template <typename VEC3>
Intersection intersectionSegmentTriangle(const VEC3& PA, const VEC3& PB, const VEC3& Ta, const VEC3& Tb, const VEC3& Tc, VEC3& Inter)
{
Sylvain Thery's avatar
Sylvain Thery committed
351 352
	typedef typename VEC3::DATA_TYPE REAL ;
	const REAL precision = 0.0001f;//std::numeric_limits<T>::min();
Pierre Kraemer's avatar
Pierre Kraemer committed
353 354 355 356 357 358 359 360

	VEC3 u = Tb - Ta ;
	VEC3 v = Tc - Ta ;
	VEC3 Dir = PB - PA ;

	VEC3 n = u ^ v ;

	VEC3 w0 = PA - Ta ;
Sylvain Thery's avatar
Sylvain Thery committed
361 362
	REAL a = -(n * w0);
	REAL b = (n * Dir);
Pierre Kraemer's avatar
Pierre Kraemer committed
363 364

    if(fabs(b) < precision)			//ray parallel to triangle
365
		return NO_INTERSECTION ;
Pierre Kraemer's avatar
Pierre Kraemer committed
366 367

	//compute intersection
Sylvain Thery's avatar
Sylvain Thery committed
368
	REAL r = a / b;
Pierre Kraemer's avatar
Pierre Kraemer committed
369

Sylvain Thery's avatar
Sylvain Thery committed
370
	if ((r < -precision) || (r >(REAL(1) + precision)))
Pierre Kraemer's avatar
Pierre Kraemer committed
371 372 373 374 375
		return NO_INTERSECTION;

	Inter = PA + r * Dir;			// intersect point of ray and plane

    // is I inside T?
Sylvain Thery's avatar
Sylvain Thery committed
376 377 378
	REAL uu = u.norm2();
	REAL uv = u * v;
	REAL vv = v.norm2();
Pierre Kraemer's avatar
Pierre Kraemer committed
379
	VEC3 w = Inter - Ta ;
Sylvain Thery's avatar
Sylvain Thery committed
380 381 382
	REAL wu = w * u;
	REAL wv = w * v;
	REAL D = (uv * uv) - (uu * vv);
Pierre Kraemer's avatar
Pierre Kraemer committed
383 384

    // get and test parametric coords
Sylvain Thery's avatar
Sylvain Thery committed
385
	REAL s = ((uv * wv) - (vv * wu)) / D;
386 387 388 389

	if(s <= precision)
		s = 0.0f;

Sylvain Thery's avatar
Sylvain Thery committed
390
	if (s < REAL(0) || s > REAL(1))
Pierre Kraemer's avatar
Pierre Kraemer committed
391
		return NO_INTERSECTION ;
392

Sylvain Thery's avatar
Sylvain Thery committed
393
	REAL t = ((uv * wu) - (uu * wv)) / D;
394 395 396 397

	if(t <= precision)
		t = 0.0f;

Sylvain Thery's avatar
Sylvain Thery committed
398
	if (t < REAL(0) || (s + t) > REAL(1))
Pierre Kraemer's avatar
Pierre Kraemer committed
399 400
        return NO_INTERSECTION ;

Sylvain Thery's avatar
Sylvain Thery committed
401 402
	if ((s == REAL(0) || s == REAL(1)))
		if (t == REAL(0) || t == REAL(1))
Pierre Kraemer's avatar
Pierre Kraemer committed
403 404 405
			return VERTEX_INTERSECTION ;
		else
			return EDGE_INTERSECTION ;
Sylvain Thery's avatar
Sylvain Thery committed
406
	else if (t == REAL(0) || t == REAL(1))
Pierre Kraemer's avatar
Pierre Kraemer committed
407 408 409 410 411 412 413 414
			return EDGE_INTERSECTION ;

    return FACE_INTERSECTION ;
}

// template <typename VEC3>
// Intersection intersectionSegmentSegment2D(const VEC3& PA, const VEC3& PB, const VEC3& PC,  const VEC3& PD, VEC3& Inter) 
// {
415
// 	CGoGNout << __FILE__ << " " << __LINE__ << " to write intersectionSegmentSegment2D" << CGoGNendl;
Pierre Kraemer's avatar
Pierre Kraemer committed
416 417 418 419
// 	return NO_INTERSECTION;
// }

template <typename VEC3, typename PLANE3D>
Pierre Kraemer's avatar
Pierre Kraemer committed
420
Intersection intersectionPlaneRay(const PLANE3D& pl, const VEC3& p1, const VEC3& dir, VEC3& Inter)
Pierre Kraemer's avatar
Pierre Kraemer committed
421
{
untereiner's avatar
untereiner committed
422
	typename VEC3::DATA_TYPE denom = pl.normal()*dir;
423

Pierre Kraemer's avatar
Pierre Kraemer committed
424
	if (denom == 0)
425
	{
Pierre Kraemer's avatar
Pierre Kraemer committed
426
		if (pl.distance(p1) == 0)
427
		{
Pierre Kraemer's avatar
Pierre Kraemer committed
428 429
			Inter = p1 ;
			return FACE_INTERSECTION ;
430 431
		}
		else
Pierre Kraemer's avatar
Pierre Kraemer committed
432
			return NO_INTERSECTION ;
433 434
	}

Pierre Kraemer's avatar
Pierre Kraemer committed
435
	typename VEC3::DATA_TYPE isect = (pl.normal() * (pl.normal() * -1.0f * pl.d() - p1)) / denom ;
436

Pierre Kraemer's avatar
Pierre Kraemer committed
437
	Inter = p1 + dir * isect ;
Pierre Kraemer's avatar
Pierre Kraemer committed
438

Pierre Kraemer's avatar
Pierre Kraemer committed
439
	if (0.0f <= isect)
untereiner's avatar
untereiner committed
440
	{
Pierre Kraemer's avatar
Pierre Kraemer committed
441
		return FACE_INTERSECTION ;
untereiner's avatar
untereiner committed
442
	}
Pierre Kraemer's avatar
Pierre Kraemer committed
443 444 445 446 447

	return NO_INTERSECTION;
}

template <typename VEC3>
Pierre Kraemer's avatar
Pierre Kraemer committed
448
Intersection intersection2DSegmentSegment(const VEC3& PA, const VEC3& PB, const VEC3& QA, const VEC3& QB, VEC3& Inter)
Pierre Kraemer's avatar
Pierre Kraemer committed
449 450 451
{
	typedef typename VEC3::DATA_TYPE T ;

untereiner's avatar
untereiner committed
452 453 454
	VEC3 vp1p2 = PB - PA;
	VEC3 vq1q2 = QB - QA;
	VEC3 vp1q1 = QA - PA;
Pierre Kraemer's avatar
Pierre Kraemer committed
455 456
	T delta = vp1p2[0] * vq1q2[1] - vp1p2[1] * vq1q2[0] ;
	T coeff = vp1q1[0] * vq1q2[1] - vp1q1[1] * vq1q2[0] ;
untereiner's avatar
untereiner committed
457

Pierre Kraemer's avatar
Pierre Kraemer committed
458
	if (delta == 0) //parallel
untereiner's avatar
untereiner committed
459 460
	{
		//test if collinear
Pierre Kraemer's avatar
Pierre Kraemer committed
461
		if (coeff == 0)
untereiner's avatar
untereiner committed
462 463 464 465 466 467 468 469 470 471
		{
			//collinear
			//TODO : check if there is a common point between the two edges
			Inter = QA;
			return EDGE_INTERSECTION;
		}
		else
			return NO_INTERSECTION;
	}
	else
Pierre Kraemer's avatar
Pierre Kraemer committed
472
		Inter = VEC3((PA[0] * delta + vp1p2[0] * coeff) / delta, (PA[1] * delta + vp1p2[1] * coeff) / delta, (PA[2] * delta + vp1p2[2] * coeff) / delta) ;
untereiner's avatar
untereiner committed
473

474
	//test if inter point is outside the edges
475
	if(
Pierre Kraemer's avatar
Pierre Kraemer committed
476 477 478 479
		(Inter[0] < PA[0] && Inter[0] < PB[0]) || (Inter[0] > PA[0] && Inter[0] > PB[0]) ||
		(Inter[0] < QA[0] && Inter[0] < QB[0]) || (Inter[0] > QA[0] && Inter[0] > QB[0]) ||
		(Inter[1] < PA[1] && Inter[1] < PB[1]) || (Inter[1] > PA[1] && Inter[1] > PB[1]) ||
		(Inter[1] < QA[1] && Inter[1] < QB[1]) || (Inter[1] > QA[1] && Inter[1] > QB[1])
480 481 482
	)
		return NO_INTERSECTION;

Pierre Kraemer's avatar
Pierre Kraemer committed
483
	if(Geom::arePointsEquals(PA, Inter) || Geom::arePointsEquals(PB, Inter) || Geom::arePointsEquals(QA, Inter) || Geom::arePointsEquals(QB, Inter))
untereiner's avatar
untereiner committed
484 485
		return VERTEX_INTERSECTION;

Pierre Kraemer's avatar
Pierre Kraemer committed
486 487 488
	return EDGE_INTERSECTION;
}

489 490 491
template <typename VEC3>
Intersection intersectionSegmentPlan(const VEC3& PA, const VEC3& PB, const VEC3& PlaneP, const VEC3& NormP)//, VEC3& Inter)
{
492 493 494 495 496 497 498 499 500 501 502
//	typename VEC3::DATA_TYPE panp = NormP * PA;
//	typename VEC3::DATA_TYPE pbnp = NormP * PB;

//	if(panp == 0 || pbnp == 0)
//		return VERTEX_INTERSECTION;
//	else if((panp < 0 && pbnp > 0) || (panp > 0 && pbnp < 0))
//		return EDGE_INTERSECTION;
//	else
//		return NO_INTERSECTION;

#define EPSILON 1e-12
503

504 505 506
	typename VEC3::DATA_TYPE panp = NormP * (PA-PlaneP);
	typename VEC3::DATA_TYPE pbnp = NormP * (PB-PlaneP);

Sylvain Thery's avatar
Sylvain Thery committed
507
	if(std::abs(panp) < EPSILON || std::abs(pbnp) < EPSILON)
508
		return VERTEX_INTERSECTION;
509 510 511
//	else if((panp < 0 && pbnp > 0) || (panp > 0 && pbnp < 0))
	else if (panp*pbnp < 0)
			return EDGE_INTERSECTION;
512 513
	else
		return NO_INTERSECTION;
514 515 516 517 518 519 520 521 522 523
#undef EPSILON
}

template <typename VEC3>
Intersection intersectionSegmentPlan(const VEC3& PA, const VEC3& PB, const VEC3& PlaneP, const VEC3& NormP, VEC3& Inter)
{
#define EPSILON 1e-12

	typename VEC3::DATA_TYPE panp = NormP * (PA-PlaneP);
	typename VEC3::DATA_TYPE pbnp = NormP * (PB-PlaneP);
524

Sylvain Thery's avatar
Sylvain Thery committed
525
	if(fabs(panp) < EPSILON)
526 527 528 529
	{
		Inter = PA;
		return VERTEX_INTERSECTION;
	}
Sylvain Thery's avatar
Sylvain Thery committed
530
	else if(fabs(pbnp) < EPSILON)
531 532 533 534 535 536
	{
		Inter = PB;
		return VERTEX_INTERSECTION;
	}
	else if (panp*pbnp < 0)
	{
Sylvain Thery's avatar
Sylvain Thery committed
537
		Inter = (fabs(panp)*PB + fabs(pbnp)*PA)/(fabs(panp)+fabs(pbnp)) ;
538 539 540 541 542
		return EDGE_INTERSECTION;
	}
	else
		return NO_INTERSECTION;
#undef EPSILON
543 544 545
}


546

547 548 549 550 551 552 553 554 555 556 557 558 559 560 561 562 563 564 565 566 567 568 569 570 571 572 573 574 575 576 577 578 579 580 581 582 583 584 585 586 587 588 589 590 591 592 593 594 595 596 597 598 599 600 601 602 603 604 605 606 607 608 609 610 611 612
template <typename VEC3>
Intersection intersectionTrianglePlan(const VEC3& Ta, const VEC3& Tb, const VEC3& Tc, const VEC3& PlaneP, const VEC3& NormP) //, VEC3& Inter) ;
{
	if((intersectionSegmentPlan<VEC3>(Ta,Tb,PlaneP, NormP) == EDGE_INTERSECTION)
			|| (intersectionSegmentPlan<VEC3>(Ta,Tc,PlaneP, NormP) == EDGE_INTERSECTION)
			|| (intersectionSegmentPlan<VEC3>(Tb,Tc,PlaneP, NormP)  == EDGE_INTERSECTION))
	{
		return FACE_INTERSECTION;
	}
	else if((intersectionSegmentPlan<VEC3>(Ta,Tb,PlaneP, NormP) == VERTEX_INTERSECTION)
			|| (intersectionSegmentPlan<VEC3>(Ta,Tc,PlaneP, NormP) == VERTEX_INTERSECTION)
			|| (intersectionSegmentPlan<VEC3>(Tb,Tc,PlaneP, NormP)  == VERTEX_INTERSECTION))
	{
		return VERTEX_INTERSECTION;
	}
	else
	{
		return NO_INTERSECTION;
	}
}

template <typename VEC3>
Intersection intersectionSegmentHalfPlan(const VEC3& PA, const VEC3& PB,
		const VEC3& P, const VEC3& DirP, const VEC3& OrientP)//, VEC3& Inter)
{
	VEC3 NormP = (DirP-P) ^ (OrientP-P) ;
	NormP.normalize() ;

	//intersection SegmentPlan
	Intersection inter = intersectionSegmentPlan(PA,PB,P,NormP);
	if(inter == EDGE_INTERSECTION)
	{
		//and one of the two points must be in the right side of the line
		return intersectionSegmentPlan(PA,PB, P, OrientP);
	}
	else
	{
		return inter;
	}



}

template <typename VEC3>
Intersection intersectionTriangleHalfPlan(const VEC3& Ta, const VEC3& Tb, const VEC3& Tc,
		const VEC3& P, const VEC3& DirP, const VEC3& OrientP) //, VEC3& Inter)
{
	if((intersectionSegmentHalfPlan<VEC3>(Ta,Tb,P, DirP, OrientP) == EDGE_INTERSECTION)
			|| (intersectionSegmentHalfPlan<VEC3>(Ta,Tc,P, DirP, OrientP) == EDGE_INTERSECTION)
			|| (intersectionSegmentHalfPlan<VEC3>(Tb,Tc,P, DirP, OrientP)  == EDGE_INTERSECTION))
	{
		return FACE_INTERSECTION;
	}
	else if((intersectionSegmentHalfPlan<VEC3>(Ta,Tb,P, DirP, OrientP) == VERTEX_INTERSECTION)
			|| (intersectionSegmentHalfPlan<VEC3>(Ta,Tc,P, DirP, OrientP) == VERTEX_INTERSECTION)
			|| (intersectionSegmentHalfPlan<VEC3>(Tb,Tb,P, DirP, OrientP)  == VERTEX_INTERSECTION))
	{
		return FACE_INTERSECTION;
	}
	else
	{
		return NO_INTERSECTION;
	}
}

613 614 615 616 617 618 619 620 621 622 623 624 625 626 627 628 629 630 631 632 633 634 635 636 637 638 639 640 641
template <typename VEC3>
bool interLineSeg(const VEC3& A, const VEC3& AB, typename VEC3::DATA_TYPE AB2,
				  const VEC3& P, const VEC3& Q, VEC3& inter)
{
#define EPSILON (1.0e-5)
	typedef typename VEC3::DATA_TYPE T ;

	T dist = Geom::distancePoint2TrianglePlane(AB-A,A,P,Q);

//	std::cout << "dist "<<  dist << std::endl;

	if (dist>EPSILON)
		return false;

	VEC3 AP = P - A ;
	VEC3 PQ = Q - P ;
	T X = AB * PQ ;
	T beta = ( AB2 * (AP*PQ) - X * (AP*AB) ) / ( X*X - AB2 * PQ.norm2() ) ;

//	std::cout << "beta "<<  beta << std::endl;

	if ((beta<0.0) || (beta>1.0))
		return false;

	inter = beta*Q +(1.0-beta)*P;
	return true;
#undef EPSILON
}

untereiner's avatar
untereiner committed
642
}
Pierre Kraemer's avatar
Pierre Kraemer committed
643

untereiner's avatar
untereiner committed
644
}