qem.h 8.95 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
25
26
27
* Contact information: cgogn@unistra.fr                                        *
*                                                                              *
*******************************************************************************/

#ifndef __QEM__
#define __QEM__

28
#include "Utils/os_spec.h" // allow compilation under windows
29
#include <cmath>
30

Pierre Kraemer's avatar
Pierre Kraemer committed
31
32
33
34
#include "Geometry/vector_gen.h"
#include "Geometry/matrix.h"
#include "Geometry/plane_3d.h"

35
36
37
38
namespace CGoGN {

//namespace Utils {

Pierre Kraemer's avatar
Pierre Kraemer committed
39
40
41
42
43
44
template <typename REAL>
class Quadric
{
public:
	static std::string CGoGNnameOfType() { return "Quadric" ; }

45
46
47
	typedef Geom::Vector<3,REAL> VEC3 ;
	typedef Geom::Vector<4,REAL> VEC4 ;
	typedef Geom::Matrix<4,4,double> MATRIX44 ; // double is crucial here !
Pierre Kraemer's avatar
Pierre Kraemer committed
48
49
50
51
52
53
54
55
56
57
58
59
60
61

	Quadric()
	{
		A.zero() ;
	}

	Quadric(int i)
	{
		A.zero() ;
	}

	Quadric(VEC3& p1, VEC3& p2, VEC3& p3)
	{
		// compute the equation of the plane of the 3 points
62
63
64
65
66
		Geom::Plane3D<REAL> plane(p1, p2, p3) ;
		const VEC3& n = plane.normal() ;

		Geom::Vector<4,double> p = Geom::Vector<4,double>(n[0],n[1],n[2],plane.d()) ;
		A = Geom::transposed_vectors_mult(p,p) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
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
	}

	void zero()
	{
		A.zero() ;
	}

	void operator= (const Quadric<REAL>& q)
	{
		A = q.A ;
	}
	Quadric& operator+= (const Quadric<REAL>& q)
	{
		A += q.A ;
		return *this ;
	}
	Quadric& operator -= (const Quadric<REAL>& q)
	{
		A -= q.A ;
		return *this ;
	}
	Quadric& operator *= (REAL v)
	{
		A *= v ;
		return *this ;
	}
	Quadric& operator /= (REAL v)
	{
		A /= v ;
		return *this ;
	}

	REAL operator() (const VEC4& v) const
	{
		return evaluate(v) ;
	}

	REAL operator() (const VEC3& v) const
	{
		VEC4 hv(v[0], v[1], v[2], 1.0f) ;
		return evaluate(hv) ;
	}

	friend std::ostream& operator<<(std::ostream& out, const Quadric<REAL>& q)
	{
		out << q.A ;
		return out ;
	}

	friend std::istream& operator>>(std::istream& in, Quadric<REAL>& q)
	{
		in >> q.A ;
		return in ;
	}

	bool findOptimizedPos(VEC3& v)
	{
		VEC4 hv ;
		bool b = optimize(hv) ;
		if(b)
		{
			v[0] = hv[0] ;
			v[1] = hv[1] ;
			v[2] = hv[2] ;
		}
		return b ;
	}

private:
	MATRIX44 A ;

138
	REAL evaluate(const VEC4& v) const
Pierre Kraemer's avatar
Pierre Kraemer committed
139
	{
140
141
		// Double computation is crucial for stability
		Geom::Vector<4, double> Av = A * v ;
Pierre Kraemer's avatar
Pierre Kraemer committed
142
143
144
145
146
		return v * Av ;
	}

	bool optimize(VEC4& v) const
	{
147
		if (std::isnan(A(0,0)))
Kenneth Vanhoey's avatar
Kenneth Vanhoey committed
148
149
			return false ;

Pierre Kraemer's avatar
Pierre Kraemer committed
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
		MATRIX44 A2(A) ;
		for(int i = 0; i < 3; ++i)
			A2(3,i) = 0.0f ;
		A2(3,3) = 1.0f ;

		MATRIX44 Ainv ;
		REAL det = A2.invert(Ainv) ;

		if(det > -1e-6 && det < 1e-6)
			return false ;

		VEC4 right(0,0,0,1) ;
		v = Ainv * right ;

		return true;
	}
} ;

168
169
170
171
172
173
174
175
176
177
178
template <typename REAL, unsigned int N>
class QuadricNd
{
public:
	static std::string CGoGNnameOfType() { return "QuadricNd" ; }

