vector_gen.hpp 9.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
* 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 51
{
	std::stringstream ss ;
	ss << "Geom::Vector<" ;
	ss << DIM ;
	ss << "," ;
	ss << nameOfType(T()) ;
	ss << ">" ;

	return ss.str() ;
}

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

template <unsigned int DIM, typename T>
David Cazier's avatar
-  
David Cazier committed
52
Vector<DIM, T>::Vector()
Pierre Kraemer's avatar
Pierre Kraemer committed
53 54 55 56 57 58
{
	CGoGN_STATIC_ASSERT(DIM > 0, invalid_zero_dimensional_Vector) ;
	zero() ;
}

template <unsigned int DIM, typename T>
David Cazier's avatar
-  
David Cazier committed
59
Vector<DIM, T>::Vector(const Vector<DIM, T>& v)
Pierre Kraemer's avatar
Pierre Kraemer committed
60
{
David Cazier's avatar
-  
David Cazier committed
61
	for (unsigned int i = 0; i < DIM; ++i)
Pierre Kraemer's avatar
Pierre Kraemer committed
62 63 64
		m_data[i] = v[i] ;
}

65 66
template <unsigned int DIM, typename T>
template <typename T2>
David Cazier's avatar
-  
David Cazier committed
67
Vector<DIM, T>::Vector(const Vector<DIM, T2>& v)
68
{
David Cazier's avatar
-  
David Cazier committed
69
	for (unsigned int i = 0; i < DIM; ++i)
70 71 72
		m_data[i] = T(v[i]) ;
}

Pierre Kraemer's avatar
Pierre Kraemer committed
73
template <unsigned int DIM, typename T>
David Cazier's avatar
-  
David Cazier committed
74
Vector<DIM, T>::Vector(T x, T y)
Pierre Kraemer's avatar
Pierre Kraemer committed
75
{
Pierre Kraemer's avatar
Pierre Kraemer committed
76
	CGoGN_STATIC_ASSERT(DIM == 2, incompatible_Vector_constructor_dimension) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
77 78 79 80 81
	m_data[0] = x ;
	m_data[1] = y ;
}

