vector_gen.hpp 11.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 25
* Contact information: cgogn@unistra.fr                                        *
*                                                                              *
*******************************************************************************/

#include <sstream>
David Cazier's avatar
-  
David Cazier committed
26
#include <cmath>
Pierre Kraemer's avatar
Pierre Kraemer committed
27 28 29 30 31 32 33 34

namespace CGoGN
{

namespace Geom
{

template <unsigned int DIM, typename T>
David Cazier's avatar
-  
David Cazier committed
35
std::string Vector<DIM, T>::CGoGNnameOfType()
Pierre Kraemer's avatar
Pierre Kraemer committed
36 37 38 39 40 41 42 43 44 45 46 47 48 49 50
{
	std::stringstream ss ;
	ss << "Geom::Vector<" ;
	ss << DIM ;
	ss << "," ;
	ss << nameOfType(T()) ;
	ss << ">" ;

	return ss.str() ;
}

/**********************************************/
/*                CONSTRUCTORS                */
/**********************************************/

51 52 53 54 55 56 57 58 59
template <unsigned int DIM, typename T>
Vector<DIM, T>::Vector(typename std::initializer_list<T> args)
{
	unsigned int i=0;
	// fill from initializer list
	for (auto iter = args.begin(); (iter != args.end()) && (i<DIM); ++iter, ++i)
		m_data[i] = *iter;

	// finish with 0 if necessary
Sylvain Thery's avatar
Sylvain Thery committed
60
	for (unsigned int j=i;j<DIM;++j)
61 62 63
		m_data[j] = 0;
}

Pierre Kraemer's avatar
Pierre Kraemer committed
64
template <unsigned int DIM, typename T>
David Cazier's avatar
-  
David Cazier committed
65
Vector<DIM, T>::Vector()
Pierre Kraemer's avatar
Pierre Kraemer committed
66 67 68 69 70 71
{
	CGoGN_STATIC_ASSERT(DIM > 0, invalid_zero_dimensional_Vector) ;
	zero() ;
}

template <unsigned int DIM, typename T>
David Cazier's avatar
-  
David Cazier committed
72
Vector<DIM, T>::Vector(const Vector<DIM, T>& v)
Pierre Kraemer's avatar
Pierre Kraemer committed
73
{
David Cazier's avatar
-  
David Cazier committed
74
	for (unsigned int i = 0; i < DIM; ++i)
Pierre Kraemer's avatar
Pierre Kraemer committed
75 76 77
		m_data[i] = v[i] ;
}

78 79
template <unsigned int DIM, typename T>
template <typename T2>
David Cazier's avatar
-  
David Cazier committed
80
Vector<DIM, T>::Vector(const Vector<DIM, T2>& v)
81
{
David Cazier's avatar
-  
David Cazier committed
82
	for (unsigned int i = 0; i < DIM; ++i)
83 84 85
		m_data[i] = T(v[i]) ;
}

Pierre Kraemer's avatar
Pierre Kraemer committed
86
template <unsigned int DIM, typename T>
David Cazier's avatar
-  
David Cazier committed
87
Vector<DIM, T>::Vector(T x, T y)
Pierre Kraemer's avatar
Pierre Kraemer committed
88
{
Pierre Kraemer's avatar
Pierre Kraemer committed
89
	CGoGN_STATIC_ASSERT(DIM == 2, incompatible_Vector_constructor_dimension) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
90 91 92 93 94
	m_data[0] = x ;
	m_data[1] = y ;
}

template <unsigned int DIM, typename T>
David Cazier's avatar
-  
David Cazier committed
95
Vector<DIM, T>::Vector(T x, T y, T z)
Pierre Kraemer's avatar
Pierre Kraemer committed
96
{
Pierre Kraemer's avatar
Pierre Kraemer committed
97
	CGoGN_STATIC_ASSERT(DIM == 3, incompatible_Vector_constructor_dimension) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
98 99 100 101 102 103
	m_data[0] = x ;
	m_data[1] = y ;
	m_data[2] = z ;
}

template <unsigned int DIM, typename T>
David Cazier's avatar
-  
David Cazier committed
104
Vector<DIM, T>::Vector(T x, T y, T z, T w)
Pierre Kraemer's avatar
Pierre Kraemer committed
105
{
Pierre Kraemer's avatar
Pierre Kraemer committed
106
	CGoGN_STATIC_ASSERT(DIM == 4, incompatible_Vector_constructor_dimension) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
107 108 109 110 111 112 113
	m_data[0] = x ;
	m_data[1] = y ;
	m_data[2] = z ;
	m_data[3] = w ;
}

template <unsigned int DIM, typename T>
David Cazier's avatar
-  
David Cazier committed
114
Vector<DIM, T>::Vector(T x)
Pierre Kraemer's avatar
Pierre Kraemer committed
115 116 117 118 119
{
	set(x) ;
}

template <unsigned int DIM, typename T>
David Cazier's avatar
-  
David Cazier committed
120
inline void Vector<DIM, T>::set(T a)
Pierre Kraemer's avatar
Pierre Kraemer committed
121
{
David Cazier's avatar
-  
David Cazier committed
122
	for (unsigned int i = 0; i < DIM; ++i)
Pierre Kraemer's avatar
Pierre Kraemer committed
123 124 125 126
		m_data[i] = a ;
}

template <unsigned int DIM, typename T>
David Cazier's avatar
-  
David Cazier committed
127
inline void Vector<DIM, T>::zero()
Pierre Kraemer's avatar
Pierre Kraemer committed
128 129 130 131 132 133 134 135 136
{
	set(T(0)) ;
}

/**********************************************/
/*                 ACCESSORS                  */
/**********************************************/

template <unsigned int DIM, typename T>
David Cazier's avatar
-  
David Cazier committed
137
inline T& Vector<DIM, T>::operator[](unsigned int index)
Pierre Kraemer's avatar
Pierre Kraemer committed
138 139 140 141 142 143
{
	assert(index < DIM) ;
	return m_data[index] ;
}

template <unsigned int DIM, typename T>
David Cazier's avatar
-  
David Cazier committed
144
inline const T& Vector<DIM, T>::operator[](unsigned int index) const
Pierre Kraemer's avatar
Pierre Kraemer committed
145 146 147 148 149 150
{
	assert(index < DIM) ;
	return m_data[index] ;
}

template <unsigned int DIM, typename T>
David Cazier's avatar
-  
David Cazier committed
151
inline unsigned int Vector<DIM, T>::dimension() const
Pierre Kraemer's avatar
Pierre Kraemer committed
152 153 154 155 156
{
	return DIM ;
}

template <unsigned int DIM, typename T>
David Cazier's avatar
-  
David Cazier committed
157
inline T* Vector<DIM, T>::data()
Pierre Kraemer's avatar
Pierre Kraemer committed
158 159 160 161 162
{
	return m_data ;
}

template <unsigned int DIM, typename T>
David Cazier's avatar
-  
David Cazier committed
163
inline const T* Vector<DIM, T>::data() const
Pierre Kraemer's avatar
Pierre Kraemer committed
164 165 166 167 168 169 170 171 172
{
	return m_data ;
}

/**********************************************/
/*         ARITHMETIC SELF-OPERATORS          */
/**********************************************/

template <unsigned int DIM, typename T>
David Cazier's avatar
-  
David Cazier committed
173
inline Vector<DIM, T>& Vector<DIM, T>::operator+=(const Vector<DIM, T>& v)
Pierre Kraemer's avatar
Pierre Kraemer committed
174
{
David Cazier's avatar
-  
David Cazier committed
175
	for (unsigned int i = 0; i < DIM; ++i)
Pierre Kraemer's avatar
Pierre Kraemer committed
176 177 178 179 180
		m_data[i] += v[i] ;
	return *this ;
}

template <unsigned int DIM, typename T>
David Cazier's avatar
-  
David Cazier committed
181
inline Vector<DIM, T>& Vector<DIM, T>::operator-=(const Vector<DIM, T>& v)
Pierre Kraemer's avatar
Pierre Kraemer committed
182
{
David Cazier's avatar
-  
David Cazier committed
183
	for (unsigned int i = 0; i < DIM; ++i)
Pierre Kraemer's avatar
Pierre Kraemer committed
184 185 186 187 188
		m_data[i] -= v[i] ;
	return *this ;
}

template <unsigned int DIM, typename T>
Sylvain Thery's avatar
Sylvain Thery committed
189
inline Vector<DIM, T>& Vector<DIM, T>::operator*=(T a)
Pierre Kraemer's avatar
Pierre Kraemer committed
190
{
David Cazier's avatar
-  
David Cazier committed
191
	for (unsigned int i = 0; i < DIM; ++i)
Pierre Kraemer's avatar
Pierre Kraemer committed
192 193 194 195 196
		m_data[i] *= a ;
	return *this ;
}

template <unsigned int DIM, typename T>
Sylvain Thery's avatar
Sylvain Thery committed
197
inline Vector<DIM, T>& Vector<DIM, T>::operator/=(T a)
Pierre Kraemer's avatar
Pierre Kraemer committed
198
{
David Cazier's avatar
-  
David Cazier committed
199
	for (unsigned int i = 0; i < DIM; ++i)
Pierre Kraemer's avatar
Pierre Kraemer committed
200 201 202 203 204 205 206 207 208
		m_data[i] /= a ;
	return *this ;
}

/**********************************************/
/*            ARITHMETIC OPERATORS            */
/**********************************************/

template <unsigned int DIM, typename T>
David Cazier's avatar
-  
David Cazier committed
209
inline Vector<DIM, T> Vector<DIM, T>::operator+(const Vector<DIM, T>& v) const
Pierre Kraemer's avatar
Pierre Kraemer committed
210
{
David Cazier's avatar
-  
David Cazier committed
211 212
	Vector<DIM, T> res ;
	for (unsigned int i = 0; i < DIM; ++i)
Pierre Kraemer's avatar
Pierre Kraemer committed
213 214 215 216 217
		res[i] = m_data[i] + v[i] ;
	return res ;
}

template <unsigned int DIM, typename T>
David Cazier's avatar
-  
David Cazier committed
218
inline Vector<DIM, T> Vector<DIM, T>::operator-(const Vector<DIM, T>& v) const
Pierre Kraemer's avatar
Pierre Kraemer committed
219
{
David Cazier's avatar
-  
David Cazier committed
220 221
	Vector<DIM, T> res ;
	for (unsigned int i = 0; i < DIM; ++i)
Pierre Kraemer's avatar
Pierre Kraemer committed
222 223 224 225
		res[i] = m_data[i] - v[i] ;
	return res ;
}

226 227 228 229 230 231 232 233 234 235
template <unsigned int DIM, typename T>
inline Vector<DIM, T> Vector<DIM, T>::operator-() const
{
	Vector<DIM, T> res ;
	for (unsigned int i = 0; i < DIM; ++i)
		res[i] = - m_data[i] ;
	return res ;
}


Pierre Kraemer's avatar
Pierre Kraemer committed
236
template <unsigned int DIM, typename T>
David Cazier's avatar
-  
David Cazier committed
237
inline Vector<DIM, T> Vector<DIM, T>::operator*(T a) const
Pierre Kraemer's avatar
Pierre Kraemer committed
238
{
David Cazier's avatar
-  
David Cazier committed
239 240
	Vector<DIM, T> res ;
	for (unsigned int i = 0; i < DIM; ++i)
Pierre Kraemer's avatar
Pierre Kraemer committed
241 242 243 244 245
		res[i] = m_data[i] * a ;
	return res ;
}

template <unsigned int DIM, typename T>
David Cazier's avatar
-  
David Cazier committed
246
inline Vector<DIM, T> Vector<DIM, T>::operator/(T a) const
Pierre Kraemer's avatar
Pierre Kraemer committed
247
{
David Cazier's avatar
-  
David Cazier committed
248 249
	Vector<DIM, T> res ;
	for (unsigned int i = 0; i < DIM; ++i)
Pierre Kraemer's avatar
Pierre Kraemer committed
250 251 252 253 254 255 256 257 258
		res[i] = m_data[i] / a ;
	return res ;
}

/**********************************************/
/*             UTILITY FUNCTIONS              */
/**********************************************/

template <unsigned int DIM, typename T>
David Cazier's avatar
-  
David Cazier committed
259
inline T Vector<DIM, T>::norm2() const
Pierre Kraemer's avatar
Pierre Kraemer committed
260 261
{
	T n(0) ;
David Cazier's avatar
-  
David Cazier committed
262
	for (unsigned int i = 0; i < DIM; ++i)
Pierre Kraemer's avatar
Pierre Kraemer committed
263 264 265 266 267
		n += m_data[i] * m_data[i] ;
	return n ;
}

template <unsigned int DIM, typename T>
David Cazier's avatar
-  
David Cazier committed
268
inline double Vector<DIM, T>::norm() const
Pierre Kraemer's avatar
Pierre Kraemer committed
269 270 271 272 273
{
	return sqrt(norm2()) ;
}

template <unsigned int DIM, typename T>
David Cazier's avatar
-  
David Cazier committed
274
inline double Vector<DIM, T>::normalize()
Pierre Kraemer's avatar
Pierre Kraemer committed
275 276
{
	double n = norm() ;
David Cazier's avatar
-  
David Cazier committed
277 278
	if (n != T(0)) for (unsigned int i = 0; i < DIM; ++i)
		m_data[i] /= T(n) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
279 280 281
	return n ;
}

282 283 284 285 286 287 288 289
template <unsigned int DIM, typename T>
inline Vector<DIM, T> Vector<DIM, T>::normalized() const
{
	Vector<DIM, T> v(*this);
	v.normalize();
	return v;
}

Pierre Kraemer's avatar
Pierre Kraemer committed
290
template <unsigned int DIM, typename T>
Pierre Kraemer's avatar
Pierre Kraemer committed
291
inline T Vector<DIM, T>::operator*(const Vector<DIM, T>& v) const
Pierre Kraemer's avatar
Pierre Kraemer committed
292 293
{
	T d(0) ;
David Cazier's avatar
-  
David Cazier committed
294
	for (unsigned int i = 0; i < DIM; ++i)
Pierre Kraemer's avatar
Pierre Kraemer committed
295 296 297 298 299
		d += m_data[i] * v[i] ;
	return d ;
}

template <unsigned int DIM, typename T>
Pierre Kraemer's avatar
Pierre Kraemer committed
300
inline Vector<DIM, T> Vector<DIM, T>::operator^(const Vector<DIM, T>& v) const
Pierre Kraemer's avatar
Pierre Kraemer committed
301 302
{
	CGoGN_STATIC_ASSERT(DIM == 3, incompatible_Vector_cross_product_dimension) ;
David Cazier's avatar
-  
David Cazier committed
303
	Vector<DIM, T> c ;
Pierre Kraemer's avatar
Pierre Kraemer committed
304 305 306 307 308 309 310
	c[0] = m_data[1] * v[2] - m_data[2] * v[1] ;
	c[1] = m_data[2] * v[0] - m_data[0] * v[2] ;
	c[2] = m_data[0] * v[1] - m_data[1] * v[0] ;
	return c ;
}

template <unsigned int DIM, typename T>
David Cazier's avatar
-  
David Cazier committed
311
inline bool Vector<DIM, T>::operator==(const Vector<DIM, T>& v) const
Pierre Kraemer's avatar
Pierre Kraemer committed
312
{
David Cazier's avatar
-  
David Cazier committed
313 314
	for (unsigned int i = 0; i < DIM; ++i)
		if (v[i] != m_data[i]) return false ;
Pierre Kraemer's avatar
Pierre Kraemer committed
315 316 317
	return true ;
}

Pierre Kraemer's avatar
Pierre Kraemer committed
318
template <unsigned int DIM, typename T>
David Cazier's avatar
-  
David Cazier committed
319
inline bool Vector<DIM, T>::operator!=(const Vector<DIM, T>& v) const
Pierre Kraemer's avatar
Pierre Kraemer committed
320
{
David Cazier's avatar
-  
David Cazier committed
321 322
	for (unsigned int i = 0; i < DIM; ++i)
		if (v[i] != m_data[i]) return true ;
Pierre Kraemer's avatar
Pierre Kraemer committed
323 324 325
	return false ;
}

326
template <unsigned int DIM, typename T>
David Cazier's avatar
-  
David Cazier committed
327
inline bool Vector<DIM, T>::hasNan() const
328
{
David Cazier's avatar
-  
David Cazier committed
329 330
	for (unsigned int i = 0; i < DIM; ++i)
		if (m_data[i] != m_data[i]) return true ;
331 332 333
	return false ;
}

334
template <unsigned int DIM, typename T>
David Cazier's avatar
David Cazier committed
335
inline bool Vector<DIM, T>::isFinite() const
David Cazier's avatar
-  
David Cazier committed
336 337
{
	for (unsigned int i = 0; i < DIM; ++i)
David Cazier's avatar
David Cazier committed
338
		if (!std::isfinite(m_data[i])) return false ;
David Cazier's avatar
-  
David Cazier committed
339 340 341 342 343
	return true ;
}

template <unsigned int DIM, typename T>
inline bool Vector<DIM, T>::isNormalized(const T& epsilon) const
344
{
David Cazier's avatar
-  
David Cazier committed
345
	return (1 - epsilon < norm2() && norm2() < 1 + epsilon) ;
346 347 348
}

template <unsigned int DIM, typename T>
David Cazier's avatar
David Cazier committed
349
inline bool Vector<DIM, T>::isOrthogonal(const Vector<DIM, T>& v, const T& epsilon) const
350
{
David Cazier's avatar
David Cazier committed
351 352 353 354 355
	return (fabs(v * (*this)) < epsilon) ;
}

template <unsigned int DIM, typename T>
inline bool Vector<DIM, T>::isNear(const Vector<DIM, T>& v, int precision) const
356
{
David Cazier's avatar
David Cazier committed
357 358 359 360 361 362 363 364 365
	T diff ;
	T norm2(0) ;
	for (unsigned int i = 0 ; i < DIM ; ++i)
	{
		diff = m_data[i] - v[i] ;
		if (!isNull(diff, precision)) return false ;
		norm2 += diff * diff ;
	}
	return isNull2(norm2, precision) ;
366 367
}

Pierre Kraemer's avatar
Pierre Kraemer committed
368 369 370 371 372
/**********************************************/
/*             STREAM OPERATORS               */
/**********************************************/

template <unsigned int DIM, typename T>
David Cazier's avatar
-  
David Cazier committed
373
std::ostream& operator<<(std::ostream& out, const Vector<DIM, T>& v)
Pierre Kraemer's avatar
Pierre Kraemer committed
374
{
David Cazier's avatar
-  
David Cazier committed
375
	for (unsigned int i = 0; i < DIM; ++i)
Pierre Kraemer's avatar
Pierre Kraemer committed
376 377 378 379 380
		out << v[i] << " " ;
	return out ;
}

template <unsigned int DIM, typename T>
David Cazier's avatar
-  
David Cazier committed
381
std::istream& operator>>(std::istream& in, Vector<DIM, T>& v)
Pierre Kraemer's avatar
Pierre Kraemer committed
382
{
David Cazier's avatar
-  
David Cazier committed
383
	for (unsigned int i = 0; i < DIM; ++i)
Pierre Kraemer's avatar
Pierre Kraemer committed
384 385 386 387
		in >> v[i] ;
	return in ;
}

David Cazier's avatar
David Cazier committed
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
/***
 * Test if x is null within precision.
 * 3 cases are possible :
 *  - precision = 0 : x is null <=> (x == 0)
 *  - precision > 0 : x is null <=> (|x| < precision)
 *  - precision < 0 : x is null <=> (|x| < 1/precision) <=> (precision * |x| < 1)
 */
template <typename T>
inline bool isNull(T x, int precision)
{
	if (precision == 0)
		return (x == 0) ;
	else if (precision > 0)
			return (fabs(x) < precision) ;
	else
		return (precision * fabs(x) < 1) ;
}

/***
 * Test if the square root of x is null within precision.
 * In other words, test if x is null within precision*precision
 */
template <typename T>
inline bool isNull2(T x, int precision)
{
	if (precision == 0)
		return (x == 0) ;
	else if (precision > 0)
		return (isNull(x, precision * precision)) ;
	else
		return (isNull(x, - (precision * precision))) ;
}

421 422
template <unsigned int DIM, typename T, typename T2>
inline Vector<DIM, T> operator*(T2 b, const Vector<DIM, T>& v)
Pierre Kraemer's avatar
Pierre Kraemer committed
423
{
424
	return v * T(b) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
425 426
}

427

Pierre Kraemer's avatar
Pierre Kraemer committed
428
template <unsigned int DIM, typename T>
David Cazier's avatar
-  
David Cazier committed
429
inline Vector<DIM, T> operator/(T a, const Vector<DIM, T>& v)
Pierre Kraemer's avatar
Pierre Kraemer committed
430 431 432 433 434
{
	return v / a ;
}

template <unsigned int DIM, typename T>
David Cazier's avatar
-  
David Cazier committed
435
inline T tripleProduct(const Vector<DIM, T>& v1, const Vector<DIM, T>& v2, const Vector<DIM, T>& v3)
Pierre Kraemer's avatar
Pierre Kraemer committed
436 437 438 439 440
{
	return v1 * (v2 ^ v3) ;
}

template <unsigned int DIM, typename T>
David Cazier's avatar
-  
David Cazier committed
441
inline Vector<DIM, T> slerp(const Vector<DIM, T>& v1, const Vector<DIM, T>& v2, const T& t)
442
{
David Cazier's avatar
-  
David Cazier committed
443
	Vector<DIM, T> res ;
Pierre Kraemer's avatar
Pierre Kraemer committed
444

445 446 447 448 449 450 451 452 453
	T scal = v1 * v2 ;

	// Prevention for floating point errors
	if (1 < scal && scal < 1 + 1e-6)
		scal = T(1) ;
	if (-1. - 1e-6 < scal && scal < -1)
		scal = -T(1) ;

	T omega = acos(scal) ;
454
	T den = sin(omega) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
455

David Cazier's avatar
-  
David Cazier committed
456
	if (-1e-8 < den && den < 1e-8) return t < 0.5 ? v1 : v2 ;
Pierre Kraemer's avatar
Pierre Kraemer committed
457

David Cazier's avatar
-  
David Cazier committed
458 459
	T f1 = sin((T(1) - t) * omega) / den ;	// assume 0 <= t <= 1
	T f2 = sin(t * omega) / den ;
Pierre Kraemer's avatar
Pierre Kraemer committed
460

461 462
	res += f1 * v1 ;
	res += f2 * v2 ;
Pierre Kraemer's avatar
Pierre Kraemer committed
463 464 465 466 467 468 469

	return res ;
}

} // namespace Geom

} // namespace CGoGN