vec3 reorient_vector(const vec3 v, const vec3 up) {
const vec3 new_y = normalized(up);
const vec3 new_x = normalized(find_orthogonal_vector(new_y));
const vec3 new_z = normalized(cross(new_x, new_y));
const mat3 base2 = { new_x, new_y, new_z };
const mat3 base = transpose(base2);
return base * v;
}
LocalCS_Data::quaternion::quaternion(const float rot[9], const quaternion *pq)
{
if (rot[0] == 1.0 && rot[4] == 1.0 && rot[8] == 1.0)
{
_a = 1.0;
_b = _c = _d = 0.0;
}
else
{
float cos_theta = (rot[0] + rot[4] + rot[8] - 1.0) * 0.5;
float stuff = (cos_theta + 1.0) * 0.5;
float cos_theta_sur_2 = sqrt(stuff);
float sin_theta_sur_2 = sqrt(1 - stuff);
float x;
float y;
float z;
find_invariant_vector(rot, x, y, z);
float u;
float v;
float w;
find_orthogonal_vector(x, y, z, u, v, w);
float r;
float s;
float t;
find_vector_for_BOD(x, y, z, u, v, w, r, s, t);
float ru = rot[0] * u + rot[1] * v + rot[2] * w;
float rv = rot[3] * u + rot[4] * v + rot[5] * w;
float rw = rot[6] * u + rot[7] * v + rot[8] * w;
float angle_sign_determinator = r * ru + s * rv + t * rw;
if (angle_sign_determinator > 0.0)
{
_a = cos_theta_sur_2;
_b = x * sin_theta_sur_2;
_c = y * sin_theta_sur_2;
_d = z * sin_theta_sur_2;
}
else if (angle_sign_determinator < 0.0)
{
_a = cos_theta_sur_2;
_b = -x * sin_theta_sur_2;
_c = -y * sin_theta_sur_2;
_d = -z * sin_theta_sur_2;
}
else if (u * ru + v * rv + w * rw < 0.0)
{
_a = 0.0;
_b = x;
_c = y;
_d = z;
}
else
{
_a = 1.0;
_b = _c = _d = 0.0;
}
}
if (pq && abs2(*pq, *this) > abs2(*this, *pq))
{
_a *= -1.0;
_b *= -1.0;
_c *= -1.0;
_d *= -1.0;
}
}