//=============================================================================
bool bicg
(
const CPPL::dgematrix& A,
//const CPPL::dgsmatrix& A,
CPPL::dcovector& x,
const double& eps
)
{
double alpha, beta(0.0);
CPPL::dcovector p_0(x.l), p_1, P_0(x.l), P_1, q, Q;
CPPL::dcovector r_1(x), r_2, R_1(r_1), R_2;
double rho_0(r_1%R_1), rho_1;
x.zero();
p_0.zero();
P_0.zero();
int itc(0);
const int itmax(2*x.l);
while(fabs(damax(r_1))>eps && ++itc<itmax){
std::cout << itc << " " << fabs(damax(r_1)) << std::endl;
rho_1 =r_1%R_1;
beta =rho_1/rho_0;
p_1 =r_1 +beta*p_0;
P_1 =R_1 +beta*P_0;
q =A*p_1;
Q =t(t(P_1)*A);
alpha =rho_1/(P_1%q);
x +=alpha*p_1;
r_2 =r_1 -alpha*q;
R_2 =R_1 -alpha*Q;
rho_0 =rho_1;
swap(p_0, p_1);
swap(P_0, P_1);
swap(r_1, r_2);
swap(R_1, R_2);
}
std::cerr << "itc=" << itc << " fabs(damax(r_1))=" << fabs(damax(r_1)) << std::endl;
if(itc<itmax){ return 0; }
else{ return 1; }
}
void Ambix_rotatorAudioProcessor::calcParams()
{
// use old sampling method for generating rotation matrix...
#if 0
if (!_initialized)
{
sph_h.Init(AMBI_ORDER);
const String t_design_txt (t_design::des_3_240_21_txt);
// std::cout << t_design_txt << std::endl;
String::CharPointerType lineChar = t_design_txt.getCharPointer();
int n = 0; // how many characters been read
int numsamples = 0;
int i = 0;
int curr_n = 0;
int max_n = lineChar.length();
while (curr_n < max_n) { // check how many coordinates we have
double value;
sscanf(lineChar, "%lf\n%n", &value, &n);
lineChar += n;
curr_n += n;
numsamples++;
} // end parse numbers
numsamples = numsamples/3; // xyz
Carth_coord.resize(numsamples,3); // positions in cartesian coordinates
curr_n = 0;
lineChar = t_design_txt.getCharPointer();
// parse line for numbers again and copy to carth coordinate matrix
while (i < numsamples) {
double x,y,z;
sscanf(lineChar, "%lf%lf%lf%n", &x, &y, &z, &n);
Carth_coord(i,0) = x;
Carth_coord(i,1) = y;
Carth_coord(i,2) = z;
lineChar += n;
curr_n += n;
i++;
} // end parse numbers
Sph_coord.resize(numsamples,2); // positions in spherical coordinates
Eigen::MatrixXd Sh_matrix(numsamples,AMBI_CHANNELS);
for (int i=0; i < numsamples; i++)
{
Eigen::VectorXd Ymn(AMBI_CHANNELS); // Ymn result
Sph_coord(i,0) = atan2(Carth_coord(i,1),Carth_coord(i,0)); // azimuth
Sph_coord(i,1) = atan2(Carth_coord(i,2),sqrt(Carth_coord(i,0)*Carth_coord(i,0) + Carth_coord(i,1)*Carth_coord(i,1))); // elevation
sph_h.Calc(Sph_coord(i,0),Sph_coord(i,1)); // phi theta
sph_h.Get(Ymn);
Sh_matrix.row(i) = Ymn;
}
// inversion would not be necessary because of t-design -> transpose is enough..
Sh_matrix_inv = (Sh_matrix.transpose()*Sh_matrix).inverse()*Sh_matrix.transpose();
_initialized = true;
}
Eigen::MatrixXd Sh_matrix_mod(Sph_coord.rows(),AMBI_CHANNELS);
// rotation parameters in radiants
// use mathematical negative angles for yaw
double yaw = -((double)yaw_param*2*M_PI - M_PI); // z
double pitch = (double)pitch_param*2*M_PI - M_PI; // y
double roll = (double)roll_param*2*M_PI - M_PI; // x
Eigen::Matrix3d RotX, RotY, RotZ, Rot;
RotX = RotY = RotZ = Eigen::Matrix3d::Zero(3,3);
RotX(0,0) = 1.f;
RotX(1,1) = RotX(2,2) = cos(roll);
RotX(1,2) = -sin(roll);
RotX(2,1) = -RotX(1,2);
RotY(0,0) = RotY(2,2) = cos(pitch);
RotY(0,2) = sin(pitch);
RotY(2,0) = -RotY(0,2);
RotY(1,1) = 1.f;
RotZ(0,0) = RotZ(1,1) = cos(yaw);
RotZ(0,1) = -sin(yaw);
RotZ(1,0) = -RotZ(0,1);
RotZ(2,2) = 1.f;
// multiply individual rotation matrices
if (rot_order_param < 0.5f)
{
// ypr order zyx -> mutliply inverse!
Rot = RotX * RotY * RotZ;
} else {
// rpy order xyz -> mutliply inverse!
Rot = RotZ * RotY * RotX;
}
// combined roll-pitch-yaw rotation matrix would be here
// http://planning.cs.uiuc.edu/node102.html
for (int i=0; i < Carth_coord.rows(); i++)
{
// rotate carthesian coordinates
Eigen::Vector3d Carth_coord_mod = Carth_coord.row(i)*Rot;
Eigen::Vector2d Sph_coord_mod;
// convert to spherical coordinates
Sph_coord_mod(0) = atan2(Carth_coord_mod(1),Carth_coord_mod(0)); // azimuth
Sph_coord_mod(1) = atan2(Carth_coord_mod(2),sqrt(Carth_coord_mod(0)*Carth_coord_mod(0) + Carth_coord_mod(1)*Carth_coord_mod(1))); // elevation
Eigen::VectorXd Ymn(AMBI_CHANNELS); // Ymn result
// calc spherical harmonic
sph_h.Calc(Sph_coord_mod(0),Sph_coord_mod(1)); // phi theta
sph_h.Get(Ymn);
// save to sh matrix
Sh_matrix_mod.row(i) = Ymn;
}
// calculate new transformation matrix
Sh_transf = Sh_matrix_inv * Sh_matrix_mod;
#else
// use
// Ivanic, J., Ruedenberg, K. (1996). Rotation Matrices for Real
// Spherical Harmonics. Direct Determination by Recursion.
// The Journal of Physical Chemistry
// rotation parameters in radiants
// use mathematical negative angles for yaw
double yaw = -((double)yaw_param*2*M_PI - M_PI); // z
double pitch = (double)pitch_param*2*M_PI - M_PI; // y
double roll = (double)roll_param*2*M_PI - M_PI; // x
Eigen::Matrix3d RotX, RotY, RotZ, Rot;
RotX = RotY = RotZ = Eigen::Matrix3d::Zero(3,3);
RotX(0,0) = 1.f;
RotX(1,1) = RotX(2,2) = cos(roll);
RotX(1,2) = sin(roll);
RotX(2,1) = -RotX(1,2);
RotY(0,0) = RotY(2,2) = cos(pitch);
RotY(0,2) = sin(pitch);
RotY(2,0) = -RotY(0,2);
RotY(1,1) = 1.f;
RotZ(0,0) = RotZ(1,1) = cos(yaw);
RotZ(0,1) = sin(yaw);
RotZ(1,0) = -RotZ(0,1);
RotZ(2,2) = 1.f;
// multiply individual rotation matrices
if (rot_order_param < 0.5f)
{
// ypr order zyx -> mutliply inverse!
Rot = RotX * RotY * RotZ;
} else {
// rpy order xyz -> mutliply inverse!
Rot = RotZ * RotY * RotX;
}
// first order initialization - prototype matrix
Eigen::Matrix3d R_1;
R_1(0,0) = Rot(1,1);
R_1(0,1) = Rot(1,2);
R_1(0,2) = Rot(1,0);
R_1(1,0) = Rot(2,1);
R_1(1,1) = Rot(2,2);
R_1(1,2) = Rot(2,0);
R_1(2,0) = Rot(0,1);
R_1(2,1) = Rot(0,2);
R_1(2,2) = Rot(0,0);
// zeroth order is invariant
Sh_transf(0,0) = 1.;
// set first order
Sh_transf.block(1, 1, 3, 3) = R_1;
Eigen::MatrixXd R_lm1 = R_1;
// recursivly generate higher orders
for (int l=2; l<=AMBI_ORDER; l++)
{
Eigen::MatrixXd R_l = Eigen::MatrixXd::Zero(2*l+1, 2*l+1);
for (int m=-l;m <= l; m++)
{
for (int n=-l;n <= l; n++)
{
// Table I
int d = (m==0) ? 1 : 0;
double denom = 0.;
if (abs(n) == l)
denom = (2*l)*(2*l-1);
else
denom = (l*l-n*n);
double u = sqrt((l*l-m*m)/denom);
double v = sqrt((1.+d)*(l+abs(m)-1.)*(l+abs(m))/denom)*(1.-2.*d)*0.5;
double w = sqrt((l-abs(m)-1.)*(l-abs(m))/denom)*(1.-d)*(-0.5);
if (u != 0.)
u *= U(l,m,n,R_1,R_lm1);
if (v != 0.)
v *= V(l,m,n,R_1,R_lm1);
if (w != 0.)
w *= W(l,m,n,R_1,R_lm1);
R_l(m+l,n+l) = u + v + w;
}
}
Sh_transf.block(l*l, l*l, 2*l+1, 2*l+1) = R_l;
R_lm1 = R_l;
}
#endif
// threshold coefficients
// maybe not needed here...
for (int i = 0; i < Sh_transf.size(); i++)
{
if (abs(Sh_transf(i)) < 0.00001f)
Sh_transf(i) = 0.f;
}
}
