ITANumericUtils.cpp 14.2 KB
Newer Older
1
#include <ITANumericUtils.h>
Jonas Stienen's avatar
Jonas Stienen committed
2 3 4 5

#include <ITAConstants.h>
#include <ITAException.h>

6
#include <cassert>
7 8
#include <numeric>

Jonas Stienen's avatar
Jonas Stienen committed
9 10
using namespace ITAConstants;

11
int getExp2( const unsigned int x )
12
{
Jonas Stienen's avatar
Jonas Stienen committed
13 14 15
	// Algorithmus: Bits überprüfen. Falls zwei Bits 1 sind -> keine 2er-Potenz

	// Triviale Fälle:
16 17 18 19 20 21
	if( x == 0 ) return -1;	// 0 ist keine Zweierpotenz
	if( x == 1 ) return 0;     // 2^0 = 1 

	bool s = false;				// Suchflag
	unsigned int n = sizeof( x ) * 8;	// Anzahl der Bits im Datentyp
	unsigned int k = 0;			// Zähler
Jonas Stienen's avatar
Jonas Stienen committed
22 23 24

	// Erstes 1-Bit von rechts nach links (LSB->MSB) suchen
	// (Muss gefunden werden, da x!=0)
25 26 27 28
	while( ( ( x >> k ) & 1 ) == 0 )
		k++;

	unsigned int l = k;	// Exponent speichern
Jonas Stienen's avatar
Jonas Stienen committed
29 30

	// Weitersuchen
31 32
	if( k < n )
	{
Jonas Stienen's avatar
Jonas Stienen committed
33
		k++;
34 35
		while( !( s = ( ( ( x >> k ) & 1 ) == 1 ) ) && k < n )
			k++;
Jonas Stienen's avatar
Jonas Stienen committed
36
	}
37 38

	return ( s ? -1 : ( int ) l );
Jonas Stienen's avatar
Jonas Stienen committed
39 40
}

41
bool isPow2( const unsigned int x )
42 43 44
{
	return getExp2( x ) != -1;
}
Jonas Stienen's avatar
Jonas Stienen committed
45

46
unsigned int nextPow2( const unsigned int x )
47
{
Jonas Stienen's avatar
Jonas Stienen committed
48
	// Trivialer Fall:
49 50 51 52 53
	if( x == 0 ) return 1;

	bool s = false;		// Suchflag
	int n = sizeof( x ) * 8;	// Anzahl der Bits im Datentyp
	int k = n - 1;			// Zähler
Jonas Stienen's avatar
Jonas Stienen committed
54 55 56

	// Erstes 1-Bit von links nach rechts (MSB->LSB) suchen
	// (Muss gefunden werden, da x!=0)
57 58 59 60
	while( ( ( x >> k ) & 1 ) == 0 )
		k--;

	unsigned int l = k;	// Exponent speichern
Jonas Stienen's avatar
Jonas Stienen committed
61 62

	// Weitersuchen
63 64
	if( k > 1 )
	{
Jonas Stienen's avatar
Jonas Stienen committed
65
		k--;
66 67
		while( !( s = ( ( ( x >> k ) & 1 ) == 1 ) ) && k > 0 )
			k--;
Jonas Stienen's avatar
Jonas Stienen committed
68
	}
69

Jonas Stienen's avatar
Jonas Stienen committed
70
	// Zweites 1-Bit gefunden? Dann keine Zweierpotenz...
71
	return ( s ? ( ( unsigned int ) 1 ) << ( l + 1 ) : x );
Jonas Stienen's avatar
Jonas Stienen committed
72 73
}

74
int lwrmul( const int x, const int mul )
75
{
Jonas Stienen's avatar
Jonas Stienen committed
76
	int r = x % mul;	// Teilungsrest
77
	return ( r == 0 ? x : ( x > 0 ? x - r : x - r - mul ) );
Jonas Stienen's avatar
Jonas Stienen committed
78 79
}

80
int uprmul( const int x, const int mul )
81
{
Jonas Stienen's avatar
Jonas Stienen committed
82
	int r = x % mul;	// Teilungsrest
83
	return ( r == 0 ? x : ( x > 0 ? x + mul - r : x - r ) );
Jonas Stienen's avatar
Jonas Stienen committed
84 85
}

