← Back to branch summary

~ubuntu-branches/ubuntu/feisty/libctl/feisty

« back to all changes in this revision

Viewing changes to base/matrix3x3.scm

  • Committer: Bazaar Package Importer
  • Author(s): Josselin Mouette
  • Date: 2002-04-17 10:36:45 UTC
  • Revision ID: james.westby@ubuntu.com-20020417103645-29vomjspk4yf4olw
Tags: upstream-2.1
Import upstream version 2.1

Show diffs side-by-side

added added

removed removed

Lines of Context:
base/matrix3x3.scm
 
1
; libctl: flexible Guile-based control files for scientific software 
 
2
; Copyright (C) 1998, 1999, 2000, 2001, 2002, Steven G. Johnson
 
3
;
 
4
; This library is free software; you can redistribute it and/or
 
5
; modify it under the terms of the GNU Lesser General Public
 
6
; License as published by the Free Software Foundation; either
 
7
; version 2 of the License, or (at your option) any later version.
 
8
;
 
9
; This library is distributed in the hope that it will be useful,
 
10
; but WITHOUT ANY WARRANTY; without even the implied warranty of
 
11
; MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 
12
; Lesser General Public License for more details.
 
13
; 
 
14
; You should have received a copy of the GNU Lesser General Public
 
15
; License along with this library; if not, write to the
 
16
; Free Software Foundation, Inc., 59 Temple Place - Suite 330,
 
17
; Boston, MA  02111-1307, USA.
 
18
;
 
19
; Steven G. Johnson can be contacted at stevenj@alum.mit.edu.
 
20
 
 
21
; ****************************************************************
 
22
; matrix3x3 and associated functions (a type to represent 3x3 matrices)
 
23
 
 
24
; we represent a matrix3x3 by a vector of 3 columns, each of which
 
25
; is a 3-vector.
 
26
 
 
27
(define (matrix3x3 c1 c2 c3)
 
28
  (vector c1 c2 c3))
 
29
(define (matrix3x3? m)
 
30
  (and (vector? m)
 
31
       (= (vector-length m) 3)
 
32
       (vector-for-all? m vector3?)))
 
33
(define (real-matrix3x3? m)
 
34
  (and (matrix3x3? m) (vector-for-all? m real-vector3?)))
 
35
(define (matrix3x3-col m col)
 
36
  (vector-ref m col))
 
37
(define (matrix3x3-ref m row col)
 
38
  (vector-ref (matrix3x3-col m col) row))
 
39
(define (matrix3x3-row m row)
 
40
  (vector3 (matrix3x3-ref m row 0)
 
41
           (matrix3x3-ref m row 1)
 
42
           (matrix3x3-ref m row 2)))
 
43
 
 
44
(define (matrix3x3-transpose m)
 
45
  (matrix3x3
 
46
   (matrix3x3-row m 0)
 
47
   (matrix3x3-row m 1)
 
48
   (matrix3x3-row m 2)))
 
49
 
 
50
(define (matrix3x3-conj m) (vector-map vector3-conj m))
 
51
(define (matrix3x3-adjoint m) (matrix3x3-conj (matrix3x3-transpose m)))
 
52
 
 
53
(define (matrix3x3+ m1 m2)
 
54
  (vector-map vector3+ m1 m2))
 
55
(define (matrix3x3- m1 m2)
 
56
  (vector-map vector3- m1 m2))
 
57
 
 
58
(define (matrix3x3-scale s m)
 
59
  (vector-map (lambda (v) (vector3-scale s v)) m))
 
60
(define (matrix3x3-mv-mult m v)
 
61
  (vector3 (vector3-dot (matrix3x3-row m 0) v)
 
62
           (vector3-dot (matrix3x3-row m 1) v)
 
63
           (vector3-dot (matrix3x3-row m 2) v)))
 
64
(define (matrix3x3-vm-mult v m)
 
65
  (vector3 (vector3-dot (matrix3x3-col m 0) v)
 
66
           (vector3-dot (matrix3x3-col m 1) v)
 
67
           (vector3-dot (matrix3x3-col m 2) v)))
 