template <unsigned int DIM, typename T>
David Cazier's avatar
-  
David Cazier committed
82
Vector<DIM, T>::Vector(T x, T y, T z)
Pierre Kraemer's avatar
Pierre Kraemer committed
83
{
Pierre Kraemer's avatar
Pierre Kraemer committed
84
	CGoGN_STATIC_ASSERT(DIM == 3, incompatible_Vector_constructor_dimension) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
85 86 87 88 89 90
	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
91
Vector<DIM, T>::Vector(T x, T y, T z, T w)
Pierre Kraemer's avatar
Pierre Kraemer committed
92
{
Pierre Kraemer's avatar
Pierre Kraemer committed
93
	CGoGN_STATIC_ASSERT(DIM == 4, incompatible_Vector_constructor_dimension) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
94 95 96 97 98 99 100
	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
101
Vector<DIM, T>::Vector(T x)
Pierre Kraemer's avatar
Pierre Kraemer committed
102 103 104 105 106
{
	set(x) ;
}

template <unsigned int DIM, typename T>
David Cazier's avatar
-  
David Cazier committed
107
inline void Vector<DIM, T>::set(T a)
Pierre Kraemer's avatar
Pierre Kraemer committed
108
{
David Cazier's avatar
-  
David Cazier committed
109
	for (unsigned int i = 0; i < DIM; ++i)
Pierre Kraemer's avatar
Pierre Kraemer committed
110 111 112 113
		m_data[i] = a ;
}

template <unsigned int DIM, typename T>
David Cazier's avatar
-  
David Cazier committed
114
inline void Vector<DIM, T>::zero()
Pierre Kraemer's avatar
Pierre Kraemer committed
115 116 117 118 119 120 121 122 123
{
	set(T(0)) ;
}

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

template <unsigned int DIM, typename T>
David Cazier's avatar
-  
David Cazier committed
124
inline T& Vector<DIM, T>::operator[](unsigned int index)
Pierre Kraemer's avatar
Pierre Kraemer committed
125 126 127 128 129 130
{
	assert(index < DIM) ;
	return m_data[index] ;
}

template <unsigned int DIM, typename T>
David Cazier's avatar
-  
David Cazier committed
131
inline const T& Vector<DIM, T>::operator[](unsigned int index) const
Pierre Kraemer's avatar
Pierre Kraemer committed
132 133 134 135 136 137
{
	assert(index < DIM) ;
	return m_data[index] ;
}

template <unsigned int DIM, typename T>
David Cazier's avatar
-  
David Cazier committed
138
inline unsigned int Vector<DIM, T>::dimension() const
Pierre Kraemer's avatar
Pierre Kraemer committed
139 140 141 142 143
{
	return DIM ;
}

template <unsigned int DIM, typename T>
David Cazier's avatar
-  
David Cazier committed
144
inline T* Vector<DIM, T>::data()
Pierre Kraemer's avatar
Pierre Kraemer committed
145 146 147 148 149
{
	return m_data ;
}

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

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

template <unsigned int DIM, typename T>
David Cazier's avatar
-  
David Cazier committed
160
inline Vector<DIM, T>& Vector<DIM, T>::operator+=(const Vector<DIM, T>& v)
Pierre Kraemer's avatar
Pierre Kraemer committed
161
{
David Cazier's avatar
-  
David Cazier committed
162
	for (unsigned int i = 0; i < DIM; ++i)
Pierre Kraemer's avatar
Pierre Kraemer committed
163 164 165 166 167
		m_data[i] += v[i] ;
	return *this ;
}

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

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

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

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

template <unsigned int DIM, typename T>
David Cazier's avatar
-  
David Cazier committed
196
inline Vector<DIM, T> Vector<DIM, T>::operator+(const Vector<DIM, T>& v) const
Pierre Kraemer's avatar
Pierre Kraemer committed
197
{
David Cazier's avatar
-  
David Cazier committed
198 199
	Vector<DIM, T> res ;
	for (unsigned int i = 0; i < DIM; ++i)
Pierre Kraemer's avatar
Pierre Kraemer committed
200 201 202 203 204
		res[i] = m_data[i] + v[i] ;
	return res ;
}

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

template <unsigned int DIM, typename T>
David Cazier's avatar
-  
David Cazier committed
214
inline Vector<DIM, T> Vector<DIM, T>::operator*(T a) const
Pierre Kraemer's avatar
Pierre Kraemer committed
215
{
David Cazier's avatar
-  
David Cazier committed
216 217
	Vector<DIM, T> res ;
	for (unsigned int i = 0; i < DIM; ++i)
Pierre Kraemer's avatar
Pierre Kraemer committed
218 219 220 221 222
		res[i] = m_data[i] * a ;
	return res ;
}

template <unsigned int DIM, typename T>
David Cazier's avatar
-  
David Cazier committed
223
inline Vector<DIM, T> Vector<DIM, T>::operator/(T a) const
Pierre Kraemer's avatar
Pierre Kraemer committed
224
{
David Cazier's avatar
-  
David Cazier committed
225 226
	Vector<DIM, T> res ;
	for (unsigned int i = 0; i < DIM; ++i)
Pierre Kraemer's avatar
Pierre Kraemer committed
227 228 229 230 231 232 233 234 235
		res[i] = m_data[i] / a ;
	return res ;
}

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

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

template <unsigned int DIM, typename T>
David Cazier's avatar
-  
David Cazier committed
245
inline double Vector<DIM, T>::norm() const
Pierre Kraemer's avatar
Pierre Kraemer committed
246 247 248 249 250
{
	return sqrt(norm2()) ;
}

template <unsigned int DIM, typename T>
David Cazier's avatar
-  
David Cazier committed
251
inline double Vector<DIM, T>::normalize()
Pierre Kraemer's avatar
Pierre Kraemer committed
252 253
{
	double n = norm() ;
David Cazier's avatar
-  
David Cazier committed
254 255
	if (n != T(0)) for (unsigned int i = 0; i < DIM; ++i)
		m_data[i] /= T(n) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
256 257 258 259
	return n ;
}

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

template <unsigned int DIM, typename T>
David Cazier's avatar
-  
David Cazier committed
269
inline Vector<DIM, T> Vector<DIM, T>::operator^(const Vector<DIM, T> v) const
Pierre Kraemer's avatar
Pierre Kraemer committed
270 271
{
	CGoGN_STATIC_ASSERT(DIM == 3, incompatible_Vector_cross_product_dimension) ;
David Cazier's avatar
-  
David Cazier committed
272
	Vector<DIM, T> c ;
Pierre Kraemer's avatar
Pierre Kraemer committed
273 274 275 276 277 278 279
	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
280
inline bool Vector<DIM, T>::operator==(const Vector<DIM, T>& v) const
Pierre Kraemer's avatar
Pierre Kraemer committed
281
{
David Cazier's avatar
-  
David Cazier committed
282 283
	for (unsigned int i = 0; i < DIM; ++i)
		if (v[i] != m_data[i]) return false ;
Pierre Kraemer's avatar
Pierre Kraemer committed
284 285 286
	return true ;
}

Pierre Kraemer's avatar
Pierre Kraemer committed
287
template <unsigned int DIM, typename T>
David Cazier's avatar
-  
David Cazier committed
288
inline bool Vector<DIM, T>::operator!=(const Vector<DIM, T>& v) const
Pierre Kraemer's avatar
Pierre Kraemer committed
289
{
David Cazier's avatar
-  
David Cazier committed
290 291
	for (unsigned int i = 0; i < DIM; ++i)
		if (v[i] != m_data[i]) return true ;
Pierre Kraemer's avatar
Pierre Kraemer committed
292 293 294
	return false ;
}

Pierre Kraemer's avatar
Pierre Kraemer committed
295
template <unsigned int DIM, typename T>
David Cazier's avatar
-  
David Cazier committed
296
inline bool Vector<DIM, T>::hasNan() const
Pierre Kraemer's avatar
Pierre Kraemer committed
297
{
David Cazier's avatar
-  
David Cazier committed
298 299
	for (unsigned int i = 0; i < DIM; ++i)
		if (m_data[i] != m_data[i]) return true ;
Pierre Kraemer's avatar
Pierre Kraemer committed
300 301 302
	return false ;
}

303
template <unsigned int DIM, typename T>
David Cazier's avatar
-  
David Cazier committed
304 305 306 307 308 309 310 311 312
inline bool Vector<DIM, T>::isNormal() const
{
	for (unsigned int i = 0; i < DIM; ++i)
		if (!std::isnormal(m_data[i])) return false ;
	return true ;
}

template <unsigned int DIM, typename T>
inline bool Vector<DIM, T>::isNormalized(const T& epsilon) const
313
{
David Cazier's avatar
-  
David Cazier committed
314
	return (1 - epsilon < norm2() && norm2() < 1 + epsilon) ;
315 316 317
}

template <unsigned int DIM, typename T>
David Cazier's avatar
-  
David Cazier committed
318
inline bool Vector<DIM, T>::isOrthogonal(const Vector<DIM, T>& V, const T& epsilon) const
319 320 321 322
{
	return (fabs(V * (*this)) < epsilon) ;
}

Pierre Kraemer's avatar
Pierre Kraemer committed
323 324 325 326 327
/**********************************************/
/*             STREAM OPERATORS               */
/**********************************************/

template <unsigned int DIM, typename T>
David Cazier's avatar
-  
David Cazier committed
328
std::ostream& operator<<(std::ostream& out, const Vector<DIM, T>& v)
Pierre Kraemer's avatar
Pierre Kraemer committed
329
{
David Cazier's avatar
-  
David Cazier committed
330
	for (unsigned int i = 0; i < DIM; ++i)
Pierre Kraemer's avatar
Pierre Kraemer committed
331 332 333 334 335
		out << v[i] << " " ;
	return out ;
}

template <unsigned int DIM, typename T>
David Cazier's avatar
-  
David Cazier committed
336
std::istream& operator>>(std::istream& in, Vector<DIM, T>& v)
Pierre Kraemer's avatar
Pierre Kraemer committed
337
{
David Cazier's avatar
-  
David Cazier committed
338
	for (unsigned int i = 0; i < DIM; ++i)
Pierre Kraemer's avatar
Pierre Kraemer committed
339 340 341 342 343
		in >> v[i] ;
	return in ;
}

template <unsigned int DIM, typename T>
David Cazier's avatar
-  
David Cazier committed
344
inline Vector<DIM, T> operator*(T a, const Vector<DIM, T>& v)
Pierre Kraemer's avatar
Pierre Kraemer committed
345 346 347 348 349
{
	return v * a ;
}

template <unsigned int DIM, typename T>
David Cazier's avatar
-  
David Cazier committed
350
inline Vector<DIM, T> operator/(T a, const Vector<DIM, T>& v)
Pierre Kraemer's avatar
Pierre Kraemer committed
351 352 353 354 355
{
	return v / a ;
}

template <unsigned int DIM, typename T>
David Cazier's avatar
-  
David Cazier committed
356
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
357 358 359 360 361
{
	return v1 * (v2 ^ v3) ;
}

template <unsigned int DIM, typename T>
David Cazier's avatar
-  
David Cazier committed
362
inline Vector<DIM, T> slerp(const Vector<DIM, T>& v1, const Vector<DIM, T>& v2, const T& t)
363
{
David Cazier's avatar
-  
David Cazier committed
364
	Vector<DIM, T> res ;
Pierre Kraemer's avatar
Pierre Kraemer committed
365

366
	T omega = acos(v1 * v2) ;	// assume v1,v2 normalized
367
	T den = sin(omega) ;
Pierre Kraemer's avatar
Pierre Kraemer committed
368

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

David Cazier's avatar
-  
David Cazier committed
371 372
	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
373

374 375
	res += f1 * v1 ;
	res += f2 * v2 ;
Pierre Kraemer's avatar
Pierre Kraemer committed
376 377 378 379 380 381 382

	return res ;
}

} // namespace Geom

} // namespace CGoGN