86
unsigned int lwrmulu( const unsigned int x, const unsigned int mul )
87
{
88
	return x - ( x % mul );
Jonas Stienen's avatar
Jonas Stienen committed
89 90
}

91
unsigned int uprmulu( const unsigned int x, const unsigned int mul )
92
{
Jonas Stienen's avatar
Jonas Stienen committed
93
	unsigned int r = x % mul;	// Teilungsrest
94
	return ( r == 0 ? x : x + mul - r );
Jonas Stienen's avatar
Jonas Stienen committed
95 96
}

97 98
int uprdiv( const int a, const int b )
{
Jonas Stienen's avatar
Jonas Stienen committed
99
	// TODO: Testen mit negativen Zahlen!
100 101
	int c = a / b;
	if( ( a%b ) > 0 ) c++;
Jonas Stienen's avatar
Jonas Stienen committed
102 103 104
	return c;
}

105 106 107 108 109
unsigned int uprdivu( const unsigned int a, const unsigned int b )
{
	if( b == 0 )
		return 0;

110 111
	unsigned int c = a / b;
	if( ( a%b ) > 0 ) c++;
Jonas Stienen's avatar
Jonas Stienen committed
112 113 114
	return c;
}

115 116
float rad2gradf( const float phi )
{
Jonas Stienen's avatar
Jonas Stienen committed
117 118 119
	return phi * 180.0F / PI_F;
}

120 121
double rad2grad( const double phi )
{
Jonas Stienen's avatar
Jonas Stienen committed
122 123 124
	return phi * 180.0 / PI_D;
}

125 126 127
float grad2radf( const float phi )
{
	return phi * PI_F / 180.0f;
Jonas Stienen's avatar
Jonas Stienen committed
128 129
}

130 131 132
double grad2rad( const double phi )
{
	return phi * PI_D / 180.0f;
Jonas Stienen's avatar
Jonas Stienen committed
133 134
}

135 136
float correctAngle180( const float phi )
{
137
	if( fabs( phi ) == 180.0f )
138 139 140 141 142 143
		return phi;

	const float phi_temp = fmodf( phi, 360.0f );
	if( phi_temp < -180.0f )
		return ( phi_temp + 360.0f );
	return ( fmodf( phi_temp + 180.0f, 360.0f ) - 180.0f );
Jonas Stienen's avatar
Jonas Stienen committed
144 145
}

146 147
float correctAngle360( const float phi )
{
148
	return fmodf( phi, 360.0f );
Jonas Stienen's avatar
Jonas Stienen committed
149 150
}

151 152 153
double db10_to_ratio( const double db )
{
	return ( db == -std::numeric_limits< double >::infinity() ? 0.0f : pow( 10.0f, db / 10.0f ) );
Jonas Stienen's avatar
Jonas Stienen committed
154 155
}

156 157 158
double db20_to_ratio( const double db )
{
	return ( db == -std::numeric_limits< double >::infinity() ? 0.0f : pow( 10.0f, db / 20.0f ) );
Jonas Stienen's avatar
Jonas Stienen committed
159 160
}

161 162 163 164 165 166 167 168 169
double ratio_to_db10( const double r )
{
	if( r < 0.0f )
		ITA_EXCEPT1( INVALID_PARAMETER, "Conversion to decibel not possible for negative values" );

	if( r == 0.0f )
		return -std::numeric_limits< double >::infinity();
	else
		return 10.0f * log10( r );
Jonas Stienen's avatar
Jonas Stienen committed
170 171
}

172
double ratio_to_db20( const double r )
173 174 175 176 177
{
	if( r < 0.0f )
		ITA_EXCEPT1( INVALID_PARAMETER, "Conversion to decibel not possible for negative values" );

	if( r == 0.0f )
178
		return -std::numeric_limits< double >::infinity();
179 180
	else
		return 20.0f * log10( r );
Jonas Stienen's avatar
Jonas Stienen committed
181 182
}

183 184 185
float cabsf( const float fReal, const float fImag )
{
	return sqrt( fReal * fReal + fImag * fImag );
Jonas Stienen's avatar
Jonas Stienen committed
186 187
}

