particle_cell_2DandHalf.hpp 12.1 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 28
#include "Geometry/frame.h"

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

namespace Algo
{

35 36 37
namespace Surface
{

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

Thomas's avatar
Thomas committed
41 42 43 44 45 46 47 48 49 50 51
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
52 53 54 55 56
	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
57

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

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

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

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

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

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

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

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
87
	const VEC3& q2 = m_positions[m.phi1(d)];
Thomas's avatar
Thomas committed
88 89
	VEC3 Inter;

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

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

95
	Geom::Plane3D<float> pl = Geometry::facePlane<PFP>(m, d, m_positions);
Thomas's avatar
Thomas committed
96 97 98 99 100 101 102 103 104 105
	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];

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

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

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

	crossCell = CROSS_OTHER;

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

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

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

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

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

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

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

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

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

220 221
			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
222
			VEC3 axis = n1 ^ n2 ;
Thomas's avatar
Thomas committed
223

Pierre Kraemer's avatar
Pierre Kraemer committed
224
			float angle = Geom::angle(n1, n2) ;
Thomas's avatar
Thomas committed
225

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

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

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

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

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

264
	assert(goal.isFinite()) ;
David Cazier's avatar
-  
David Cazier committed
265
	assert(this->getPosition().isFinite()) ;
Thomas's avatar
Thomas committed
266

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

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

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

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

 		// 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
296
				switch (getOrientationEdge(goal, d))
Thomas's avatar
Thomas committed
297 298 299
				{
				case Geom::UNDER: 	d = m.phi1(d);
									break;
300
//				case Geom::ON:		this->Algo::MovingObjects::ParticleBase<PFP>::move(current) ;
pitiot's avatar
pitiot committed
301 302
				case Geom::ON:		distance += (goal - this->getPosition()).norm();
									this->Algo::MovingObjects::ParticleBase<PFP>::move(goal) ;
David Cazier's avatar
-  
David Cazier committed
303
									edgeState(goal);
Thomas's avatar
Thomas committed
304 305
									return;
				case Geom::OVER:
David Cazier's avatar
-  
David Cazier committed
306
//									CGoGNout << "smthg went bad " << m_position << " " << goal << CGoGNendl;
Thomas's avatar
Thomas committed
307
//									CGoGNout << "d1 " << m_positions[d] << " d2 " << m_positions[m.phi1(d)] << CGoGNendl;
308
//									this->Algo::MovingObjects::ParticleBase<PFP>::move(intersectLineEdge(current, this->getPosition(), d));
pitiot's avatar
pitiot committed
309 310 311
									VEC3 inter = intersectLineEdge(goal, this->getPosition(), d);
									distance += (inter - this->getPosition()).norm();
									this->Algo::MovingObjects::ParticleBase<PFP>::move(inter);
Thomas's avatar
Thomas committed
312 313
//									CGoGNout << " " << m_position << CGoGNendl;

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

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

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

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

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

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

} // namespace MovingObjects

414 415
} // namespace Surface

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

} // namespace CGoGN