	typedef Geom::Vector<N,REAL> VECN ;
	typedef Geom::Vector<N+1,REAL> VECNp ;

	QuadricNd()
	{
179
180
181
		A.zero() ;
		b.zero() ;
		c = 0 ;
182
183
184
185
	}

	QuadricNd(int i)
	{
186
187
188
		A.zero() ;
		b.zero() ;
		c = 0 ;
189
190
191
192
193
194
195
196
197
	}

	QuadricNd(const VECN& p1_r, const VECN& p2_r, const VECN& p3_r)
	{
		const Geom::Vector<N,double>& p1 = p1_r ;
		const Geom::Vector<N,double>& p2 = p2_r ;
		const Geom::Vector<N,double>& p3 = p3_r ;

		Geom::Vector<N,double> e1 = p2 - p1 ; 						e1.normalize() ;
198
		Geom::Vector<N,double> e2 = (p3 - p1) - (e1*(p3-p1))*e1 ; 	e2.normalize() ;
199
200

		A.identity() ;
201
		A -= Geom::transposed_vectors_mult(e1,e1) + Geom::transposed_vectors_mult(e2,e2) ;
202

203
		b = (p1*e1)*e1 + (p1*e2)*e2 - p1 ;
204

205
		c = p1*p1 - pow((p1*e1),2) - pow((p1*e2),2) ;
206
207
208
209
	}

	void zero()
	{
210
211
212
		A.zero() ;
		b.zero() ;
		c = 0 ;
213
214
215
216
	}

	void operator= (const QuadricNd<REAL,N>& q)
	{
217
218
219
		A = q.A ;
		b = q.b ;
		c = q.c ;
220
221
222
	}
	QuadricNd& operator+= (const QuadricNd<REAL,N>& q)
	{
223
224
225
		A += q.A ;
		b += q.b ;
		c += q.c ;
226
227
		return *this ;
	}
228

229
230
	QuadricNd& operator -= (const QuadricNd<REAL,N>& q)
	{
231
232
233
		A -= q.A ;
		b -= q.b ;
		c -= q.c ;
234
235
		return *this ;
	}
236

237
238
	QuadricNd& operator *= (REAL v)
	{
239
240
241
		A *= v ;
		b *= v ;
		c *= v ;
242
243
244
245
		return *this ;
	}
	QuadricNd& operator /= (REAL v)
	{
246
247
248
		A /= v ;
		b /= v ;
		c /= v ;
249
250
251
252
253
		return *this ;
	}

	REAL operator() (const VECNp& v) const
	{
254
255
256
257
		VECN hv ;
		for (unsigned int i = 0 ; i < N ; ++i)
			hv[i] = v[i] ;

258
259
260
261
262
		return evaluate(v) ;
	}

	REAL operator() (const VECN& v) const
	{
263
		return evaluate(v) ;
264
265
266
267
	}

	friend std::ostream& operator<<(std::ostream& out, const QuadricNd<REAL,N>& q)
	{
268
		out << "(" << q.A << ", " << q.b << ", " << q.c << ")" ;
269
270
271
272
273
		return out ;
	}

	friend std::istream& operator>>(std::istream& in, QuadricNd<REAL,N>& q)
	{
274
275
276
		in >> q.A ;
		in >> q.b ;
		in >> q.c ;
277
278
279
		return in ;
	}

280
	bool findOptimizedVec(VECN& v)
281
	{
282
		return optimize(v) ;
283
284
285
	}

private:
286
287
288
289
	// Double computation is crucial for stability
	Geom::Matrix<N,N,double> A ;
	Geom::Vector<N,double> b ;
	double c ;
290

291
	REAL evaluate(const VECN& v) const
292
	{
293
294
		Geom::Vector<N, double> v_d = v ;
		return v_d*A*v_d + 2.*(b*v_d) + c ;
295
296
	}

297
	bool optimize(VECN& v) const
298
	{
299
		if (std::isnan(A(0,0)))
300
301
			return false ;

302
303
		Geom::Matrix<N,N,double> Ainv ;
		double det = A.invert(Ainv) ;
304
305
306
307

		if(det > -1e-6 && det < 1e-6)
			return false ;

308
309
		v.zero() ;
		v -= Ainv * b ;
310

311
		return true ;
312
313
314
	}
} ;

315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
template <typename REAL>
class QuadricHF
{
	// TODO
public:
	static std::string CGoGNnameOfType() { return "QuadricHF" ; }