188 189 190 191
float canglef( const float re, const float im )
{
	if( re == 0 )
	{
Jonas Stienen's avatar
Jonas Stienen committed
192
		// Realteil = 0
193
		if( im == 0 )
Jonas Stienen's avatar
Jonas Stienen committed
194 195 196 197
			// Komplexe Zahl 0+0i. Winkel nach Konvention 0
			return 0;
		else
			// Rein Imaginäre Zahl, d.h. im/re -> Division durch Null!
198
			return ( im > 0 ? PI_F / 2.0f : -PI_F / 2.0f );
199 200 201
	}
	else {
		if( re > 0 )
Jonas Stienen's avatar
Jonas Stienen committed
202
			// Re > 0, Im != 0 -> Standardberechnung über Arcustangens.
203
			return atan( im / re );
Jonas Stienen's avatar
Jonas Stienen committed
204 205
		else
			// Re < 0, Im != 0 -> Fallunterscheidungen
206
			return ( im >= 0 ? atan( im / re ) + PI_F : atan( im / re ) - PI_F );
Jonas Stienen's avatar
Jonas Stienen committed
207 208 209
	}
}

210 211 212 213 214
void csabsparg( const float in_re, const float in_im, const float dest_abs, float& out_re, float& out_im )
{
	if( dest_abs < 0.0f )
		ITA_EXCEPT_INVALID_PARAMETER( "Absolute value must be greater or equal zero" );

Jonas Stienen's avatar
Jonas Stienen committed
215 216
	// Nur Anpassung der Länge, d.h. Multiplikation mit konstantem
	// reellen Verlängerungsfaktor c = dest_abs / abs(in)
217

218 219 220 221 222 223 224 225 226 227 228 229
	const float fMag = cabsf( in_re, in_im );
	if( fMag == 0.0f )
	{
		out_re = 0.0f;
		out_im = 0.0f;
	}
	else
	{
		const float c = dest_abs / fMag;
		out_re = in_re * c;
		out_im = in_im * c;
	}
Jonas Stienen's avatar
Jonas Stienen committed
230 231
}

232 233
void csargpabs( const float in_re, const float in_im, const float dest_arg, float& out_re, float& out_im )
{
Jonas Stienen's avatar
Jonas Stienen committed
234 235
	// Nur Anpassung des Winkels: (1) Bestimmung des gegebenen 
	// Betrags danach (2) Auswerten der Eulerformel
236 237 238 239

	float l = cabsf( in_re, in_im );
	out_re = l * cos( dest_arg );
	out_im = l * sin( dest_arg );
Jonas Stienen's avatar
Jonas Stienen committed
240 241 242
}

/* +----------- RAD -----------+ */
243 244
float anglef_proj_0_2PI( const float alpha )
{
245
	float alpha_temp = fmodf( alpha, 2 * PI_F );
246 247 248
	if( alpha_temp < 0 )
		alpha_temp += 2 * PI_F;
	return alpha_temp;
Jonas Stienen's avatar
Jonas Stienen committed
249 250
}

251 252
float anglef_proj_NPI_PI( const float alpha )
{
253
	float x = anglef_proj_0_2PI( alpha - PI_F ) + PI_F;
Jonas Stienen's avatar
Jonas Stienen committed
254 255 256
	return x;
}

257 258
float anglef_proj_NPIH_PIH( const float alpha )
{
259
	float alpha_temp = fmodf( alpha + PI_F / 2.0f, PI_F ) - PI_F / 2.0f;
260 261 262 263 264
	if( alpha_temp < -PI_F/2.0f )
		alpha_temp += PI_F;
	return alpha_temp;
}

265 266
float anglef_mindiff_0_2PI( const float alpha, const float beta )
{
267 268 269
	float gamma = anglef_proj_0_2PI( beta ) - anglef_proj_0_2PI( alpha );
	if( gamma >= 0 )
		return ( gamma <= PI_F ? gamma : gamma - 2 * PI_F );
Jonas Stienen's avatar
Jonas Stienen committed
270
	else
271
		return ( gamma >= -PI_F ? gamma : gamma + 2 * PI_F );
Jonas Stienen's avatar
Jonas Stienen committed
272 273
}

