Skip to content

HTTPS clone URL

Subversion checkout URL

You can clone with
or
.
Download ZIP
Newer
Older
100644 82 lines (76 sloc) 3.269 kB
46079e7 @mveiskats Added matrix.lisp with invert-matrix in it
mveiskats authored
1 (in-package :letcn)
2
4d76856 @mveiskats Moved matrix stuff from utility.lisp to matrix.lisp
mveiskats authored
3 (defun make-matrix (contents)
4 (make-array (list (length contents) (length (first contents)))
5 :element-type 'float
6 :initial-contents contents))
7
8 (defparameter identity-matrix
9 (make-matrix '((1.0 0.0 0.0 0.0)
10 (0.0 1.0 0.0 0.0)
11 (0.0 0.0 1.0 0.0)
12 (0.0 0.0 0.0 1.0))))
13
14 ;;; Calculate inverse matrix. Returns nil if not inversible.
46079e7 @mveiskats Added matrix.lisp with invert-matrix in it
mveiskats authored
15 (defun invert-matrix (m)
16 (declare (type (array float (3 3)) m))
17 (let ((m00 (aref m 0 0)) (m01 (aref m 0 1)) (m02 (aref m 0 2))
18 (m10 (aref m 1 0)) (m11 (aref m 1 1)) (m12 (aref m 1 2))
19 (m20 (aref m 2 0)) (m21 (aref m 2 1)) (m22 (aref m 2 2)))
20 (let ((det3 (- (+ (* m00 m11 m22) (* m10 m21 m02) (* m20 m01 m12))
21 (* m00 m21 m12) (* m10 m01 m22) (* m20 m11 m02))))
22 (unless (zerop det3)
23 (flet ((det2/det3 (a b c d) (/ (- (* a d) (* b c)) det3)))
24 (make-matrix (list (list (det2/det3 m11 m12 m21 m22)
25 (det2/det3 m02 m01 m22 m21)
26 (det2/det3 m01 m02 m11 m12))
27 (list (det2/det3 m12 m10 m22 m20)
28 (det2/det3 m00 m02 m20 m22)
29 (det2/det3 m02 m00 m12 m10))
30 (list (det2/det3 m10 m11 m20 m21)
31 (det2/det3 m01 m00 m21 m20)
32 (det2/det3 m00 m01 m10 m11)))))))))
4d76856 @mveiskats Moved matrix stuff from utility.lisp to matrix.lisp
mveiskats authored
33
34 ;;; Matrix multiplication
35 (defun matrix-product (a b)
36 (declare (array a b))
37 (if (eq (array-rank b) 1)
38 ;; matrix * vector
39 (let* ((result (make-array 3 :initial-element 0)))
40 (dotimes (i 3)
41 (dotimes (k 3)
42 (incf (aref result i)
43 (* (aref a i k)
44 (aref b k)))))
45 result)
46 ;; matrix * matrix
47 (let* ((m (array-dimension a 0))
48 (n (array-dimension b 1))
49 (result (make-array (list m n) :initial-element 0)))
50 (dotimes (i m)
51 (dotimes (j n)
52 (dotimes (k (array-dimension a 1))
53 (incf (aref result i j)
54 (* (aref a i k)
55 (aref b k j))))))
56 result)))
57
58 ;;; Axis has to be normalized vector
59 (defun rotation-matrix (axis angle)
60 (let* ((x (aref axis 0))
61 (y (aref axis 1))
62 (z (aref axis 2))
63 (sina (sin angle))
64 (cosa (cos angle))
65 (1-cosa (- 1 cosa)))
66 (make-array
67 '(4 4)
68 :element-type 'float
69 :initial-contents (list (list (+ cosa (* x x 1-cosa))
70 (- (* x y 1-cosa) (* z sina))
71 (+ (* x z 1-cosa) (* y sina))
72 0.0)
73 (list (+ (* y x 1-cosa) (* z sina))
74 (+ cosa (* y y 1-cosa))
75 (- (* y z 1-cosa) (* x sina))
76 0.0)
77 (list (- (* z x 1-cosa) (* y sina))
78 (+ (* z y 1-cosa) (* x sina))
79 (+ cosa (* z z 1-cosa))
80 0.0)
81 '(0.0 0.0 0.0 1.0)))))
Something went wrong with that request. Please try again.