;    Copyright (C) 2006  Will M. Farr <farr@mit.edu>
;
;    This program is free software; you can redistribute it and/or modify
;    it under the terms of the GNU General Public License as published by
;    the Free Software Foundation; either version 2 of the License, or
;    (at your option) any later version.
;
;    This program is distributed in the hope that it will be useful,
;    but WITHOUT ANY WARRANTY; without even the implied warranty of
;    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
;    GNU General Public License for more details.
;
;    You should have received a copy of the GNU General Public License along
;    with this program; if not, write to the Free Software Foundation, Inc.,
;    51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.

(module quaternions-test "generics.ss"
  (require (all-except (lib "43.ss" "srfi") vector-fill! vector->list)
           "quaternions.ss" "tuples.ss" "test.ss")
  
  (provide quaternions-test-suite)
  
  (define quaternions-test-suite
    (test-suite
     "quaternions.ss test suite"
     (test-case
      "quaternion linearity"
      (let ((q (make <quaternion> :real-part 10 :imag-part (up 2 87 3))))
        (check equals? (+ q q) (* q 2))
        (check equals? (+ (* 10 q) (- q) (* -6 q)) (* 3 q))
        (check equals? (- (* 10 q) (* 5 q)) (* 5 q))))
     (test-case 
      "rotations and multiplication of quaternions"
      (let ((theta (/ pi 7)))
        (let ((rz (vector->quaternion-rotation (* theta (up 0 0 1)))))
          (let ((v (up 12 32 512)))
            (check (tuple-close? 1e-6)
                   (quaternion-rotate v rz)
                   (up (- (* (ref v 0)
                             (cos theta))
                          (* (ref v 1)
                             (sin theta)))
                       (+ (* (ref v 1)
                             (cos theta))
                          (* (ref v 0)
                             (sin theta)))
                       (ref v 2))))))
      (let ((theta (/ pi 4)))
        (let ((rx-theta (vector->quaternion-rotation (* theta (up 1 0 0))))
              (ry-theta (vector->quaternion-rotation (* theta (up 0 1 0))))
              (rx-inv (vector->quaternion-rotation (up (asin (sqrt 2/3)) 0 0)))
              (ry-inv (vector->quaternion-rotation (up 0 (/ pi 6) 0)))
              (rz-inv (vector->quaternion-rotation (up 0 0 (acos (sqrt 2/3))))))
          (let ((v (up 23 43 -243)))
            (check (tuple-close? 1e-6)
                   (quaternion-rotate v (* rx-theta ry-theta))
                   (quaternion-rotate v (* rz-inv ry-inv rx-inv)))))))
     (test-case
      "(compose q->ea ea->q) is identity"
      (check-tuple-close? 
       1e-6
       ((compose quaternion-rotation->euler-angles euler-angles->quaternion-rotation) 
        (up 0.0579822 0.0437467 0.0786416))
       (up 0.0579822 0.0437467 0.0786416)))
     (test-case 
      "(compose q->ra ra->q) is identity"
      (let ((ra (up (random) (random) (random))))
        (check-tuple-close? 
         1e-6
         ((compose quaternion-rotation->rectangular-angles
                   rectangular-angles->quaternion-rotation)
          ra)
         ra))))))