274 275
float anglef_mindiff_abs_0_2PI( const float alpha, const float beta )
{
Jonas Stienen's avatar
Jonas Stienen committed
276
	// TODO: Schnellere Implementierung möglich
277
	return fabs( anglef_mindiff_0_2PI( alpha, beta ) );
Jonas Stienen's avatar
Jonas Stienen committed
278 279
}

280 281
float anglef_proj_0_360_DEG( const float alpha )
{
282
	return rad2gradf( anglef_proj_0_2PI( grad2radf( alpha ) ) );
Jonas Stienen's avatar
Jonas Stienen committed
283 284
}

285
float anglef_proj_N180_180_DEG( const float alpha ) {
286
	return rad2gradf( anglef_proj_NPI_PI( grad2radf( alpha ) ) );
Jonas Stienen's avatar
Jonas Stienen committed
287 288
}

289 290 291 292
float anglef_proj_N90_90_DEG( const float alpha ) {
	return rad2gradf( anglef_proj_NPIH_PIH( grad2radf( alpha ) ) );
}

293 294
float anglef_mindiff_0_360_DEG( const float alpha, const float beta )
{
295
	return rad2gradf( anglef_mindiff_0_2PI( grad2radf( alpha ), grad2radf( beta ) ) );
Jonas Stienen's avatar
Jonas Stienen committed
296 297
}

298 299
float anglef_mindiff_abs_0_360_DEG( const float alpha, const  float beta )
{
300
	return rad2gradf( anglef_mindiff_abs_0_2PI( grad2radf( alpha ), grad2radf( beta ) ) );
Jonas Stienen's avatar
Jonas Stienen committed
301 302 303
}


304
void convertYPR2VU( const float yaw, const float pitch, const float roll, float& vx, float& vy, float& vz, float& ux, float& uy, float& uz )
Jonas Stienen's avatar
Jonas Stienen committed
305
{
306 307 308 309 310 311 312 313 314 315
	double vx_, vy_, vz_, ux_, uy_, uz_;
	convertYPR2VU( double( yaw ), double( pitch ), double( roll ), vx_, vy_, vz_, ux_, uy_, uz_ );
	vx = float( vx_ );
	vy = float( vy_ );
	vz = float( vz_ );
	ux = float( ux_ );
	uy = float( uy_ );
	uz = float( uz_ );
}

316
void convertYPR2VU( const double yaw, const double pitch, const double roll, double& vx, double& vy, double& vz, double& ux, double& uy, double& uz )
Jonas Stienen's avatar
Jonas Stienen committed
317 318 319
{
	/*
	 *  Yaw-pitch-roll (YPR) angles rotation order (referring to OpenGL axis)
320
	 *
Jonas Stienen's avatar
Jonas Stienen committed
321 322 323 324 325
	 *  R := Ry(yaw) * Rx(pitch) * Rz(-roll);
	 *
	 *  Note: Roll accounts to the view axis (-Z), not the Z axis (+Z). Hence inversion.
	 */

326 327 328
	double sy = sin( yaw ), cy = cos( yaw );
	double sp = sin( pitch ), cp = cos( pitch );
	double sr = sin( roll ), cr = cos( roll );
Jonas Stienen's avatar
Jonas Stienen committed
329 330 331 332 333 334 335 336 337

	vx = -sy*cp;
	vy = sp;
	vz = -cy*cp;

	ux = cy*sr + sy*sp*cr;
	uy = cp*cr;
	uz = -sy*sr + cy*sp*cr;
}
338
void convertVU2YPR( const float vx, const float vy, const float vz, const float ux, const float uy, const float uz, float& yaw, float& pitch, float& roll )
Jonas Stienen's avatar
Jonas Stienen committed
339
{
340 341 342 343 344
	double y, p, r;
	convertVU2YPR( double( vx ), double( vy ), double( vz ), double( ux ), double( uy ), double( uz ), y, p, r );
	yaw = float( y );
	pitch = float( p );
	roll = float( r );
Jonas Stienen's avatar
Jonas Stienen committed
345 346
}

