;    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 lie-group-SO3 "generics.ss"
  (require "manifolds.ss" "lie-groups.ss" "quaternions.ss" "tuples.ss")
  
  (provide SO3 SO3-euler-angles-chart SO3-rectangular-angles-chart)
  
  (define-values (SO3-euler-angles-chart SO3)
    (recursive-make
     (SO3-euler-angles-chart
      <chart>
      :dim 3
      :chi (lambda (m) 
             (quaternion-rotation->euler-angles 
              (slot-ref m 'point)))
      :chiinv (lambda (x)
                (make <lie-group-element>
                      :point (euler-angles->quaternion-rotation x)
                      :group SO3)))
     (SO3 
      <lie-group>
      :standard-chart SO3-euler-angles-chart
      :identity (make <lie-group-element>
                      :point quaternion-identity-rotation
                      :group SO3)
      :inverse (lambda (g)
                 (make <lie-group-element>
                       :point (conjugate (slot-ref g 'point))
                       :group SO3))
      :multiplication (lambda (g1 g2)
                        (make <lie-group-element>
                              :point (* (slot-ref g1 'point) 
                                        (slot-ref g2 'point))
                              :group SO3)))))

  ;; rectangular chart is defined by (x,y,z) |-> Rz(z).Ry(y).Rx(x) (as a rotation matrix)
  (define SO3-rectangular-angles-chart
    (make <chart>
          :dim 3
          :chi (lambda (m)
                 (with-slots m (point)
                   (quaternion-rotation->rectangular-angles point)))
          :chiinv (lambda (ra)
                    (make <lie-group-element>
                          :point (rectangular-angles->quaternion-rotation ra)
                          :group SO3)))))