68
(define (matrix3x3-mm-mult m1 m2)
 
69
  (matrix3x3
 
70
   (vector3 (vector3-dot (matrix3x3-row m1 0) (matrix3x3-col m2 0))
 
71
            (vector3-dot (matrix3x3-row m1 1) (matrix3x3-col m2 0))
 
72
            (vector3-dot (matrix3x3-row m1 2) (matrix3x3-col m2 0)))
 
73
   (vector3 (vector3-dot (matrix3x3-row m1 0) (matrix3x3-col m2 1))
 
74
            (vector3-dot (matrix3x3-row m1 1) (matrix3x3-col m2 1))
 
75
            (vector3-dot (matrix3x3-row m1 2) (matrix3x3-col m2 1)))
 
76
   (vector3 (vector3-dot (matrix3x3-row m1 0) (matrix3x3-col m2 2))
 
77
            (vector3-dot (matrix3x3-row m1 1) (matrix3x3-col m2 2))
 
78
            (vector3-dot (matrix3x3-row m1 2) (matrix3x3-col m2 2)))))
 
79
(define (matrix3x3* a b)
 
80
  (cond
 
81
   ((number? a) (matrix3x3-scale a b))
 
82
   ((number? b) (matrix3x3-scale b a))
 
83
   ((vector3? a) (matrix3x3-vm-mult a b))
 
84
   ((vector3? b) (matrix3x3-mv-mult a b))
 
85
   (else (matrix3x3-mm-mult a b))))
 
86
 
 
87
(define (matrix3x3-determinant m)
 
88
  (-
 
89
   (+ (* (matrix3x3-ref m 0 0) (matrix3x3-ref m 1 1) (matrix3x3-ref m 2 2))
 
90
      (* (matrix3x3-ref m 0 1) (matrix3x3-ref m 1 2) (matrix3x3-ref m 2 0))
 
91
      (* (matrix3x3-ref m 1 0) (matrix3x3-ref m 2 1) (matrix3x3-ref m 0 2)))
 
92
   (+ (* (matrix3x3-ref m 0 2) (matrix3x3-ref m 1 1) (matrix3x3-ref m 2 0))
 
93
      (* (matrix3x3-ref m 0 1) (matrix3x3-ref m 1 0) (matrix3x3-ref m 2 2))
 
94
      (* (matrix3x3-ref m 1 2) (matrix3x3-ref m 2 1) (matrix3x3-ref m 0 0)))))
 
95
 
 
96
(define (matrix3x3-inverse m)
 
97
  (matrix3x3-scale
 
98
   (/ (matrix3x3-determinant m))
 
99
   (matrix3x3
 
100
    (vector3
 
101
     (- (* (matrix3x3-ref m 1 1) (matrix3x3-ref m 2 2))
 
102
        (* (matrix3x3-ref m 1 2) (matrix3x3-ref m 2 1)))
 
103
     (- (* (matrix3x3-ref m 1 2) (matrix3x3-ref m 2 0))
 
104
        (* (matrix3x3-ref m 1 0) (matrix3x3-ref m 2 2)))
 
105
     (- (* (matrix3x3-ref m 1 0) (matrix3x3-ref m 2 1))
 
106
        (* (matrix3x3-ref m 1 1) (matrix3x3-ref m 2 0))))
 
107
    (vector3
 
108
     (- (* (matrix3x3-ref m 2 1) (matrix3x3-ref m 0 2))
 
109
        (* (matrix3x3-ref m 0 1) (matrix3x3-ref m 2 2)))
 
110
     (- (* (matrix3x3-ref m 0 0) (matrix3x3-ref m 2 2))
 
111
        (* (matrix3x3-ref m 0 2) (matrix3x3-ref m 2 0)))
 
112
     (- (* (matrix3x3-ref m 0 1) (matrix3x3-ref m 2 0))
 
113
        (* (matrix3x3-ref m 0 0) (matrix3x3-ref m 2 1))))
 
114
    (vector3
 
115
     (- (* (matrix3x3-ref m 0 1) (matrix3x3-ref m 1 2))
 
116
        (* (matrix3x3-ref m 1 1) (matrix3x3-ref m 0 2)))
 
117
     (- (* (matrix3x3-ref m 1 0) (matrix3x3-ref m 0 2))
 
118
        (* (matrix3x3-ref m 0 0) (matrix3x3-ref m 1 2)))
 
119
     (- (* (matrix3x3-ref m 1 1) (matrix3x3-ref m 0 0))
 
120
        (* (matrix3x3-ref m 1 0) (matrix3x3-ref m 0 1)))))))
 
121
 
 
122
; ****************************************************************
 
123
 
 
124
; Return the rotation matrix for rotating by theta around axis:
 
125
(define (rotation-matrix3x3 axis theta)
 
126
  (matrix3x3
 
127
   (rotate-vector3 axis theta (vector3 1 0 0))
 
128
   (rotate-vector3 axis theta (vector3 0 1 0))
 
129
   (rotate-vector3 axis theta (vector3 0 0 1))))
 
130
 
 
131
; ****************************************************************