347
void convertVU2YPR( const double vx, const double vy, const double vz, const double ux, const double uy, const double uz, double& yaw, double& pitch, double& roll )
Jonas Stienen's avatar
Jonas Stienen committed
348 349
{
	const float EPSILON = 0.0001F;
350

Jonas Stienen's avatar
Jonas Stienen committed
351 352 353 354 355 356 357 358
	// DEBUG: Test on normalization and orthogonality
	/*
	double Lv = sqrt( vx*vx + vy*vy + vz*vz );
	double Lu = sqrt( ux*ux + uy*uy + uz*uz );
	double Svu = vx*ux + vy*uy + vz*uz;
	*/

	// Problem: View points into north pole => Gimbal lock between yaw and roll
359 360
	if( vy >= ( 1 - EPSILON ) )
	{
Jonas Stienen's avatar
Jonas Stienen committed
361 362
		/*
		 *  Solution: Forget about roll. Everything is yaw.
363
		 *
Jonas Stienen's avatar
Jonas Stienen committed
364 365 366 367
		 *  Ry(y).Rx(Pi/2).Rz(0).u0 = (sin(y) 0 cos(y))
		 *
		 *  yaw = atan2(ux, uz)
		 *
368
		 */
Jonas Stienen's avatar
Jonas Stienen committed
369

370
		yaw = atan2( ux, uz );
Jonas Stienen's avatar
Jonas Stienen committed
371 372 373 374 375 376 377
		pitch = HALF_PI_F;
		roll = 0;

		return;
	}

	// Problem: View points into south pole => Gimbal lock between yaw and roll
378 379
	if( vy <= -( 1 - EPSILON ) )
	{
Jonas Stienen's avatar
Jonas Stienen committed
380 381
		/*
		 *  Solution: Forget about roll. Everything is yaw.
382
		 *
Jonas Stienen's avatar
Jonas Stienen committed
383 384 385 386
		 *  Ry(y).Rx(Pi/2).Rz(0).u0 = (sin(y) 0 cos(y))
		 *
		 *  yaw = atan2(-ux, -uz)
		 *
387
		 */
Jonas Stienen's avatar
Jonas Stienen committed
388

389
		yaw = atan2( -ux, -uz );
Jonas Stienen's avatar
Jonas Stienen committed
390 391 392 393 394 395
		pitch = -HALF_PI_F;
		roll = 0;

		return;
	}

396 397 398
	yaw = atan2( -vx, -vz );
	pitch = asin( vy );

Jonas Stienen's avatar
Jonas Stienen committed
399 400 401 402 403 404 405 406
	/*
	 *  Problem: View does not point into a pole (see above)
	 *           but up-vector lies within horizontal XZ plane.
	 *
	 *  Solution: Roll can only be +90°/-90°.
	 *            Decide by hemisphere which crossprod(v,u) falls.
	 *            Upper hemisphere (y>=0) means -90°
	 *
407
	 */
Jonas Stienen's avatar
Jonas Stienen committed
408 409 410 411

	// y-component of cross production v x u
	double zy = vz*ux - vx*uz;

412
	if( ( uy <= EPSILON ) && ( uy >= -EPSILON ) ) {
Jonas Stienen's avatar
Jonas Stienen committed
413 414
		// y-component of cross production v x u
		//double zy = vz*ux - vx*uz;
415
		roll = ( zy <= 0 ? HALF_PI_F : -HALF_PI_F );
Jonas Stienen's avatar
Jonas Stienen committed
416 417 418 419
		return;
	}

	// Hint: cos(pitch) = cos( arcsin(vy) ) = sqrt(1-vy^2)
420 421
	double cp = sqrt( 1 - vy*vy );
	roll = ( zy <= 0 ? acos( uy / cp ) : -acos( uy / cp ) );
422

423
	/* debug
Jonas Stienen's avatar
Jonas Stienen committed
424
	double v_norm = vx*vx + vy*vy + vz*vz;
425
	double u_norm = ux*ux + uy*uy + uz*uz;
426
	*/
Jonas Stienen's avatar
Jonas Stienen committed
427 428
}

