You can try to construct quaternions directly from yaw/pitch:
q = quat_from_axis_angle(up_vector, yaw) * quat_from_axis_angle(local_right, pitch)
(you may have to multiply these in the reverse order depending on how exactly you turn them into rotation matrices), or realign them every time you change them:
rotated_right = apply_rotation(q, local_right);
projected_right = rotated_right - dot(rotated_right, up_vector) * up_vector;
realign = quat_align_vector(rotated_right, normalized(projected_right));
q = realign * q
projected_right
here is a projection of rotated_right
onto the horizontal plane. Without rolling, these two vectors must be the same, which implies dot(rotated_right, up_vector) = 0
. The last equation is the actual constraint that must be satisfied. It is quadratic in q
. E.g. for local_right=(1,0,0)
, and up_vector=(0,0,1)
, it becomes dot(q*(1i+0j+0k)*conj(q), 0i+0j+1k)=2*x*z-2*w*y=0
, with q=w+xi+yi+zk
.
You can find formulas for quat_from_axis_angle
and apply_rotation
at http://en.wikipedia.org/wiki/Quaternion and http://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation. As for quat_align_vector
, one way would be
quat_align_vector(src, dst) = sqrt([dot(src, dst), cross(src, dst)])
with [a, b]
beign a quaternion with a real part a
, and an imaginary part b
. Sqrt(x)
can be calculated as exp(ln(x)/2)
(these functions are on the wiki, too). You could also try replacing sqrt with exp(ln(x)/2*tick*realign_rate)
for a smooth restoration of the up-vector :) . Or go the opposite way and simplify the formula a bit:
quat_align_vector(src, dst) = [dot(halfway, dst), cross(halfway, dst)],
halfway = normalized(normalized(src) + normalized(dst))
See also https://stackoverflow.com/a/1171995.
EDIT: corrected vectors, added the constraint.