	typedef Geom::Vector<3,REAL> VECN ;
	typedef Geom::Vector<4,REAL> VECNp ;

	QuadricHF()
	{
		A.zero() ;
		b.zero() ;
		c = 0 ;
	}

	QuadricHF(int i)
	{
		A.zero() ;
		b.zero() ;
		c = 0 ;
	}

	QuadricHF(const VECN& p1_r, const VECN& p2_r, const VECN& p3_r)
	{
		const Geom::Vector<3,double>& p1 = p1_r ;
		const Geom::Vector<3,double>& p2 = p2_r ;
		const Geom::Vector<3,double>& p3 = p3_r ;

		Geom::Vector<3,double> e1 = p2 - p1 ; 						e1.normalize() ;
		Geom::Vector<3,double> e2 = (p3 - p1) - (e1*(p3-p1))*e1 ; 	e2.normalize() ;

		A.identity() ;
		A -= Geom::transposed_vectors_mult(e1,e1) + Geom::transposed_vectors_mult(e2,e2) ;

		b = (p1*e1)*e1 + (p1*e2)*e2 - p1 ;

		c = p1*p1 - pow((p1*e1),2) - pow((p1*e2),2) ;
	}

	void zero()
	{
		A.zero() ;
		b.zero() ;
		c = 0 ;
	}

	void operator= (const QuadricHF<REAL>& q)
	{
		A = q.A ;
		b = q.b ;
		c = q.c ;
	}
	QuadricHF& operator+= (const QuadricHF<REAL>& q)
	{
		A += q.A ;
		b += q.b ;
		c += q.c ;
		return *this ;
	}

	QuadricHF& operator -= (const QuadricHF<REAL>& q)
	{
		A -= q.A ;
		b -= q.b ;
		c -= q.c ;
		return *this ;
	}

	QuadricHF& operator *= (REAL v)
	{
		A *= v ;
		b *= v ;
		c *= v ;
		return *this ;
	}
	QuadricHF& operator /= (REAL v)
	{
		A /= v ;
		b /= v ;
		c /= v ;
		return *this ;
	}

	REAL operator() (const VECNp& v) const
	{
		VECN hv ;
		for (unsigned int i = 0 ; i < 3 ; ++i)
			hv[i] = v[i] ;

		return evaluate(v) ;
	}

	REAL operator() (const VECN& v) const
	{
		return evaluate(v) ;
	}

	friend std::ostream& operator<<(std::ostream& out, const QuadricHF<REAL>& q)
	{
		out << "(" << q.A << ", " << q.b << ", " << q.c << ")" ;
		return out ;
	}

	friend std::istream& operator>>(std::istream& in, QuadricHF<REAL>& q)
	{
		in >> q.A ;
		in >> q.b ;
		in >> q.c ;
		return in ;
	}

	bool findOptimizedVec(VECN& v)
	{
		return optimize(v) ;
	}

private:
	// Double computation is crucial for stability
	Geom::Matrix<3,3,double> A ;
	Geom::Vector<3,double> b ;
	double c ;

	REAL evaluate(const VECN& v) const
	{
		Geom::Vector<3, double> v_d = v ;
		return v_d*A*v_d + 2.*(b*v_d) + c ;
	}

	bool optimize(VECN& v) const
	{
		if (std::isnan(A(0,0)))
			return false ;

		Geom::Matrix<3,3,double> Ainv ;
		double det = A.invert(Ainv) ;

		if(det > -1e-6 && det < 1e-6)
			return false ;

		v.zero() ;
		v -= Ainv * b ;

		return true ;
	}
} ;

463
464
465
466
//} // Utils

} // CGOGN

Pierre Kraemer's avatar
Pierre Kraemer committed
467
#endif