429 430
void pow2space( std::vector<int>& dest, const int a, const int b )
{
431 432 433 434
	int xa = getExp2( a );
	int xb = getExp2( b );
	if( ( xa == -1 ) || ( xb == -1 ) )
		ITA_EXCEPT1( INVALID_PARAMETER, "Interval boundaries must be powers of two" );
Jonas Stienen's avatar
Jonas Stienen committed
435 436

	dest.clear();
437 438
	if( xa > xb )
	{
439 440 441
		dest.reserve( xa - xb + 1 );
		for( int x = xa; x >= xb; --x ) dest.push_back( 1 << x );
	}
442 443
	else
	{
444 445
		dest.reserve( xb - xa + 1 );
		for( int x = xa; x <= xb; ++x ) dest.push_back( 1 << x );
Jonas Stienen's avatar
Jonas Stienen committed
446 447 448
	}
}

449
double theta2elevation( const double dThetaRAD )
450
{
451 452
	if( dThetaRAD<0 || dThetaRAD>ITAConstants::PI_D )
		ITA_EXCEPT1( INVALID_PARAMETER, "thetaRAD is only valid between 0 and PI" );
453

454
	return ITAConstants::PI_D / 2.0f - dThetaRAD;
455 456
}

457
double elevation2theta( const double dElevationRAD )
458
{
459 460
	if( abs( dElevationRAD ) < ITAConstants::PI_D / 2.0f )
		ITA_EXCEPT1( INVALID_PARAMETER, "elevationRAD is only valid between -PI/2 and PI/2" );
461

462
	return ITAConstants::PI_D / 2.0f - dElevationRAD;
463 464
}

465
int factorial( const  int m )
Jonas Stienen's avatar
Jonas Stienen committed
466
{
467
	if( m < 0 )
468 469
		ITA_EXCEPT1( INVALID_PARAMETER, "Factorial can only be calculated for a positive integer" );
	return ( m <= 1 ? 1 : m * factorial( m - 1 ) );
Jonas Stienen's avatar
Jonas Stienen committed
470 471
}

472
double SHNormalizeConst( const int m, const int n )
Jonas Stienen's avatar
Jonas Stienen committed
473
{
474
	if( m > n )
475 476
		ITA_EXCEPT1( INVALID_PARAMETER, "Abs(degree) cannot be greater than order" );

477
	double dRes = 0;
478
	if( m % 2 == 1 )
479
		dRes = -1;
Jonas Stienen's avatar
Jonas Stienen committed
480
	else
481
		dRes = 1;
Jonas Stienen's avatar
Jonas Stienen committed
482

483
	dRes *= sqrt( ( 2.0f * n + 1 ) * ( 2 - SHKronecker( m ) ) * double( factorial( n - m ) ) / ( 4.0f * PI_F * double( factorial( n + m ) ) ) );
484
	return dRes;
Jonas Stienen's avatar
Jonas Stienen committed
485 486
}

487
int SHKronecker( const int m )
Jonas Stienen's avatar
Jonas Stienen committed
488
{
489
	if( m == 0 )
Jonas Stienen's avatar
Jonas Stienen committed
490 491 492 493 494
		return 1;
	else
		return 0;
}

495
std::vector< double > SHRealvaluedBasefunctions( const double thetaRAD, const  double azimuthRAD, const int maxOrder )
Jonas Stienen's avatar
Jonas Stienen committed
496
{
497
	std::vector< double > Y;
498
	//std::vector<double> dNormalizing;
499 500
	int nmax = ( maxOrder + 1 )*( maxOrder + 1 );
	Y.resize( nmax );
501 502
	//dNormalizing.resize(nmax);
	//dNormalizing = SHNormalizeConst();
Jonas Stienen's avatar
Jonas Stienen committed
503

504
	Y = SHAssociatedLegendre( maxOrder, cos( thetaRAD ) );
505

506
	for( int n = 0; n <= maxOrder; n++ )
Jonas Stienen's avatar
Jonas Stienen committed
507
	{
508 509
		Y[ SHDegreeOrder2Linear( 0, n ) ] *= SHNormalizeConst( 0, n );
		for( int m = 1; m <= n; m++ )
Jonas Stienen's avatar
Jonas Stienen committed
510
		{
511 512 513
			double Normalizing = SHNormalizeConst( m, n );
			Y[ SHDegreeOrder2Linear( m, n ) ] *= cos( m*azimuthRAD )*Normalizing;
			Y[ SHDegreeOrder2Linear( -m, n ) ] *= sin( m*azimuthRAD )*Normalizing;
Jonas Stienen's avatar
Jonas Stienen committed
514 515 516 517 518
		}
	}
	return Y;
}

