void Ambix_directional_loudnessAudioProcessor::calcParams()
{
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
// std::cout << "Coordinate size: " << Carth_coord.rows() << " x " << Carth_coord.cols() << std::endl;
// std::cout << Carth_coord << std::endl;
Sph_coord.resize(numsamples,2); // positions in spherical coordinates
Sh_matrix.setZero(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);
// std::cout << "Size: " << Ymn.size() << ": " << Ymn << std::endl;
//Sh_matrix.row(i) = Ymn;
Sh_matrix.block(i,0,1,AMBI_CHANNELS) = Ymn.transpose();
// std::cout << "Size: " << Sh_matrix.block(i,0,1,in_ambi_channels).size() << ": " << Sh_matrix.block(i,0,1,in_ambi_channels) << std::endl;
}
// Sh_matrix_inv.setZero();
//Sh_matrix_inv = (Sh_matrix.transpose()*Sh_matrix).inverse()*Sh_matrix.transpose(); // not working for dynamic input order
// if input order is different a better solving has to be used for the inverse:
Sh_matrix_inv = (Sh_matrix.transpose()*Sh_matrix).colPivHouseholderQr().inverse()*Sh_matrix.transpose();
// std::cout << "Size: " << Sh_matrix_inv.rows() << " x " << Sh_matrix_inv.cols() << std::endl;
// std::cout << Sh_matrix_inv << std::endl;
_initialized = true;
}
if (_param_changed)
{
// convert parameters to values for the filter
// ArrayIntParam _shape = shape;
ArrayParam _width = width * (float)M_PI; // 0...pi
ArrayParam _height = height * (float)M_PI;
ArrayParam _gain;
for (int i=0; i < gain.rows();i++)
{
_gain(i) = ParamToRMS(gain(i));
}
SphCoordParam _center_sph = (center_sph - 0.5f)*2.f*(float)M_PI;
// std::cout << _center_sph << std::endl;
CarthCoordParam _center_carth;
// convert center spherical coordinates to carthesian
for (int i=0; i < _center_sph.rows(); i++)
{
_center_carth(i,0) = cos(_center_sph(i,0))*cos(_center_sph(i,1)); // x
_center_carth(i,1) = sin(_center_sph(i,0))*cos(_center_sph(i,1)); // y
_center_carth(i,2) = sin(_center_sph(i,1)); // z
}
// scale the SH_matrix and save as Sh_matrix_mod
Sh_matrix_mod = Sh_matrix;
// iterate over all sample points
for (int i=0; i < Sh_matrix_mod.rows(); i++)
{
double multipl = 1.f;
// iterate over all filters
for (int k=0; k < NUM_FILTERS; k++)
{
Eigen::Vector2d Sph_coord_vec = Sph_coord.row(i);
Eigen::Vector2d _center_sph_vec = _center_sph.row(k);
multipl *= (double)sph_filter.GetWeight(&Sph_coord_vec, Carth_coord.row(i), &_center_sph_vec, _center_carth.row(k), (int)shape(k), _width(k), _height(k), _gain(k), (window(k) > 0.5f), transition(k));
}
Sh_matrix_mod.row(i) *= multipl;
}
// calculate new transformation matrix
Sh_transf = Sh_matrix_inv * Sh_matrix_mod;
// threshold coefficients
for (int i = 0; i < Sh_transf.size(); i++)
{
if (abs(Sh_transf(i)) < 0.00001f)
Sh_transf(i) = 0.f;
}
_param_changed = false;
}
}
void Ambix_vmicAudioProcessor::calcParams()
{
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
// std::cout << "Coordinate size: " << Carth_coord.rows() << " x " << Carth_coord.cols() << std::endl;
// std::cout << Carth_coord << std::endl;
Sph_coord.resize(numsamples,2); // positions in spherical coordinates
Sh_matrix.setZero(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.block(i,0,1,AMBI_CHANNELS) = Ymn.transpose();
}
_initialized = true;
}
if (_param_changed)
{
// convert parameters to values for the filter
// ArrayIntParam _shape = shape;
ArrayParam _width = width * (float)M_PI; // 0...pi
ArrayParam _height = height * (float)M_PI;
ArrayParam _gain;
for (int i=0; i < gain.rows();i++)
{
_gain(i) = ParamToRMS(gain(i));
}
SphCoordParam _center_sph = (center_sph - 0.5f)*2.f*M_PI;
// std::cout << _center_sph << std::endl;
CarthCoordParam _center_carth;
// convert center spherical coordinates to carthesian
for (int i=0; i < _center_sph.rows(); i++)
{
_center_carth(i,0) = cos(_center_sph(i,0))*cos(_center_sph(i,1)); // x
_center_carth(i,1) = sin(_center_sph(i,0))*cos(_center_sph(i,1)); // y
_center_carth(i,2) = sin(_center_sph(i,1)); // z
}
Sh_matrix_mod.setZero();
// iterate over all filters
for (int k=0; k < NUM_FILTERS_VMIC; k++)
{
// copy the SH_matrix to
Eigen::MatrixXd Sh_matrix_temp = Sh_matrix;
// iterate over all sample points
for (int i=0; i < Sh_matrix_temp.rows(); i++)
{
Eigen::Vector2d Sph_coord_vec = Sph_coord.row(i);
Eigen::Vector2d _center_sph_vec = _center_sph.row(k);
double multipl = sph_filter.GetWeight(&Sph_coord_vec, Carth_coord.row(i), &_center_sph_vec, _center_carth.row(k), (int)floor(shape(k)+0.5f), _width(k), _height(k), 1, true, transition(k));
if (multipl < 0.f) // -1.f in case of outside region
multipl = 0.f;
Sh_matrix_temp.row(i) *= multipl;
}
// Sh_matrix_mod row is the sum over the columns
Sh_matrix_mod.row(k) = Sh_matrix_temp.colwise().sum();
// normalize and apply gain
if (Sh_matrix_mod.row(k)(0) > 0.f)
Sh_matrix_mod.row(k) *= 1/Sh_matrix_mod.row(k)(0)*_gain(k);
}
// std::cout << "Size Sh_matrix_mod : " << Sh_matrix_mod.rows() << " x " << Sh_matrix_mod.cols() << std::endl;
// std::cout << Sh_matrix_mod << std::endl;
// this is the new transformation matrix
Sh_transf = Sh_matrix_mod;
// threshold coefficients
for (int i = 0; i < Sh_transf.size(); i++)
{
if (abs(Sh_transf(i)) < 0.00001f)
Sh_transf(i) = 0.f;
}
_param_changed = false;
}
}
void Ambix_warpAudioProcessor::calcParams()
{
if (!_initialized)
{
sph_h_in.Init(in_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
// std::cout << "Coordinate size: " << Carth_coord.rows() << " x " << Carth_coord.cols() << std::endl;
// std::cout << Carth_coord << std::endl;
Sph_coord.resize(numsamples,2); // positions in spherical coordinates
Eigen::MatrixXd Sh_matrix;
Sh_matrix.setZero(numsamples,AMBI_CHANNELS);
int in_ambi_channels = (in_order+1)*(in_order+1);
for (int i=0; i < numsamples; i++)
{
Eigen::VectorXd Ymn(in_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_in.Calc(Sph_coord(i,0),Sph_coord(i,1)); // phi theta
sph_h_in.Get(Ymn);
// std::cout << "Ymn Size: " << Ymn.size() << std::endl;
// std::cout << "Sh_matrix Size: " << Sh_matrix.size() << std::endl;
//Sh_matrix.row(i) = Ymn;
Sh_matrix.block(i,0,1,in_ambi_channels) = Ymn.transpose();
// std::cout << "Size: " << Sh_matrix.block(i,0,1,in_ambi_channels).size() << ": " << Sh_matrix.block(i,0,1,in_ambi_channels) << std::endl;
}
// Sh_matrix_inv.setZero();
//Sh_matrix_inv = (Sh_matrix.transpose()*Sh_matrix).inverse()*Sh_matrix.transpose(); // not working for dynamic input order
// if input order is different a better solving has to be used for the inverse:
Sh_matrix_inv = (Sh_matrix.transpose()*Sh_matrix).colPivHouseholderQr().inverse()*Sh_matrix.transpose();
// std::cout << "Size Sh_matrix: " << Sh_matrix.rows() << " x " << Sh_matrix.cols() << std::endl;
// std::cout << "Size Inverse: " << Sh_matrix_inv.rows() << " x " << Sh_matrix_inv.cols() << std::endl;
// std::cout << Sh_matrix_inv << std::endl;
_initialized = true;
}
if ( (phi_param != _phi_param) ||
(floor(phi_curve_param+0.5f) != floor(_phi_curve_param+0.5f)) ||
(theta_param != _theta_param) ||
(floor(theta_curve_param+0.5f) != floor(_theta_curve_param+0.5f)) ||
(in_order != _in_order) ||
(out_order != _out_order) ||
(floor(preemp_param+0.5f) != floor(_preemp_param+0.5f))
)
{
if (_out_order != out_order)
{
// init or reinit spherical harmonics
sph_h_out.Init(out_order);
}
Eigen::MatrixXd Sh_matrix_mod;
Sh_matrix_mod.setZero(Sph_coord.rows(),AMBI_CHANNELS);
Eigen::VectorXd pre_emphasis;
pre_emphasis.setOnes(Sph_coord.rows());
Eigen::MatrixXd Sph_coord_mod = Sph_coord;
// warping parameters - use inverse parameters! (see paper)
double phi_alpha = -((double)phi_param*1.8f - 0.9f);
double theta_alpha = -((double)theta_param*1.8f - 0.9f);
int out_ambi_channels = (out_order+1)*(out_order+1);
for (int i=0; i < Sph_coord_mod.rows(); i++)
{
if (theta_alpha != 0.)
{
// warp elevation
// as we are using elevation from -pi/2....pi/2
double mu = sin(Sph_coord(i,1));
if (theta_curve_param <= 0.5f)
{
// warp towards a pole
Sph_coord_mod(i,1) = asin((mu+theta_alpha)/(1+theta_alpha*mu));
// pre emphasis
pre_emphasis(i) = (1+theta_alpha*mu)/sqrt(1-theta_alpha*theta_alpha);
} else {
//warp towards equator (< 0.)or away from equator (> 0.)
// in the paper this parameter is called beta
// pre emphasis
pre_emphasis(i) = sqrt((1-abs(theta_alpha))*(1+abs(theta_alpha)*mu*mu))/(1-abs(theta_alpha)*mu*mu);
if (theta_alpha > 0.) {
Sph_coord_mod(i,1) = asin((theta_alpha-1+sqrt(pow(theta_alpha-1,2)+4*theta_alpha*pow(mu,2)))/(2*theta_alpha*mu));
} else {
Sph_coord_mod(i,1) = asin(((1+theta_alpha)*mu)/(1+theta_alpha*pow(mu,2)));
pre_emphasis(i) = 1/pre_emphasis(i); // this is ^sgn(beta)
}
}
}
/*
// this is not fully working yet
if (phi_alpha != 0.)
{
// warp azimuth
// as we are using azimuth from -pi....pi
double phi_temp = Sph_coord(i,0) * 0.5f;
double mu = sin(phi_temp);
if (phi_curve_param <= 0.5f)
{
// warp towards a pole
Sph_coord_mod(i,0) = asin((mu+phi_alpha)/(1+phi_alpha*mu));
// pre emphasis
pre_emphasis(i) *= (1+phi_alpha*mu)/sqrt(1-phi_alpha*phi_alpha);
} else {
//warp towards equator (< 0.)or away from equator (> 0.)
// in the paper this parameter is called beta
// pre emphasis
double pre_emph_temp = sqrt((1-abs(phi_alpha))*(1+abs(phi_alpha)*mu*mu))/(1-abs(phi_alpha)*mu*mu);
if (phi_alpha > 0.) {
Sph_coord_mod(i,0) = asin((phi_alpha-1+sqrt(pow(phi_alpha-1,2)+4*phi_alpha*pow(mu,2)))/(2*phi_alpha*mu));
pre_emphasis(i) *= pre_emph_temp;
} else {
Sph_coord_mod(i,0) = asin(((1+phi_alpha)*mu)/(1+phi_alpha*pow(mu,2)));
pre_emphasis(i) *= 1/pre_emph_temp; // this is ^sgn(beta)
}
}
Sph_coord_mod(i,0) *= 2;
}
*/
if (phi_alpha != 0.f)
{
// warp azimuth
// as we are using azimuth from -pi....pi
double phi_temp = Sph_coord(i,0) * 0.5f;
if (phi_curve_param <= 0.5f)
{
// warp towards poles
Sph_coord_mod(i,0) = asin((sin(phi_temp)+phi_alpha)/(1.f+phi_alpha*sin(phi_temp)));
} else {
//warp towards equator
if (phi_temp > 0)
{
Sph_coord_mod(i,0) = acos((cos(phi_temp*2)+phi_alpha)/(1+phi_alpha*cos(phi_temp*2))) / 2;
} else {
Sph_coord_mod(i,0) = -acos((cos(M_PI-(phi_temp-M_PI/2)*2)+phi_alpha)/(1+phi_alpha*cos(M_PI-(phi_temp-M_PI/2)*2))) / 2;
}
}
Sph_coord_mod(i,0) *= 2;
}
// rotate
// Sph_mod(i,0) = Sph(i,0);
Eigen::VectorXd Ymn(out_ambi_channels); // Ymn result
sph_h_out.Calc(Sph_coord_mod(i,0),Sph_coord_mod(i,1)); // phi theta
sph_h_out.Get(Ymn);
// Sh_matrix_mod.row(i) = Ymn;
Sh_matrix_mod.block(i,0,1,out_ambi_channels) = Ymn.transpose();
}
// apply preemphasis if wanted
if (preemp_param > 0.5f)
{
Sh_matrix_mod = Sh_matrix_mod * pre_emphasis.asDiagonal();
}
// calculate new transformation matrix
Sh_transf = Sh_matrix_inv * Sh_matrix_mod;
// threshold coefficients
for (int i = 0; i < Sh_transf.size(); i++)
{
if (abs(Sh_transf(i)) < 0.00001f)
Sh_transf(i) = 0.f;
}
_phi_param = phi_param;
_phi_curve_param = phi_curve_param;
_theta_param = theta_param;
_theta_curve_param = theta_curve_param;
_in_order = in_order;
_out_order = out_order;
_preemp_param = preemp_param;
}
}
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;
}
}