Quaternions rotation has a weird behaviour (Haskell OpenGL) - haskell

Question

I've been following the Haskell OpenGL tutorial.
Rotations in a 3D space intrigued me so I started learning about Euler angles and finally, quaternions.
I wanted to implement my own function using quaternions to perform a rotation (on a cube), I've based myself on those two papers: mostly this one and this one.
My function works fine when I'm performing a rotation on only one axis, but when I do it on X and Y for example, the cube start to randomly go forward and being "blocked" when it rotates.
Video of the cube performing rotation on XY.
When I set the three axis (X, Y, Z), it zooms even more (but doesn't have that weird blocking thing): video.
Here is the code of my program:
Here is the main file that creates a window, set idle function and outputs result of rotation by angle A on the screen where A is increment by 0.05 at each frames.
module Main (main) where
import Core
import Utils
import Data.IORef
import Graphics.UI.GLUT
import Graphics.Rendering.OpenGL
main :: IO ()
main = do
createAWindow "177013"
mainLoop
createAWindow :: [Char] -> IO ()
createAWindow windowName = do
(procName, _args) <- getArgsAndInitialize
createWindow windowName
initialDisplayMode $= [DoubleBuffered]
angle <- newIORef 0.0
delta <- newIORef 0.05
displayCallback $= (start angle)
reshapeCallback $= Just reshape
keyboardMouseCallback $= Just keyboardMouse
idleCallback $= Just (idle angle delta)
reshape :: ReshapeCallback
reshape size = do
viewport $= (Position 0 0, size)
postRedisplay Nothing
keyboardMouse :: KeyboardMouseCallback
keyboardMouse _ _ _ _ = return ()
idle :: IORef GLfloat -> IORef GLfloat -> IdleCallback
idle angle delta = do
d <- get delta
a <- get angle
angle $~! (+d)
postRedisplay Nothing
start :: IORef GLfloat -> DisplayCallback
start angle = do
clear [ColorBuffer]
loadIdentity
a <- get angle
let c = rotate3f (0, 0, 0) [X,Y,Z] a $ cube3f 0.2 -- here I'm rotating on X, Y and Z axis
draw3f Quads c CCyan
flush
swapBuffers
where
Here is the core file where the rotation function is defined (with a few other ones). I added some comments as it's probably some low quality haskell code.
module Core (draw3f, vertex3f, rotate3f, translate3f, rotate3d, Colors(..), Axes(..)) where
import Control.Lens
import Graphics.Rendering.OpenGL
data Axes = X | Y | Z
deriving Eq
data Colors = CRed | CGreen | CBlue | CYellow | CWhite | CMagenta | CCyan | CBlack | CNone | CPreset
deriving Eq
rotate3f :: (GLfloat, GLfloat, GLfloat) -> [Axes] -> GLfloat -> [(GLfloat, GLfloat, GLfloat)] -> [(GLfloat, GLfloat, GLfloat)]
rotate3f _ _ _ [] = []
rotate3f _ [] _ _ = []
rotate3f o axes a p = let p' = translate3f p u -- translation if I don't want to rotate it by the origin
q = cos a' : ((\x -> if x `elem` axes then sin a' else 0) <$> [X,Y,Z]) -- if the axe is set then its related component is equal to sin theta/2, otherwise it will be 0
q' = q !! 0 : (negate <$> (tail q)) -- quaternion inversion
in translate3f ((rotate q q') <$> p') [(0,0,0),o] -- rotate and translate again to put the object where it belongs
where
a' = (a * (pi / 180)) / 2 -- convert to radians and divide by 2 as all q components takes theta/2
u :: [(GLfloat, GLfloat, GLfloat)]
u = [o,(0,0,0)]
rotate :: [GLfloat] -> [GLfloat] -> (GLfloat, GLfloat, GLfloat) -> (GLfloat, GLfloat, GLfloat)
rotate q q' (x,y,z) = let p = [0,x,y,z]
qmul q1 q2 = [(q1 !! 0) * (q2 !! 0) - (q1 !! 1) * (q2 !! 1) - (q1 !! 2) * (q2 !! 2) - (q1 !! 3) * (q2 !! 3),
(q1 !! 0) * (q2 !! 1) + (q1 !! 1) * (q2 !! 0) + (q1 !! 2) * (q2 !! 3) - (q1 !! 3) * (q2 !! 2),
(q1 !! 0) * (q2 !! 2) - (q1 !! 1) * (q2 !! 3) + (q1 !! 2) * (q2 !! 0) + (q1 !! 3) * (q2 !! 1),
(q1 !! 0) * (q2 !! 3) + (q1 !! 1) * (q2 !! 2) - (q1 !! 2) * (q2 !! 1) + (q1 !! 3) * (q2 !! 0)]
p' = qmul (qmul q p) q'
in (p' !! 1, p' !! 2, p' !! 3)
translate3f :: [(GLfloat, GLfloat, GLfloat)] -> [(GLfloat, GLfloat, GLfloat)] -> [(GLfloat, GLfloat, GLfloat)]
translate3f p [(ax,ay,az),(bx,by,bz)] = map (\(x,y,z) -> (x + (bx - ax), y + (by - ay), z + (bz - az))) p
draw3f :: PrimitiveMode -> [(GLfloat, GLfloat, GLfloat)] -> Colors -> IO()
draw3f shape points color = renderPrimitive shape $ mapM_ (\(x,y,z) -> vertex3f x y z color) points
vertex3f :: GLfloat -> GLfloat -> GLfloat -> Colors -> IO()
vertex3f x y z c = do
if c /= CPreset
then color $ Color3 (c' ^. _1) (c' ^. _2) ((c' ^. _3) :: GLfloat)
else return ()
vertex $ Vertex3 x y z
where
c' :: (GLfloat, GLfloat, GLfloat)
c' = case c of CRed -> (1,0,0)
CGreen -> (0,1,0)
CBlue -> (0,0,1)
CYellow -> (1,1,0)
CMagenta -> (1,0,1)
CCyan -> (0,1,1)
CBlack -> (0,0,0)
_ -> (1,1,1)
And here is the utils file where there's just the definition of the cube, from the Haskell OpenGL tutorial
module Utils (cube3f) where
import Core
import Graphics.UI.GLUT
import Graphics.Rendering.OpenGL
cube3f :: GLfloat -> [(GLfloat, GLfloat, GLfloat)]
cube3f w = [( w, w, w), ( w, w,-w), ( w,-w,-w), ( w,-w, w),
( w, w, w), ( w, w,-w), (-w, w,-w), (-w, w, w),
( w, w, w), ( w,-w, w), (-w,-w, w), (-w, w, w),
(-w, w, w), (-w, w,-w), (-w,-w,-w), (-w,-w, w),
( w,-w, w), ( w,-w,-w), (-w,-w,-w), (-w,-w, w),
( w, w,-w), ( w,-w,-w), (-w,-w,-w), (-w, w,-w)]
Finally, if it can helps people to see if there's a problem in my algorithms, here are some rotation samples using my function:
Rotation at 90°, of point (1, 2, 3) on X axis around point (0, 0, 0) (origin) gives: (0.99999994,-3.0,2.0)
Same rotation but on X & Y axis gives: (5.4999995,-0.99999994,-0.49999988)
Same rotation again but on X, Y and Z axis gives: (5.9999995,1.9999999,3.9999995)

Answer 1

The second paper about rotations by quaternions that you point to has this sentence:
“(x̂, ŷ, ẑ) is a unit vector that defines the axis of rotation.”.
So the quaternion has to be normalized, the sum of components squared being equal to 1.
So for example if you have all 3 axis involved, it has to be (cos θ/2, r3sin θ/2, r3sin θ/2, r3*sin θ/2) where r3 is the reciprocal of the square root of 3. This is how I would explain that the rotation results you mention at the end of your post fail to conserve the length of the vector when several axis are involved.
The critical piece is thus this line in function rotate3f:
q = cos a' : ((\x -> if x `elem` axes then sin a' else 0) <$> [X,Y,Z])
where a normalization factor is missing.
Your code offers a number of opportunities for readability improvement. You might consider using CodeReview for further details.
A major concern is the fact that the source code lines are too wide. If the reader has to use an horizontal slider, it is much more difficult to understand the code and find the bugs. Below, I will try to avoid going beyond 80 characters width.
First, we need some quaternion infrastructure:
{-# LANGUAGE ScopedTypeVariables #-}
{-# LANGUAGE ExplicitForAll #-}
type GLfloat = Float
type GLfloatV3 = (GLfloat, GLfloat, GLfloat)
type QuatFloat = [GLfloat]
data Axes = X | Y | Z deriving Eq
qmul :: QuatFloat -> QuatFloat -> QuatFloat
qmul [qa0, qa1, qa2, qa3] [qb0, qb1, qb2, qb3] =
[
qa0*qb0 - qa1*qb1 - qa2*qb2 - qa3*qb3 ,
qa0*qb1 + qa1*qb0 + qa2*qb3 - qa3*qb2 ,
qa0*qb2 - qa1*qb3 + qa2*qb0 + qa3*qb1 ,
qa0*qb3 + qa1*qb2 - qa2*qb1 + qa3*qb0
]
qmul _ _ = error "Quaternion length differs from 4"
qconj :: QuatFloat -> QuatFloat
qconj q = (head q) : (map negate (tail q)) -- q-conjugation
rotate :: [GLfloat] -> [GLfloat] -> GLfloatV3 -> GLfloatV3
rotate q q' (x,y,z) = let p = [0, x,y,z]
[q0,q1,q2,q3] = qmul (qmul q p) q'
in (q1, q2, q3)
Note that the idea of defining ad hoc types not only allows for reduced code width, but that also gives extra flexibility. If some day you decide to represent quaternions by some other data structure which is more efficient than a plain list, it can be done while leaving the client code unchanged.
Next, the rotation code proper. Function rotQuat0 is your initial algorithm, which reproduces the numerical results mentioned at the end of your question. Function rotQuat1 is the modified version giving a 1-normalized quaternion.
-- original code:
rotQuat0 :: [Axes] -> GLfloat -> QuatFloat
rotQuat0 axes angle = let fn x = if (x `elem` axes) then (sin angle) else 0
in (cos angle) : (map fn [X,Y,Z])
-- modified code:
rotQuat1 :: [Axes] -> GLfloat -> QuatFloat
rotQuat1 axes angle = let corr = 1.0 / sqrt (fromIntegral (length axes))
fn x = if (x `elem` axes) then corr*(sin angle) else 0
in (cos angle) : (map fn [X,Y,Z])
Code using rotQuat1:
rotate3f :: GLfloatV3 -> [Axes] -> GLfloat -> [GLfloatV3] -> [GLfloatV3]
rotate3f _ _ _ [] = []
rotate3f _ [] _ _ = []
rotate3f org axes degθ pts =
let -- convert to radians and divide by 2, as all q components take θ/2
a' = (degθ * (pi / 180)) / 2
u :: [GLfloatV3]
u = [org, (0,0,0)]
-- translation if I don't want to rotate it by the origin
p' = translate3f pts u
-- if the axis is set, then its related component is
-- equal to sin θ/2, otherwise it will be zero
---- q = cos a' : ((\x -> if x `elem` axes then sin a' else 0) <$> [X,Y,Z])
q = rotQuat1 axes a' -- modified version
q' = qconj q
-- rotate and translate again to put the object where it belongs
in translate3f ((rotate q q') <$> p') [(0,0,0), org]
translate3f :: [GLfloatV3] -> [GLfloatV3] -> [GLfloatV3]
translate3f pts [(ax,ay,az), (bx,by,bz)] =
let dx = bx - ax
dy = by - ay
dz = bz - az
in map (\(x,y,z) -> (x + dx, y + dy, z + dz)) pts
Testing code:
sqNorm3 :: GLfloatV3 -> GLfloat
sqNorm3 (x,y,z) = x*x + y*y +z*z
printAsLines :: Show α => [α] -> IO ()
printAsLines xs = mapM_ (putStrLn . show) xs
main = do
let pt = (1,2,3) :: GLfloatV3
pt1 = rotate3f (0,0,0) [X] 90 [pt]
pt2 = rotate3f (0,0,0) [X,Y] 90 [pt]
pt3 = rotate3f (0,0,0) [X,Y,Z] 90 [pt]
pts = map head [pt1, pt2, pt3]
ptN = map sqNorm3 pts
printAsLines pts
putStrLn " "
printAsLines ptN
Let's check that with function rotQuat1, the squared norm of your initial (1,2,3) input vector (that is 1+4+9=13) remains unchanged, as befits a proper rotation:
$ ghc opengl00.hs -o ./opengl00.x && ./opengl00.x
[1 of 1] Compiling Main ( opengl00.hs, opengl00.o )
Linking ./opengl00.x ...
(0.99999994,-3.0,2.0)
(3.6213198,-0.62132025,0.70710695)
(2.5773501,0.84529924,2.5773501)
14.0
13.999995
13.999998
$
Unfortunately I don't have enough time to install the OpenGL infrastructure and reproduce the animation. Please let us know whether this fixes the whole thing.