source: trunk/source/examples/rubix/vectors.lisp @ 13474

Last change on this file since 13474 was 843, checked in by gb, 15 years ago

new files

  • Property svn:eol-style set to native
  • Property svn:keywords set to Author Date Id Revision
File size: 8.0 KB
Line 
1(in-package :cl-user)
2
3;; A stylistic ideosynchracy of C++ was passing result pointers into functions
4;; to reduce the impact of the lack of garbage collection. It reduces consing
5;; and allows functions to modify wrapped vectors and the like in place, so
6;; it's laudable to keep around, but in general I've made such things an
7;; optional final argument.
8
9;; To-do list:
10;; When i make foreign function calls in to glut, glu, or opengl, i should
11;; do type checking to trap errors in lisp.
12
13(defparameter *x-axis*     (make-array 3 :initial-contents '( 1.0  0.0  0.0)
14                                       :element-type 'single-float))
15(defparameter *y-axis*     (make-array 3 :initial-contents '( 0.0  1.0  0.0)
16                                       :element-type 'single-float))
17(defparameter *z-axis*     (make-array 3 :initial-contents '( 0.0  0.0  1.0)
18                                       :element-type 'single-float))
19(defparameter *neg-x-axis* (make-array 3 :initial-contents '(-1.0  0.0  0.0)
20                                       :element-type 'single-float))
21(defparameter *neg-y-axis* (make-array 3 :initial-contents '( 0.0 -1.0  0.0)
22                                       :element-type 'single-float))
23(defparameter *neg-z-axis* (make-array 3 :initial-contents '( 0.0  0.0 -1.0)
24                                       :element-type 'single-float))
25(defparameter *the-origin* (make-array 3 :initial-contents '( 0.0  0.0  0.0)
26                                       :element-type 'single-float))
27
28(defparameter *hel-white*   (make-array 4 :initial-contents '(1.0 1.0  1.0 1.0)
29                                        :element-type 'single-float))
30(defparameter *hel-grey*    (make-array 4 :initial-contents '(0.3 0.3  0.3 1.0)
31                                        :element-type 'single-float))
32(defparameter *hel-black*   (make-array 4 :initial-contents '(0.0 0.0  0.0 1.0)
33                                        :element-type 'single-float))
34(defparameter *hel-red*     (make-array 4 :initial-contents '(1.0 0.0  0.0 1.0)
35                                        :element-type 'single-float))
36(defparameter *hel-green*   (make-array 4 :initial-contents '(0.0 0.33 0.0 1.0)
37                                        :element-type 'single-float))
38(defparameter *hel-blue*    (make-array 4 :initial-contents '(0.0 0.0  1.0 1.0)
39                                        :element-type 'single-float))
40(defparameter *hel-yellow*  (make-array 4 :initial-contents '(1.0 1.0  0.0 1.0)
41                                        :element-type 'single-float))
42(defparameter *hel-cyan*    (make-array 4 :initial-contents '(0.0 1.0  1.0 1.0)
43                                        :element-type 'single-float))
44(defparameter *hel-magenta* (make-array 4 :initial-contents '(1.0 0.0  1.0 1.0)
45                                        :element-type 'single-float))
46(defparameter *hel-peach*   (make-array 4 :initial-contents '(1.0 0.3  0.2 1.0)
47                                        :element-type 'single-float))
48(defparameter *hel-pink*    (make-array 4 :initial-contents '(1.0 0.3  0.3 1.0)
49                                        :element-type 'single-float))
50(defparameter *hel-orange*  (make-array 4 :initial-contents '(1.0 0.3  0.0 1.0)
51                                        :element-type 'single-float))
52
53(defun radians (degrees)
54  (/ (* 3.14159 degrees) 180.0))
55(defun degrees (radians)
56  (/ (* 180.0 radians) 3.14159))
57(defun mag (p)
58  (let ((p0 (elt p 0))
59        (p1 (elt p 1))
60        (p2 (elt p 2)))
61    (+ (* p0 p0) (* p1 p1) (* p2 p2))))
62(defun normalize (p)
63  (let ((d 0.0))
64    (dotimes (i 3) (incf d (expt (elt p i) 2)))
65    (when (< 0.0 d)
66      (setf d (sqrt d))
67      (dotimes (i 3) (setf (elt p i) (/ (elt p i) d))))
68    p))
69
70(defun add-vectors (a b &optional result)
71  (or result (setf result (make-array 3)))
72  (dotimes (i 3)
73    (setf (elt result i) (+ (elt a i) (elt b i))))
74  result)
75(defun scale-vector (a n &optional result)
76  (or result (setf result (make-array 3)))
77  (dotimes (i 3)
78    (setf (elt result i) (* (elt a i) n)))
79  result)
80#+ignore ; overridden by lower defn anyway
81(defun cross (a b c &optional norm)
82  (or norm (setf norm (make-array 3)))
83  (let ((a0 (elt a 0)) (a1 (elt a 1)) (a2 (elt a 2))
84        (b0 (elt b 0)) (b1 (elt b 1)) (b2 (elt b 2))
85        (c0 (elt c 0)) (c1 (elt c 1)) (c2 (elt c 2)))
86    (setf (elt norm 0) (- (* (- b1 a1) (- c2 a2)) (* (- b2 a2) (- c1 a1)))
87          (elt norm 1) (- (* (- b2 a2) (- c0 a0)) (* (- b0 a0) (- c2 a2)))
88          (elt norm 2) (- (* (- b0 a0) (- c1 a1)) (* (- b1 a1) (- c0 a0)))))
89  norm)
90(defun cross (v1 v2 &optional crossproduct)
91  (or crossproduct (setf crossproduct (make-array 3)))
92  (setf (elt crossproduct 0) (- (* (elt v1 1) (elt v2 2))
93                                (* (elt v1 2) (elt v2 1)))
94        (elt crossproduct 1) (- (* (elt v1 2) (elt v2 0))
95                                (* (elt v1 0) (elt v2 2)))
96        (elt crossproduct 2) (- (* (elt v1 0) (elt v2 1))
97                                (* (elt v1 1) (elt v2 0))))
98  crossproduct)
99(defun dot (v1 v2)
100  (+ (* (elt v1 0) (elt v2 0))
101     (* (elt v1 1) (elt v2 1))
102     (* (elt v1 2) (elt v2 2))))
103
104
105;; quaterion class (note that in my c++ code i use a type for this,
106;; but since the quaternions aren't ever going to be in the C world
107;; the lisp representation doesn't matter)
108(defclass quaternion ()
109  ((w :initform 1.0 :initarg :w :accessor w)
110   (xyz :initform nil :initarg :xyz :accessor xyz))
111  (:default-initargs :xyz (make-array 3 :initial-element 0.0)))
112(defmethod addquats ((q1 quaternion) (q2 quaternion) &optional result)
113  (or result (setf result (make-instance 'quaternion)))
114  (setf (w result) (+ (w q1) (w q2)))
115  (add-vectors (xyz q1) (xyz q2) (xyz result))
116  result)
117;; this computes q1*q2 not the other way around, so it does q2's rotation first
118(defmethod mulquats ((q1 quaternion) (q2 quaternion) &optional result)
119  (or result (setf result (make-instance 'quaternion)))
120  (let ((t1 (make-array 3 :initial-element 0.0))
121        (t2 (make-array 3 :initial-element 0.0))
122        (t3 (make-array 3 :initial-element 0.0)))
123    (scale-vector (xyz q1) (w q2) t1)
124    (scale-vector (xyz q2) (w q1) t2)
125    (cross (xyz q1) (xyz q2) t3)
126
127    (setf (w result) (- (* (w q1) (w q2)) (dot (xyz q1) (xyz q2))))
128    (add-vectors t1 t2 (xyz result))
129    (add-vectors t3 (xyz result) (xyz result))
130    result))
131
132;; unit quaternions are made up of the axis of rotation (xyz) as a vector with
133;; magnitude sin(theta/2) and a scalar (w) with magnitude cos(theta/2);
134(defun axis-angle->quat (axis angle &optional q)
135  (or q (setf q (make-instance 'quaternion)))
136  (let ((theta (radians angle)))
137    (setf (w q) (cos (/ theta 2.0)))
138    (dotimes (i 3) (setf (elt (xyz q) i) (elt axis i)))
139    (normalize (xyz q))
140    (scale-vector (xyz q) (sin (/ theta 2.0)) (xyz q))
141    q))
142(defun quat->axis-angle (q &optional axis-angle) ; <- cons pair, bleah
143  (or axis-angle (setf axis-angle (cons (make-array 3 :initial-element 0.0)
144                                        0.0)))
145  (let ((len (mag (xyz q))))
146    (cond ((> len 0.0001)
147           (setf (cdr axis-angle) (degrees (* 2.0 (acos (w q)))))
148           (dotimes (i 3) (setf (elt (car axis-angle) i)
149                                (/ (elt (xyz q) i) len))))
150          (t ;; if len is near 0, angle of rotation is too, which can cause
151             ;; trouble elsewhere, so just return zero
152           (setf (cdr axis-angle) 0.0)
153           (setf (elt (car axis-angle) 0) 0.0
154                 (elt (car axis-angle) 1) 0.0
155                 (elt (car axis-angle) 2) 1.0)))
156    axis-angle))
157
158;; this wraps a 9-number function with a point/point/vector function
159;; note that this could REALLY stand to do some type checking...
160(defun myLookAt (camera-position target-position upvector)
161  (#_gluLookAt
162   (coerce (elt camera-position 0) 'double-float)
163   (coerce (elt camera-position 1) 'double-float)
164   (coerce (elt camera-position 2) 'double-float)
165   (coerce (elt target-position 0) 'double-float)
166   (coerce (elt target-position 1) 'double-float)
167   (coerce (elt target-position 2) 'double-float)
168   (coerce (elt upvector 0) 'double-float)
169   (coerce (elt upvector 1) 'double-float)
170   (coerce (elt upvector 2) 'double-float)))
Note: See TracBrowser for help on using the repository browser.