particle_cell_2DandHalf.hpp 12.2 KB
Newer Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
/*******************************************************************************
* CGoGN: Combinatorial and Geometric modeling with Generic N-dimensional Maps  *
* version 0.1                                                                  *
* Copyright (C) 2009-2012, IGG Team, LSIIT, University of Strasbourg           *
*                                                                              *
* 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.           *
*                                                                              *
* Web site: http://cgogn.unistra.fr/                                           *
* Contact information: cgogn@unistra.fr                                        *
*                                                                              *
*******************************************************************************/

Thomas's avatar
Thomas committed
25
26
//#define DEBUG

Pierre Kraemer's avatar
Pierre Kraemer committed
27
#include "Geometry/frame.h"
Sylvain Thery's avatar
Sylvain Thery committed
28
#include "Geometry/vector_gen.h"
Pierre Kraemer's avatar
Pierre Kraemer committed
29

Pierre Kraemer's avatar
Pierre Kraemer committed
30
31
32
33
34
35
namespace CGoGN
{

namespace Algo
{

36
37
38
namespace Surface
{

Pierre Kraemer's avatar
Pierre Kraemer committed
39
40
41
namespace MovingObjects
{

Thomas's avatar
Thomas committed
42
43
44
45
46
47
48
49
50
51
52
template <typename PFP>
void ParticleCell2DAndHalf<PFP>::display()
{
// 	CGoGNout << "pos : " << this->m_position << CGoGNendl;
// 	CGoGNout << "d : " << this->d << CGoGNendl;
// 	CGoGNout << "state : " << this->state << CGoGNendl;
}

template <typename PFP>
typename PFP::VEC3 ParticleCell2DAndHalf<PFP>::pointInFace(Dart d)
{
Pierre Kraemer's avatar
Pierre Kraemer committed
53
54
55
56
57
	const VEC3& p1(m_positions[d]) ;
	Dart dd = m.phi1(d) ;
	const VEC3& p2(m_positions[dd]) ;
	dd = m.phi1(dd) ;
	VEC3& p3(m_positions[dd]) ;
Thomas's avatar
Thomas committed
58

Pierre Kraemer's avatar
Pierre Kraemer committed
59
	VEC3 v1(p2 - p1) ;
Thomas's avatar
Thomas committed
60

Pierre Kraemer's avatar
Pierre Kraemer committed
61
	while ((v1 ^ VEC3(p3 - p1)).norm2() == 0.0f)
Thomas's avatar
Thomas committed
62
	{
Pierre Kraemer's avatar
Pierre Kraemer committed
63
64
		dd = m.phi1(dd) ;
		p3 = m_positions[dd] ;
Thomas's avatar
Thomas committed
65
66
	}

Pierre Kraemer's avatar
Pierre Kraemer committed
67
	CGoGNout << "pointInFace " << (p1 + p3) * 0.5f << CGoGNendl ;
Thomas's avatar
Thomas committed
68

Pierre Kraemer's avatar
Pierre Kraemer committed
69
	return (p1 + p3) * 0.5f ;
Thomas's avatar
Thomas committed
70
71
72
73
74
}

template <typename PFP>
Geom::Orientation3D ParticleCell2DAndHalf<PFP>::getOrientationEdge(const VEC3& point, Dart d)
{
Pierre Kraemer's avatar
Pierre Kraemer committed
75
	const VEC3& endPoint = m_positions[m.phi1(d)];
Thomas's avatar
Thomas committed
76
77
	const VEC3& vertexPoint = m_positions[d];

78
	const VEC3& n1 = Geometry::faceNormal<PFP>(m, d, m_positions);
Thomas's avatar
Thomas committed
79
80

	//orientation relative to the plane orthogonal to the face going through the edge
Pierre Kraemer's avatar
Pierre Kraemer committed
81
	return Geom::testOrientation3D(point, vertexPoint, endPoint, vertexPoint+n1);
Thomas's avatar
Thomas committed
82
83
84
85
86
87
}

template <typename PFP>
typename PFP::VEC3 ParticleCell2DAndHalf<PFP>::intersectLineEdge(const VEC3& pA, const VEC3& pB, Dart d)
{
	const VEC3& q1 = m_positions[d];
Pierre Kraemer's avatar
Pierre Kraemer committed
88
	const VEC3& q2 = m_positions[m.phi1(d)];
Thomas's avatar
Thomas committed
89
90
	VEC3 Inter;

91
	VEC3 n1 = Geometry::faceNormal<PFP>(m, d, m_positions);
Pierre Kraemer's avatar
Pierre Kraemer committed
92
	VEC3 n = (q2 - q1) ^ n1 ;
Thomas's avatar
Thomas committed
93

Pierre Kraemer's avatar
Pierre Kraemer committed
94
	Geom::intersectionLinePlane(pA, pB - pA, q1, n, Inter) ;
Thomas's avatar
Thomas committed
95

Sylvain Thery's avatar
Sylvain Thery committed
96
	Geom::Plane3D<REAL> pl = Geometry::facePlane<PFP>(m, d, m_positions);
Thomas's avatar
Thomas committed
97
98
99
100
101
102
103
104
105
106
	pl.project(Inter);

	return Inter;
}

template <typename PFP>
Geom::Orientation3D ParticleCell2DAndHalf<PFP>::getOrientationFace(VEC3 point, VEC3 sourcePoint, Dart d)
{
	const VEC3& dPoint = m_positions[d];

107
	VEC3 n1 = Geometry::faceNormal<PFP>(m, d, m_positions);
Thomas's avatar
Thomas committed
108

Thomas's avatar
Thomas committed
109
	return Geom::testOrientation3D(point, sourcePoint, dPoint+n1, dPoint);
Thomas's avatar
Thomas committed
110
111
112
}

template <typename PFP>
David Cazier's avatar
-    
David Cazier committed
113
void ParticleCell2DAndHalf<PFP>::vertexState(VEC3 goal)
Thomas's avatar
Thomas committed
114
115
116
117
{
	#ifdef DEBUG
	CGoGNout << "vertexState" << d << CGoGNendl;
	#endif
Sylvain Thery's avatar
Sylvain Thery committed
118
	assert(Geom::isFinite(goal));
Thomas's avatar
Thomas committed
119
120
121

	crossCell = CROSS_OTHER;

122
	if(Geometry::isPointOnVertex<PFP>(m,d,m_positions,goal))
Thomas's avatar
Thomas committed
123
124
	{
		state = VERTEX;
pitiot's avatar
pitiot committed
125
		distance += (goal - this->getPosition()).norm();
126
		this->Algo::MovingObjects::ParticleBase<PFP>::move(goal) ;
Thomas's avatar
Thomas committed
127
128
129
130
131
132
		return;
	}
	else
	{
		//orientation step
		if(m_positions[d][0] == m_positions[m.phi1(d)][0] && m_positions[d][1] == m_positions[m.phi1(d)][1])
133
			d = m.phi2_1(d);
134
		if(getOrientationEdge(this->getPosition(),m.phi2_1(d)) != Geom::UNDER)
Thomas's avatar
Thomas committed
135
136
137
138
		{
			Dart dd_vert = d;
			do
			{
139
				d = m.phi2_1(d);
Thomas's avatar
Thomas committed
140
				if(m_positions[d][0] == m_positions[m.phi1(d)][0] && m_positions[d][1] == m_positions[m.phi1(d)][1])
141
					d = m.phi2_1(d);
142
143
//			} while(getOrientationEdge(current, m.phi2_1(d)) != Geom::UNDER && dd_vert != d);
			} while(getOrientationEdge(goal, m.phi2_1(d)) != Geom::UNDER && dd_vert != d);
Thomas's avatar
Thomas committed
144
145
146
147

			if(dd_vert == d)
			{
				//orbit with 2 edges : point on one edge
148
				if(m.phi2_1(m.phi2_1(d)) == d)
Thomas's avatar
Thomas committed
149
				{
150
151
					if(!Geometry::isPointOnHalfEdge<PFP>(m,d,m_positions,goal))
//					if(!Geometry::isPointOnHalfEdge<PFP>(m,d,m_positions,current))
152
						d = m.phi2_1(d);
Thomas's avatar
Thomas committed
153
154
155
				}
				else
				{
pitiot's avatar
pitiot committed
156
					distance += (goal - this->getPosition()).norm();
157
					this->Algo::MovingObjects::ParticleBase<PFP>::move(goal);
Thomas's avatar
Thomas committed
158
159
160
161
162
163
164
					state = VERTEX;
					return;
				}
			}
		}
		else
		{
165
			Dart dd_vert = m.phi2_1(d);
166
167
			while(getOrientationEdge(goal, d) == Geom::OVER && dd_vert != d)
//			while(getOrientationEdge(current, d) == Geom::OVER && dd_vert != d)
Thomas's avatar
Thomas committed
168
			{
169
				d = m.phi12(d);
Thomas's avatar
Thomas committed
170
				if(m_positions[d][0] == m_positions[m.phi1(d)][0] && m_positions[d][1] == m_positions[m.phi1(d)][1])
171
					d = m.phi12(d);
Thomas's avatar
Thomas committed
172
173
174
175
			}
		}

		//displacement step
176
		if(getOrientationEdge(goal, d) == Geom::ON && Geometry::isPointOnHalfEdge<PFP>(m, d, m_positions, goal))
David Cazier's avatar
-    
David Cazier committed
177
			edgeState(goal);
Thomas's avatar
Thomas committed
178
179
180
		else
		{
			d = m.phi1(d);
David Cazier's avatar
-    
David Cazier committed
181
			faceState(goal);
Thomas's avatar
Thomas committed
182
183
184
185
186
		}
	}
}

template <typename PFP>
David Cazier's avatar
-    
David Cazier committed
187
void ParticleCell2DAndHalf<PFP>::edgeState(VEC3 goal, Geom::Orientation3D sideOfEdge)
Thomas's avatar
Thomas committed
188
189
190
191
192
{
	#ifdef DEBUG
	CGoGNout << "edgeState" <<  d << CGoGNendl;
	#endif

Sylvain Thery's avatar
Sylvain Thery committed
193
	assert(Geom::isFinite(goal));
194
// 	assert(Geometry::isPointOnEdge<PFP>(m,d,m_positions,m_position));
Thomas's avatar
Thomas committed
195
196
197
198
199
200
201
202
203
204

	if(crossCell == NO_CROSS)
	{
		crossCell = CROSS_EDGE;
		lastCrossed = d;
	}
	else
		crossCell = CROSS_OTHER;

	if(sideOfEdge == Geom::ON)
David Cazier's avatar
-    
David Cazier committed
205
		sideOfEdge = getOrientationEdge(goal, d);
Thomas's avatar
Thomas committed
206
207
208
209

	switch(sideOfEdge)
	{
		case Geom::UNDER :
Pierre Kraemer's avatar
Pierre Kraemer committed
210
		{
Thomas's avatar
Thomas committed
211
			d = m.phi1(d);
David Cazier's avatar
-    
David Cazier committed
212
			faceState(goal);
Thomas's avatar
Thomas committed
213
			return;
Pierre Kraemer's avatar
Pierre Kraemer committed
214
215
		}

Thomas's avatar
Thomas committed
216
		case Geom::OVER:
Thomas's avatar
Thomas committed
217
		{
Thomas's avatar
Thomas committed
218
			//transform the displacement into the new entered face
219
			VEC3 displ = goal - this->getPosition();
Thomas's avatar
Thomas committed
220

221
222
			VEC3 n1 = Geometry::faceNormal<PFP>(m, d, m_positions);
			VEC3 n2 = Geometry::faceNormal<PFP>(m, m.phi2(d), m_positions);
Pierre Kraemer's avatar
Pierre Kraemer committed
223
			VEC3 axis = n1 ^ n2 ;
Thomas's avatar
Thomas committed
224

Sylvain Thery's avatar
Sylvain Thery committed
225
			REAL angle = Geom::angle(n1, n2) ;
Thomas's avatar
Thomas committed
226

Pierre Kraemer's avatar
Pierre Kraemer committed
227
			displ = Geom::rotate(axis, angle, displ) ;
228
			goal = this->getPosition() + displ;
Thomas's avatar
Thomas committed
229
230

			d = m.phi1(m.phi2(d));
David Cazier's avatar
-    
David Cazier committed
231
			faceState(goal);
Thomas's avatar
Thomas committed
232
			return;
Thomas's avatar
Thomas committed
233
		}
Pierre Kraemer's avatar
Pierre Kraemer committed
234

Thomas's avatar
Thomas committed
235
236
237
238
		default :
			state = EDGE;
	}

239
	if(!Geometry::isPointOnHalfEdge<PFP>(m, d, m_positions, goal))
Thomas's avatar
Thomas committed
240
	{
pitiot's avatar
pitiot committed
241
		distance += (goal - this->getPosition()).norm();
242
		this->Algo::MovingObjects::ParticleBase<PFP>::move(m_positions[d]) ;
David Cazier's avatar
-    
David Cazier committed
243
		vertexState(goal);
Thomas's avatar
Thomas committed
244
245
		return;
	}
246
	else if(!Geometry::isPointOnHalfEdge<PFP>(m, m.phi2(d), m_positions, goal))
Thomas's avatar
Thomas committed
247
248
	{
		d = m.phi2(d);
pitiot's avatar
pitiot committed
249
		distance += (m_positions[d] - this->getPosition()).norm();
250
		this->Algo::MovingObjects::ParticleBase<PFP>::move(m_positions[d]) ;
David Cazier's avatar
-    
David Cazier committed
251
		vertexState(goal);
Thomas's avatar
Thomas committed
252
253
		return;
	}
pitiot's avatar
pitiot committed
254
	distance += (goal - this->getPosition()).norm();
255
	this->Algo::MovingObjects::ParticleBase<PFP>::move(goal);
Thomas's avatar
Thomas committed
256
257
258
}

template <typename PFP>
David Cazier's avatar
-    
David Cazier committed
259
void ParticleCell2DAndHalf<PFP>::faceState(VEC3 goal)
Thomas's avatar
Thomas committed
260
261
262
263
264
{
	#ifdef DEBUG
	CGoGNout << "faceState" <<  d << CGoGNendl;
	#endif

Sylvain Thery's avatar
Sylvain Thery committed
265
266
	assert(Geom::isFinite(goal));
	assert(Geom::isFinite(this->getPosition()));
Thomas's avatar
Thomas committed
267

David Cazier's avatar
-    
David Cazier committed
268
	//project goal within face plane
269
	VEC3 n1 = Geometry::faceNormal<PFP>(m,d,m_positions);
270
	VEC3 n2 = goal - this->getPosition();
Pierre Kraemer's avatar
Pierre Kraemer committed
271
272
//	n1.normalize();
	VEC3 n3 = n1 ^ n2;
pitiot's avatar
pitiot committed
273

Pierre Kraemer's avatar
Pierre Kraemer committed
274
	VEC3 n4 = n3 ^ n1;
pitiot's avatar
pitiot committed
275
	goal = this->getPosition() + n4;
Thomas's avatar
Thomas committed
276
277

	//track new position within map
Thomas's avatar
Thomas committed
278
	Dart dd = d;
Sylvain Thery's avatar
Sylvain Thery committed
279
	Geom::Orientation3D wsoe = getOrientationFace(goal, this->getPosition(), m.phi1(d));
Thomas's avatar
Thomas committed
280
281
282
283
284

	// orientation step
	if(wsoe != Geom::UNDER)
	{
		d = m.phi1(d);
285
		wsoe = getOrientationFace(goal, this->getPosition(), m.phi1(d));
Thomas's avatar
Thomas committed
286
287
288
		while(wsoe != Geom::UNDER && dd != d)
		{
			d = m.phi1(d);
289
			wsoe = getOrientationFace(goal, this->getPosition(), m.phi1(d));
Thomas's avatar
Thomas committed
290
291
292
293
294
295
296
		}

 		// source and position to reach are the same : verify if no edge is crossed due to numerical approximation
		if(dd == d)
		{
			do
			{
David Cazier's avatar
-    
David Cazier committed
297
				switch (getOrientationEdge(goal, d))
Thomas's avatar
Thomas committed
298
299
300
				{
				case Geom::UNDER: 	d = m.phi1(d);
									break;
301
//				case Geom::ON:		this->Algo::MovingObjects::ParticleBase<PFP>::move(current) ;
pitiot's avatar
pitiot committed
302
303
				case Geom::ON:		distance += (goal - this->getPosition()).norm();
									this->Algo::MovingObjects::ParticleBase<PFP>::move(goal) ;
David Cazier's avatar
-    
David Cazier committed
304
									edgeState(goal);
Thomas's avatar
Thomas committed
305
306
									return;
				case Geom::OVER:
David Cazier's avatar
-    
David Cazier committed
307
//									CGoGNout << "smthg went bad " << m_position << " " << goal << CGoGNendl;
Thomas's avatar
Thomas committed
308
//									CGoGNout << "d1 " << m_positions[d] << " d2 " << m_positions[m.phi1(d)] << CGoGNendl;
309
//									this->Algo::MovingObjects::ParticleBase<PFP>::move(intersectLineEdge(current, this->getPosition(), d));
pitiot's avatar
pitiot committed
310
311
312
									VEC3 inter = intersectLineEdge(goal, this->getPosition(), d);
									distance += (inter - this->getPosition()).norm();
									this->Algo::MovingObjects::ParticleBase<PFP>::move(inter);
Thomas's avatar
Thomas committed
313
314
//									CGoGNout << " " << m_position << CGoGNendl;

315
									edgeState(goal,Geom::OVER);
Thomas's avatar
Thomas committed
316
317
318
									return;
				}
			} while(d != dd);
pitiot's avatar
pitiot committed
319
			distance += (goal - this->getPosition()).norm();
320
			this->Algo::MovingObjects::ParticleBase<PFP>::move(goal);
Thomas's avatar
Thomas committed
321
322
			state = FACE;

323
// 			m_position = Geometry::faceCentroid<PFP>(m,d,m_positions);
Thomas's avatar
Thomas committed
324
325
// 			d = m.phi1(d);
// 			m_position = pointInFace(d);
David Cazier's avatar
-    
David Cazier committed
326
// 			faceState(goal);
Thomas's avatar
Thomas committed
327
328
329
330
331
332

// 			m_position = m_positions[d];
// 			vertexState(current);
			return;
		}
		// take the orientation with d1 : in case we are going through a vertex
333
334
//		wsoe = getOrientationFace(current, this->getPosition(), d);
		wsoe = getOrientationFace(goal, this->getPosition(), d);
Thomas's avatar
Thomas committed
335
336
337
	}
	else
	{
338
		wsoe = getOrientationFace(goal,this->getPosition(),d);
Thomas's avatar
Thomas committed
339
340
341
		while(wsoe == Geom::UNDER && m.phi_1(d) != dd)
		{
			d = m.phi_1(d);
342
			wsoe = getOrientationFace(goal, this->getPosition(), d);
Thomas's avatar
Thomas committed
343
344
345
346
347
348
349
350
		}

		// in case of numerical incoherence
		if(m.phi_1(d) == dd && wsoe == Geom::UNDER)
		{
			d = m.phi_1(d);
			do
			{
David Cazier's avatar
-    
David Cazier committed
351
				switch (getOrientationEdge(goal, d))
Thomas's avatar
Thomas committed
352
353
354
355
356
357
				{
				case Geom::UNDER :
					d = m.phi1(d);
					break;
				case Geom::ON :
// 					CGoGNout << "pic" << CGoGNendl;
pitiot's avatar
pitiot committed
358
					distance += (goal - this->getPosition()).norm();
359
					this->Algo::MovingObjects::ParticleBase<PFP>::move(goal) ;
David Cazier's avatar
-    
David Cazier committed
360
					edgeState(goal);
Thomas's avatar
Thomas committed
361
362
363
					return;
				case Geom::OVER:
//					CGoGNout << "smthg went bad(2) " << m_position << CGoGNendl;
pitiot's avatar
pitiot committed
364
365
366
					VEC3 inter = intersectLineEdge(goal, this->getPosition(), d);
					distance += (inter - this->getPosition()).norm();
					this->Algo::MovingObjects::ParticleBase<PFP>::move(inter) ;
Thomas's avatar
Thomas committed
367
// 					CGoGNout << " " << m_position << CGoGNendl;
368
369
//					edgeState(current, Geom::OVER);
					edgeState(goal, Geom::OVER);
Thomas's avatar
Thomas committed
370
371
372
373
					return;
				}
			} while(d != dd);

pitiot's avatar
pitiot committed
374
			distance += (goal - this->getPosition()).norm();
375
			this->Algo::MovingObjects::ParticleBase<PFP>::move(goal);
Thomas's avatar
Thomas committed
376
377
378
379
380
381
			state = FACE;
			return;
		}
	}

	//displacement step
David Cazier's avatar
-    
David Cazier committed
382
	switch (getOrientationEdge(goal, d))
Thomas's avatar
Thomas committed
383
384
	{
	case Geom::UNDER :
385
386
		distance += (goal - this->getPosition()).norm();
		this->Algo::MovingObjects::ParticleBase<PFP>::move(goal);
Pierre Kraemer's avatar
Pierre Kraemer committed
387
		state = FACE;
Thomas's avatar
Thomas committed
388
389
390
391
		break;
	default :
		if(wsoe == Geom::ON)
		{
pitiot's avatar
pitiot committed
392
//			std::cout << __FILE__ << " to uncomment and check" << std::endl;
Thomas's avatar
Thomas committed
393
394
395
//			d = m.phi1(d); //to check
//			m_position = m_positions[d];
//
David Cazier's avatar
-    
David Cazier committed
396
//			vertexState(goal);
Thomas's avatar
Thomas committed
397
		}
Pierre Kraemer's avatar
Pierre Kraemer committed
398
		else
Thomas's avatar
Thomas committed
399
400
401
402
		{
// 			CGoGNout << "wsoe : " << wsoe << CGoGNendl;
// 			CGoGNout << "current " << current << " " << m_position << CGoGNendl;
// 			CGoGNout << "d " << d << "d1 " << m_positions[d] << " d2 " << m_positions[m.phi2(d)] << CGoGNendl;
403
404
405
406
//			VEC3 isect = intersectLineEdge(current, this->getPosition(), d);
			VEC3 isect = intersectLineEdge(goal, this->getPosition(), d);
			distance += (isect - this->getPosition()).norm();
			this->Algo::MovingObjects::ParticleBase<PFP>::move(isect);
Thomas's avatar
Thomas committed
407
// 			CGoGNout << " inter : " << m_position << CGoGNendl;
David Cazier's avatar
-    
David Cazier committed
408
			edgeState(goal, Geom::OVER);
Thomas's avatar
Thomas committed
409
410
411
		}
	}
}
Pierre Kraemer's avatar
Pierre Kraemer committed
412
413
414

} // namespace MovingObjects

415
416
} // namespace Surface

Pierre Kraemer's avatar
Pierre Kraemer committed
417
418
419
} // namespace Algo

} // namespace CGoGN