mat3_t vecs2rot(const vec3_t &a_B, const vec3_t &g) {
// Create Quaternion from two vectors
const double cos_theta = a_B.normalized().transpose() * g.normalized();
const double half_cos = sqrt(0.5 * (1.0 + cos_theta));
const double half_sin = sqrt(0.5 * (1.0 - cos_theta));
const vec3_t w = a_B.cross(g).normalized();
const double qw = half_cos;
const double qx = half_sin * w(0);
const double qy = half_sin * w(1);
const double qz = half_sin * w(2);
// Convert Quaternion to rotation matrix
const double qx2 = qx * qx;
const double qy2 = qy * qy;
const double qz2 = qz * qz;
const double qw2 = qw * qw;
const double R11 = qw2 + qx2 - qy2 - qz2;
const double R12 = 2 * (qx * qy - qw * qz);
const double R13 = 2 * (qx * qz + qw * qy);
const double R21 = 2 * (qx * qy + qw * qz);
const double R22 = qw2 - qx2 + qy2 - qz2;
const double R23 = 2 * (qy * qz - qw * qx);
const double R31 = 2 * (qx * qz - qw * qy);
const double R32 = 2 * (qy * qz + qw * qx);
const double R33 = qw2 - qx2 - qy2 + qz2;
mat3_t R;
R << R11, R12, R13, R21, R22, R23, R31, R32, R33;
return R;
}
double intersection(vec3_t x_straight, vec3_t v_straight, vec3_t x_plane, vec3_t u_plane, vec3_t v_plane)
{
vec3_t n = u_plane.cross(v_plane);
return intersection(x_straight,v_straight,x_plane,n);
}