TakeV/dxx-rebirth
1
0
Fork 0
Code Issues Pull requests Packages Projects Releases Wiki Activity

Factor out vms_quaternion_from_matrix division

Browse source
This commit is contained in:
Kp 2019-02-02 18:36:39 +00:00
parent 92fdacfb60
commit 47a6f744d8
1 changed files with 22 additions and 17 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

39
common/maths/vecmat.cpp
View file

@ -612,7 +612,7 @@ fix vm_dist_to_plane(const vms_vector &checkp,const vms_vector &norm,const vms_v
}
// convert vms_matrix to vms_quaternion
void vms_quaternion_from_matrix(vms_quaternion &q, const vms_matrix &m)
void vms_quaternion_from_matrix(vms_quaternion &rq, const vms_matrix &m)
{
const auto rx = m.rvec.x;
const auto ry = m.rvec.y;
@ -624,31 +624,36 @@ void vms_quaternion_from_matrix(vms_quaternion &q, const vms_matrix &m)
const auto fy = m.fvec.y;
const auto fz = m.fvec.z;
const fix tr = rx + uy + fz;
vms_quaternion q;
if (tr > 0) {
fix s = fixmul(fix_sqrt(tr + fl2f(1.0)), fl2f(2.0));
q.w = fixmul(fl2f(0.25), s) * .5;
q.x = fixdiv(fy - uz, s) * .5;
q.y = fixdiv(rz - fx, s) * .5;
q.z = fixdiv(ux - ry, s) * .5;
q.w = fixmul(fl2f(0.25), s);
q.x = fixdiv(fy - uz, s);
q.y = fixdiv(rz - fx, s);
q.z = fixdiv(ux - ry, s);
} else if ((rx > uy) & (rx > fz)) {
fix s = fixmul(fix_sqrt(fl2f(1.0) + rx - uy - fz), fl2f(2.0));
q.w = fixdiv(fy - uz, s) * .5;
q.x = fixmul(fl2f(0.25), s) * .5;
q.y = fixdiv(ry + ux, s) * .5;
q.z = fixdiv(rz + fx, s) * .5;
q.w = fixdiv(fy - uz, s);
q.x = fixmul(fl2f(0.25), s);
q.y = fixdiv(ry + ux, s);
q.z = fixdiv(rz + fx, s);
} else if (uy > fz) {
fix s = fixmul(fix_sqrt(fl2f(1.0) + uy - rx - fz), fl2f(2.0));
q.w = fixdiv(rz - fx, s) * .5;
q.x = fixdiv(ry + ux, s) * .5;
q.y = fixmul(fl2f(0.25), s) * .5;
q.z = fixdiv(uz + fy, s) * .5;
q.w = fixdiv(rz - fx, s);
q.x = fixdiv(ry + ux, s);
q.y = fixmul(fl2f(0.25), s);
q.z = fixdiv(uz + fy, s);
} else {
fix s = fixmul(fix_sqrt(fl2f(1.0) + fz - rx - uy), fl2f(2.0));
q.w = fixdiv(ux - ry, s) * .5;
q.x = fixdiv(rz + fx, s) * .5;
q.y = fixdiv(uz + fy, s) * .5;
q.z = fixmul(fl2f(0.25), s) * .5;
q.w = fixdiv(ux - ry, s);
q.x = fixdiv(rz + fx, s);
q.y = fixdiv(uz + fy, s);
q.z = fixmul(fl2f(0.25), s);
}
rq.w = q.w / 2;
rq.x = q.x / 2;
rq.y = q.y / 2;
rq.z = q.z / 2;
}
// convert vms_quaternion to vms_matrix