Langevin Preconditioning Matrix

MatrixXd precond_3dim_new(const VectorXd &p, const VectorXd &params) {
    int n = p.size();

    double r1 = params(0);
    double r2 = params(1);
    double r3 = params(2);
    double k  = params(3);

    double x = p(0), y = p(1), z = p(2);
    double phi = atan2(y, x);
    double theta = phi * k;


    // ==== e1, e2, e3 ====
    VectorXd e1(3);
    e1(0) = -x * sin(phi) / cos(phi) + k * z * cos(phi);
    e1(1) = x * cos(phi) /  cos(phi) + k * z * sin(phi);
    e1(2) = -k * x / cos(phi) + k * r1;
    e1.normalize();

    VectorXd e2(3);
    e2(0) = cos(theta) * cos(phi);
    e2(1) = cos(theta) * sin(phi);
    e2(2) = -sin(theta);

    VectorXd e3(3);
    e3(0) = sin(theta) * cos(phi);
    e3(1) = sin(theta) * sin(phi);
    e3(2) = cos(theta);


    // ==== e1, e2, e3 coefs ====
    double de1 = sqrt(
        2 * r3 * (r1 * r1 + k*k * r2*r2) / sqrt(r1*r1 + k*k*k*k * r2*r2)
    ); // опасно r2 <-> r3
    double de2 = r2;
    double de3 = r3;


    // ==== create preconditioning matrix ====
    MatrixXd res = MatrixXd::Zero(3, 3);
    res.col(0) = e1 * de1;
    res.col(1) = e2 * de2;
    res.col(2) = e3 * de3;

    return res;
}