519
std::vector<double> SHAssociatedLegendre( const int N, const  double mu )
Jonas Stienen's avatar
Jonas Stienen committed
520
{
521
	std::vector< double > P;
522 523 524
	P.resize( ( N + 1 )*( N + 1 ) );
	P[ 0 ] = 1.0;
	for( int n = 1; n <= N; n++ )
Jonas Stienen's avatar
Jonas Stienen committed
525
	{
526 527 528 529 530
		P[ SHDegreeOrder2Linear( n, n ) ] = ( -( 2 * n - 1 )*P[ SHDegreeOrder2Linear( ( n - 1 ), ( n - 1 ) ) ] * sqrt( 1 - ( mu*mu ) ) );
		P[ SHDegreeOrder2Linear( n - 1, n ) ] = ( ( 2 * n ) - 1 )*mu*P[ SHDegreeOrder2Linear( n - 1, n - 1 ) ]; //m-ter Grad
		if( n > 1 )
		{
			for( int m = 0; m <= ( n - 2 ); m++ )
Jonas Stienen's avatar
Jonas Stienen committed
531
			{
532
				P[ SHDegreeOrder2Linear( m, n ) ] = ( ( 2 * n - 1 )*mu*P[ SHDegreeOrder2Linear( m, n - 1 ) ] - ( n + m - 1 )*P[ SHDegreeOrder2Linear( m, n - 2 ) ] ) / ( n - m );
Jonas Stienen's avatar
Jonas Stienen committed
533
			}
534 535
		}
		for( int m = 1; m <= n; m++ )
Jonas Stienen's avatar
Jonas Stienen committed
536
		{
537
			P[ SHDegreeOrder2Linear( -m, n ) ] = P[ SHDegreeOrder2Linear( m, n ) ];
Jonas Stienen's avatar
Jonas Stienen committed
538 539 540 541 542
		}
	}
	return P;
}

543
int SHDegreeOrder2Linear( const int m, const int n )
Jonas Stienen's avatar
Jonas Stienen committed
544
{
545 546
	if( abs( m ) > n )
		ITA_EXCEPT1( INVALID_PARAMETER, "Abs(degree) cannot be greater than order" );
Jonas Stienen's avatar
Jonas Stienen committed
547

548
	return ( n*n + n + m );
Jonas Stienen's avatar
Jonas Stienen committed
549
}
550

551 552 553 554 555 556 557 558 559
std::vector< double > HOARemaxWeights( int iTruncationOrder )
{
	if( iTruncationOrder > 20 )
		ITA_EXCEPT1( INVALID_PARAMETER, "ReMAx decoding only implemented up to Truncatrion Order 20" );

	std::vector< double > vdRemax;
	double dMaxRootTable[ 21 ] = { 0.993752, 0.993129, 0.992407, 0.991565, 0.990575, 0.989401, 0.987993, 0.986284, 0.984183, 0.978229, 0.978229, 0.973907, 0.96816, 0.96029, 0.949108, 0.93247, 0.90618, 0.861136, 0.774597, 0.57735, 0 };
	double dMaxRoot = dMaxRootTable[ 21 - iTruncationOrder - 1 ];
	std::vector< double > vdAssoLegendre = ( SHAssociatedLegendre( iTruncationOrder, dMaxRoot ) );
560

561
	for( int k = 0; k <= iTruncationOrder; k++ )
562
	{
563
		vdRemax.push_back( vdAssoLegendre[ SHDegreeOrder2Linear( 0, k ) ] );
564 565 566 567 568
	}

	return vdRemax;
}

569
void SHLinear2DegreeOrder( const int iLinear, int &m, int &n )
570
{
571 572
	if( iLinear < 0 )
		ITA_EXCEPT1( INVALID_PARAMETER, "The linear index has to be higher than one" );
573 574

	std::vector<int> nm;
575 576 577
	n = ( int ) floor( sqrt( ( double ) iLinear ) );
	m = ( iLinear - n * n - n );
}