flight-sim/math.lisp

97 lines
3.2 KiB
Common Lisp

(in-package #:flight-sim)
;;; degrees to radians
(defmacro dtr (d)
`(/ (* ,d pi) 180))
;;; radians to degress
(defmacro rtd (r)
`(/ (* ,r 180) pi))
(deftype point-vector () '(simple-array float (*)))
(deftype shape-vector () '(simple-array point-vector (*)))
(deftype pos-int () '(integer 0 *))
(deftype ref-vector () '(simple-array pos-int (*)))
(deftype shape-ref-vector () '(simple-array ref-vector (*)))
;; function to determine value lying on start to end taking time duration at now
(defun converge (start end duration now)
(if (> now duration)
end
(float (+ start (* (- end start) (if (eql now 0.0) 0.0 (/ (min now duration) duration)))))))
;; returns a real lisp 2d array: args in radians
(defun make-rotation-matrix (xa ya za)
(let ((sxa (sin xa))
(cxa (cos xa))
(sya (sin ya))
(cya (cos ya))
(sza (sin za))
(cza (cos za)))
(make-array '(3 3) :initial-contents (list (list (* cya cza) (+ (- (* cxa sza)) (* sxa sya cza)) (+ (* sxa sza) (* cxa sya cza)))
(list (* cya sza) (+ (* cxa cza) (* sxa sya sza)) (+ (- (* sxa cza)) (* cxa sya sza)))
(list (- sya) (* sxa cya) (* cxa cya))))))
(defun rotate* (m v)
(let ((result (make-array 3 :initial-element 0)))
(dotimes (x 3)
(dotimes (y 3)
(incf (aref result x) (* (aref v y) (aref m x y)))))
result))
(defun translate-point (v1 v2 &optional (fn #'+))
(let ((result (make-array 3)))
(dotimes (i 3)
(setf (aref result i) (funcall fn (aref v1 i) (aref v2 i))))
result))
(defun translate-triangle (tri position)
(make-array (length tri) :initial-contents
(loop for v across tri collecting (translate-point position v))))
(defun rotate-triangle (tri m)
(make-array (length tri) :initial-contents
(loop for v across tri collecting (rotate* m v))))
(defun scale-vector (v a)
(make-array (length v) :initial-contents (loop for i across v collecting (* i a))))
; scale points by a
(defun scale-points (points a)
(make-array (length points) :initial-contents (loop for v across points collecting (scale-vector v a))))
; scale poitns by v (x y z)
(defun transform-points (points xyz)
(make-array (length points) :initial-contents
(loop for v across points collecting
(make-array 3 :initial-contents
(list (* (aref v 0) (first xyz)) (* (aref v 1) (second xyz)) (* (aref v 2) (third xyz)))))))
; returns a vector with all elemts scaled to biggest 1 which is scaled to 1
; e.x. (scale-vector (8 4 2)) -> (1 .5 .25)
(defun scale-vector-1 (v)
(let ((max (loop for i across v maximize (abs i) into result finally (return result))))
(make-array (length v) :initial-contents (loop for i across v collecting (float (/ i max))))))
(defun dot (v1 v2)
(loop for i from 0 to (1- (length v1)) summing (* (aref v1 i) (aref v2 i))))
(defun vector-length (v)
(sqrt (dot v v)))
(defun scalar-proj (vector direction)
(let ((length (vector-length direction)))
(if (eql 0 length)
0
(/ (dot vector direction) length))))
(defun vector- (v1 v2)
(make-array (length v1) :initial-contents (loop for i from 0 to (1- (length v1)) collecting (- (aref v1 i) (aref v2 i)))))
(defun vector+ (v1 v2)
(make-array (length v1) :initial-contents (loop for i from 0 to (1- (length v1)) collecting (+ (aref v1 i) (aref v2 i)))))