Rewrite maths library's implementation and interface
Make use of CLOS for vector and matrix types Rewrite matrix implementation as column major Verify the computations in rewritten unit tests
This commit is contained in:
parent
9b69dee238
commit
dbc009d466
7 changed files with 749 additions and 302 deletions
|
|
@ -1,74 +1,78 @@
|
|||
#|
|
||||
This file is a part of stoe project.
|
||||
Copyright (c) 2014 Renaud Casenave-Péré (renaud@casenave-pere.fr)
|
||||
Copyright (c) 2015 Renaud Casenave-Péré (renaud@casenave-pere.fr)
|
||||
|#
|
||||
|
||||
(in-package :cl-user)
|
||||
(defpackage stoe.maths.geometry
|
||||
(:nicknames :geometry :geom)
|
||||
(:use :cl)
|
||||
(:use :cl :types :vector :matrix :quaternion)
|
||||
(:export :mat-trans :mat-scale :mat-rot
|
||||
:make-persp-matrix :make-ortho-matrix))
|
||||
(in-package :stoe.maths.geometry)
|
||||
|
||||
(defun mat-trans (vec)
|
||||
(declare (type f3:float3 vec))
|
||||
(let ((mat (f44:mat-ident)))
|
||||
(m:setcol mat 3 vec)))
|
||||
(defun mtranslate (vec)
|
||||
(let ((mat (mat-id 4 'single-float)))
|
||||
(setf (mref mat 3 0) (vref vec 0))
|
||||
(setf (mref mat 3 1) (vref vec 1))
|
||||
(setf (mref mat 3 2) (vref vec 2))
|
||||
mat))
|
||||
|
||||
(defun mat-scale (dim vec)
|
||||
(let ((mat (m:mat-ident (array-element-type vec) dim)))
|
||||
(m:setdiag mat vec)))
|
||||
(defun mscale (vec)
|
||||
(let ((mat (mat-id 4 'single-float)))
|
||||
(setf (mref mat 0 0) (vref vec 0))
|
||||
(setf (mref mat 1 1) (vref vec 1))
|
||||
(setf (mref mat 2 2) (vref vec 2))
|
||||
mat))
|
||||
|
||||
(defun mat-rot (angle &optional axis)
|
||||
(defun mrotate (angle &optional axis)
|
||||
(let ((cos (cos angle))
|
||||
(sin (sin angle)))
|
||||
(cond
|
||||
((null axis) (f22:mat cos (- sin) sin cos))
|
||||
((eq axis :x) (f44:mat 1 0 0 0
|
||||
0 cos (- sin) 0
|
||||
0 sin cos 0
|
||||
0 0 0 1))
|
||||
((eq axis :y) (f44:mat cos 0 sin 0
|
||||
0 1 0 0
|
||||
(- sin) 0 cos 0
|
||||
0 0 0 1))
|
||||
((eq axis :z) (f44:mat cos (- sin) 0 0
|
||||
sin cos 0 0
|
||||
0 0 1 0
|
||||
0 0 0 1))
|
||||
((arrayp axis)
|
||||
((null axis) (mat cos sin
|
||||
(- sin) cos))
|
||||
((eq axis :x) (mat 1.0 0.0 0.0 0.0
|
||||
0.0 cos sin 0.0
|
||||
0.0 (- sin) cos 0.0
|
||||
0.0 0.0 0.0 1.0))
|
||||
((eq axis :y) (mat cos 0.0 (-sin) 0.0
|
||||
0.0 1.0 0.0 0.0
|
||||
sin 0.0 cos 0.0
|
||||
0.0 0.0 0.0 1.0))
|
||||
((eq axis :z) (mat cos sin 0.0 0.0
|
||||
(- sin) cos 0.0 0.0
|
||||
0.0 0.0 1.0 0.0
|
||||
0.0 0.0 0.0 1.0))
|
||||
((subtypep (type-of axis) 'vect)
|
||||
(let ((1-cos (- 1.0 cos))
|
||||
(axis (v:safe-normalize axis nil))
|
||||
(mat (f44:mat-ident)))
|
||||
(axis (safe-normalize axis nil))
|
||||
(mat (mat-id 4 'single-float)))
|
||||
(unless (null axis)
|
||||
(v:with-attributes (x y z) axis
|
||||
(setf (aref mat 0 0) (+ (* x x) (* (- 1 (* x x)) cos)))
|
||||
(setf (aref mat 0 1) (- (* x y 1-cos) (* z sin)))
|
||||
(setf (aref mat 0 2) (+ (* x z 1-cos) (* y sin)))
|
||||
(setf (aref mat 1 0) (+ (* x y 1-cos) (* z sin)))
|
||||
(setf (aref mat 1 1) (+ (* y y) (* (- 1 (* y y)) cos)))
|
||||
(setf (aref mat 1 2) (- (* y z 1-cos) (* x sin)))
|
||||
(setf (aref mat 2 0) (- (* x z 1-cos) (* y sin)))
|
||||
(setf (aref mat 2 1) (+ (* y z 1-cos) (* x sin)))))
|
||||
(with-swizzle (x y z) axis
|
||||
(setf (mref mat 0 0) (+ (* 1-cos x x) cos))
|
||||
(setf (mref mat 0 1) (+ (* 1-cos x y) (* sin z)))
|
||||
(setf (mref mat 0 2) (- (* 1-cos x z) (* sin y)))
|
||||
(setf (mref mat 1 0) (- (* 1-cos x y) (* sin z)))
|
||||
(setf (mref mat 1 1) (+ (* 1-cos y y) cos))
|
||||
(setf (mref mat 1 2) (+ (* 1-cos y z) (* sin x)))
|
||||
(setf (mref mat 2 0) (+ (* 1-cos x z) (* sin y)))
|
||||
(setf (mref mat 2 1) (- (* 1-cos y z) (* sin x)))
|
||||
(setf (mref mat 2 2) (+ (* 1-cos z z) cos))))
|
||||
mat)))))
|
||||
|
||||
(defun calc-frustum-scale (fovy)
|
||||
(tan (/ (maths:deg-to-rad fovy) 2.0)))
|
||||
|
||||
(defun make-persp-matrix (fovy aspect near far)
|
||||
(let ((range (calc-frustum-scale fovy)))
|
||||
(defun mperspective (fovy aspect near far)
|
||||
(let ((range (tan (/ deg-to-rad fovy) 2.0)))
|
||||
(let ((left (* (- range) aspect))
|
||||
(right (* range aspect))
|
||||
(bottom (- range))
|
||||
(top range))
|
||||
(f44:mat (/ (* near 2) (- right left)) 0.0 0.0 0.0
|
||||
0.0 (/ (* near 2) (- top bottom)) 0.0 0.0
|
||||
0.0 0.0 (/ (+ far near) (- near far)) -1.0
|
||||
0.0 0.0 (/ (* 2.0 far near) (- near far)) 0.0))))
|
||||
(mat (/ (* near 2) (- right left)) 0.0 0.0 0.0
|
||||
0.0 (/ (* near 2) (- top bottom)) 0.0 0.0
|
||||
0.0 0.0 (/ (+ far near) (- near far)) (/ (* 2.0 far near) (- near far))
|
||||
0.0 0.0 -1.0 0.0))))
|
||||
|
||||
(defun make-ortho-matrix (width height)
|
||||
(f44:mat (/ 2.0 width) 0.0 0.0 -1.0
|
||||
0.0 (/ -2.0 height) 0.0 1.0
|
||||
0.0 0.0 1.0 0.0
|
||||
0.0 0.0 0.0 1.0))
|
||||
(defun morthogonal (width height)
|
||||
(mat (/ 2.0 width) 0.0 0.0 0.0
|
||||
0.0 (/ -2.0 height) 0.0 0.0
|
||||
0.0 0.0 1.0 0.0
|
||||
-1.0 1.0 0.0 1.0))
|
||||
|
|
|
|||
|
|
@ -1,112 +1,236 @@
|
|||
#|
|
||||
This file is a part of stoe project.
|
||||
Copyright (c) 2014 Renaud Casenave-Péré (renaud@casenave-pere.fr)
|
||||
Copyright (c) 2015 Renaud Casenave-Péré (renaud@casenave-pere.fr)
|
||||
|#
|
||||
|
||||
(in-package :cl-user)
|
||||
(defpackage stoe.maths.matrix
|
||||
(:nicknames :matrix :m)
|
||||
(:use :cl)
|
||||
(:shadow :+ :- :*)
|
||||
(:export :mat :mat-null :mat-ident
|
||||
:setrow :setcol :setdiag
|
||||
:+ :- :*))
|
||||
(:use :cl :alexandria :types :vector)
|
||||
(:import-from :vector
|
||||
:fill-vector :make-displaced-vector)
|
||||
(:export :mref
|
||||
:mat-null :mat-id
|
||||
:make-matrix
|
||||
:mat :mat2 :mat3 :mat4
|
||||
:transpose
|
||||
:m+ :m- :m*))
|
||||
(in-package :stoe.maths.matrix)
|
||||
|
||||
(defun make-matrix (type dim-x dim-y attrs)
|
||||
`(make-array (list ,dim-x ,dim-y) :element-type ',type
|
||||
:initial-contents (list ,@(loop for i below dim-x
|
||||
collect `(list ,@(loop for j below dim-y
|
||||
collect (let ((x (pop attrs)))
|
||||
(if (numberp x)
|
||||
(coerce x type)
|
||||
`(coerce ,x ',type)))))))))
|
||||
(defmethod dimension ((m matrix)) (slot-value m 'dimensions))
|
||||
|
||||
(defmacro mat (&rest attrs)
|
||||
(let* ((len (length attrs))
|
||||
(dim-x (floor (sqrt len)))
|
||||
(dim-y (if (= (cl:* dim-x dim-x) len) dim-x (cl:/ len dim-x))))
|
||||
(make-matrix 'single-float dim-x dim-y attrs)))
|
||||
(defmethod element-type ((m matrix)) (array-element-type (slot-value m 'array)))
|
||||
|
||||
(defun mat-null (type dim-x dim-y)
|
||||
(make-array `(,dim-x ,dim-y) :element-type type :initial-element (coerce 0 type)))
|
||||
(defun mref (m &rest subscripts)
|
||||
(let ((len (length subscripts))
|
||||
(dim-x (first (dimension m)))
|
||||
(dim-y (second (dimension m))))
|
||||
(assert (< len 3))
|
||||
(case len
|
||||
(2 (progn
|
||||
(assert (< (first subscripts) dim-x))
|
||||
(assert (< (second subscripts) dim-y))
|
||||
(aref (slot-value m 'array) (+ (* (first subscripts) dim-y)
|
||||
(second subscripts)))))
|
||||
(1 (progn
|
||||
(assert (< (first subscripts) dim-x))
|
||||
(make-displaced-vector (slot-value m 'array)
|
||||
(* (first subscripts) dim-y)
|
||||
dim-y))))))
|
||||
|
||||
(defun mat-ident (type dim)
|
||||
(let* ((ident-elt (coerce 1 type))
|
||||
(mat (mat-null type dim dim)))
|
||||
(defun set-mref (m &rest subscripts-or-value)
|
||||
(let ((len (length subscripts-or-value)))
|
||||
(assert (< len 4))
|
||||
(case len
|
||||
(3 (setf (aref (slot-value m 'array) (+ (* (first subscripts-or-value)
|
||||
(second (dimension m)))
|
||||
(second subscripts-or-value)))
|
||||
(third subscripts-or-value)))
|
||||
(2 (let* ((dim (second (dimension m)))
|
||||
(offset (* (first subscripts-or-value) dim))
|
||||
(v (second subscripts-or-value)))
|
||||
(assert (= dim (dimension v)))
|
||||
(loop for i below dim
|
||||
do (setf (aref (slot-value m 'array) (+ i offset)) (vref v i))
|
||||
finally (return (second subscripts-or-value))))))))
|
||||
|
||||
(defsetf mref set-mref)
|
||||
|
||||
(defun matrix-type (dim-x dim-y type)
|
||||
(if (/= dim-x dim-y)
|
||||
'matrix
|
||||
(case dim-x
|
||||
(2 (case type (single-float 'float22) (fixnum 'int22) (otherwise 'matrix)))
|
||||
(3 (case type (single-float 'float33) (fixnum 'int33) (otherwise 'matrix)))
|
||||
(4 (case type (single-float 'float44) (fixnum 'int44) (otherwise 'matrix)))
|
||||
(otherwise 'matrix))))
|
||||
|
||||
(defun mat-null (dims type)
|
||||
(let ((dim-x (if (listp dims) (first dims) dims))
|
||||
(dim-y (if (listp dims) (second dims) dims)))
|
||||
(make-instance (matrix-type dim-x dim-y type)
|
||||
:dimensions dims
|
||||
:array (make-array (* dim-x dim-y) :element-type type))))
|
||||
|
||||
(defun mat-id (dim type)
|
||||
(let ((m (mat-null dim type)))
|
||||
(loop for i below dim
|
||||
do (setf (aref mat i i) ident-elt))
|
||||
mat))
|
||||
do (setf (mref m i i) (coerce 1 type)))
|
||||
m))
|
||||
|
||||
(defun setrow (mat subscript vec)
|
||||
(loop for i below (array-dimension vec 0)
|
||||
do (setf (aref mat subscript i) (aref vec i)))
|
||||
mat)
|
||||
(defun clone-matrix (dims type mat)
|
||||
(let ((m (mat-null dims type)))
|
||||
(loop for i below (reduce #'* (dimension mat))
|
||||
do (setf (mref m i) (mref mat i)))))
|
||||
|
||||
(defun setcol (mat subscript vec)
|
||||
(loop for i below (array-dimension vec 0)
|
||||
do (setf (aref mat i subscript) (aref vec i)))
|
||||
mat)
|
||||
(defun make-matrix (dims type &rest attribs)
|
||||
(let* ((m (mat-null dims type))
|
||||
(v (make-displaced-vector (slot-value m 'array))))
|
||||
(loop with i = 0
|
||||
for attr in attribs
|
||||
do (setf i (fill-vector v attr i)))
|
||||
m))
|
||||
|
||||
(defun setdiag (mat vec)
|
||||
(loop for i below (array-dimension vec 0)
|
||||
do (setf (aref mat i i) (aref vec i)))
|
||||
mat)
|
||||
(defmacro mat (&rest attribs)
|
||||
(if (= (length attribs) 1)
|
||||
(once-only ((attrib (first attribs)))
|
||||
`(clone-matrix (dimension ,attrib) (element-type ,attrib) ,attrib))
|
||||
(let ((dim (list '+ 0)) type)
|
||||
(loop for attr in attribs
|
||||
do (progn
|
||||
(unless type
|
||||
(setf type (cond
|
||||
((floatp attr) 'single-float)
|
||||
((integerp attr) 'fixnum))))
|
||||
(if (numberp attr)
|
||||
(setf (cadr dim) (1+ (cadr dim)))
|
||||
(setf dim (append dim (list `(dimension ,attr)))))))
|
||||
`(let* ((len ,dim)
|
||||
(dim-x (floor (sqrt len)))
|
||||
(dim-y (if (= (* dim-x dim-x) len) dim-x (/ len dim-x))))
|
||||
(make-matrix (list dim-x dim-y) ',type ,@attribs)))))
|
||||
|
||||
(defun transpose (mat)
|
||||
(let ((transposed (mat-null (array-element-type mat) (array-dimension mat 1) (array-dimension mat 0))))
|
||||
(loop for i below (array-dimension mat 0)
|
||||
do (loop for j below (array-dimension mat 1)
|
||||
do (setf (aref transposed j i) (aref mat i j))))
|
||||
(defmacro mat2 (&rest attribs)
|
||||
(let (type)
|
||||
(loop for attr in attribs
|
||||
do (unless type
|
||||
(setf type (cond
|
||||
((floatp attr) 'single-float)
|
||||
((integerp attr) 'fixnum)))))
|
||||
`(make-matrix '(2 2) ',type ,@attribs)))
|
||||
|
||||
(defmacro mat3 (&rest attribs)
|
||||
(let (type)
|
||||
(loop for attr in attribs
|
||||
do (unless type
|
||||
(setf type (cond
|
||||
((floatp attr) 'single-float)
|
||||
((integerp attr) 'fixnum)))))
|
||||
`(make-matrix '(3 3) ',type ,@attribs)))
|
||||
|
||||
(defmacro mat4 (&rest attribs)
|
||||
(let (type)
|
||||
(loop for attr in attribs
|
||||
do (unless type
|
||||
(setf type (cond
|
||||
((floatp attr) 'single-float)
|
||||
((integerp attr) 'fixnum)))))
|
||||
`(make-matrix '(4 4) ',type ,@attribs)))
|
||||
|
||||
(defgeneric transpose (m))
|
||||
(defmethod transpose ((m matrix))
|
||||
(let* ((dim-x (first (dimension m)))
|
||||
(dim-y (second (dimension m)))
|
||||
(transposed (mat-null (list dim-y dim-x) (element-type m))))
|
||||
(loop for i below dim-x
|
||||
do (loop for j below dim-y
|
||||
do (setf (mref transposed j i) (mref m i j))))
|
||||
transposed))
|
||||
|
||||
(defun add-mat (mat-a mat-b)
|
||||
(let* ((mat (mat-null (array-element-type mat-a) (array-dimension mat-a 0) (array-dimension mat-a 1)))
|
||||
(len (array-total-size mat)))
|
||||
(loop for i below len
|
||||
do (setf (row-major-aref mat i) (cl:+ (row-major-aref mat-a i) (row-major-aref mat-b i))))
|
||||
(defgeneric madd (m1 m2))
|
||||
(defmethod madd ((m1 matrix) (m2 matrix))
|
||||
(let ((dim (dimension m1))
|
||||
(type (element-type m1)))
|
||||
(assert (equal dim (dimension m2)))
|
||||
(assert (eq type (element-type m2)))
|
||||
(let ((mat (mat-null dim (element-type m1))))
|
||||
(loop for i below (apply #'* dim)
|
||||
do (setf (aref (slot-value mat 'array) i)
|
||||
(+ (aref (slot-value m1 'array) i)
|
||||
(aref (slot-value m2 'array) i))))
|
||||
mat)))
|
||||
|
||||
(defun m+ (&rest m-list)
|
||||
(reduce #'madd m-list))
|
||||
|
||||
(defgeneric msub (m1 m2))
|
||||
(defmethod msub ((m1 matrix) (m2 matrix))
|
||||
(let ((dim (dimension m1))
|
||||
(type (element-type m1)))
|
||||
(assert (equal dim (dimension m2)))
|
||||
(assert (eq type (element-type m2)))
|
||||
(let ((mat (mat-null dim (element-type m1))))
|
||||
(loop for i below (apply #'* dim)
|
||||
do (setf (aref (slot-value mat 'array) i)
|
||||
(- (aref (slot-value m1 'array) i)
|
||||
(aref (slot-value m2 'array) i))))
|
||||
mat)))
|
||||
|
||||
(defun m- (&rest m-list)
|
||||
(reduce #'msub m-list))
|
||||
|
||||
(defgeneric mmul (m1 m2))
|
||||
(defmethod mmul ((m1 matrix) (m2 number))
|
||||
(let* ((dim (dimension m1))
|
||||
(mat (mat-null dim (element-type m1))))
|
||||
(loop for i below (apply #'* dim)
|
||||
do (setf (aref (slot-value mat 'array) i)
|
||||
(* (aref (slot-value m1 'array) i) m2)))
|
||||
mat))
|
||||
|
||||
(defun sub-mat (mat-a mat-b)
|
||||
(let* ((mat (mat-null (array-element-type mat-a) (array-dimension mat-a 0) (array-dimension mat-a 1)))
|
||||
(len (array-total-size mat)))
|
||||
(loop for i below len
|
||||
do (setf (row-major-aref mat i) (cl:- (row-major-aref mat-a i) (row-major-aref mat-b i))))
|
||||
mat))
|
||||
(defmethod mmul ((m1 number) (m2 matrix))
|
||||
(mmul m2 m1))
|
||||
|
||||
(defun mul-scalar (mat scalar)
|
||||
(let* ((newmat (mat-null (array-element-type mat) (array-dimension mat 0) (array-dimension mat 1)))
|
||||
(len (array-total-size newmat)))
|
||||
(loop for i below len
|
||||
do (setf (row-major-aref newmat i) (cl:* (row-major-aref mat i) scalar)))
|
||||
newmat))
|
||||
(defmethod mmul ((m1 matrix) (m2 matrix))
|
||||
(let ((dim-1 (dimension m1))
|
||||
(dim-2 (dimension m2))
|
||||
(type (element-type m1)))
|
||||
(assert (= (first dim-1) (second dim-2)))
|
||||
(assert (eq type (element-type m2)))
|
||||
(let ((mat (mat-null (list (first dim-2) (second dim-1)) type)))
|
||||
(loop for i below (first dim-2)
|
||||
do (loop for j below (second dim-1)
|
||||
do (setf (mref mat i j)
|
||||
(loop for k below (first dim-1)
|
||||
for l below (second dim-2)
|
||||
sum (* (mref m1 k j) (mref m2 i l))))))
|
||||
mat)))
|
||||
|
||||
(defun mul-mat (mat-a mat-b)
|
||||
(let ((mat (mat-null (array-element-type mat-a) (array-dimension mat-b 1) (array-dimension mat-a 0))))
|
||||
(loop for i below (array-dimension mat 0)
|
||||
do (loop for j below (array-dimension mat 1)
|
||||
do (setf (aref mat i j) (loop for k below (array-dimension mat-a 1)
|
||||
for l below (array-dimension mat-b 0)
|
||||
sum (cl:* (aref mat-a i k) (aref mat-b l j))))))
|
||||
mat))
|
||||
(defmethod mmul ((m1 matrix) (m2 vect))
|
||||
(let* ((dim-1 (dimension m1))
|
||||
(dim-2 (dimension m2))
|
||||
(type (element-type m1)))
|
||||
(assert (= (first dim-1) dim-2))
|
||||
(assert (eq type (element-type m2)))
|
||||
(let ((vec (vec-null (second dim-1) type)))
|
||||
(loop for i below (second dim-1)
|
||||
do (setf (vref vec i)
|
||||
(loop for j below (second dim-1)
|
||||
sum (* (mref m1 j i) (vref m2 j)))))
|
||||
vec)))
|
||||
|
||||
(defun mul-vec (mat vec)
|
||||
(apply #'v::make-vector (cons (array-element-type mat)
|
||||
(loop for i below (array-dimension mat 0)
|
||||
collect (loop for j below (array-dimension mat 1)
|
||||
sum (cl:* (aref mat i j) (aref vec j)))))))
|
||||
(defmethod mmul ((m1 vect) (m2 matrix))
|
||||
(let* ((dim-1 (dimension m1))
|
||||
(dim-2 (dimension m2))
|
||||
(type (element-type m1)))
|
||||
(assert (= dim-1 (second dim-2)))
|
||||
(assert (eq type (element-type m2)))
|
||||
(let ((vec (vec-null (first dim-2) type)))
|
||||
(loop for i below (first dim-2)
|
||||
do (setf (vref vec i)
|
||||
(loop for j below (second dim-2)
|
||||
sum (* (vref m1 j) (mref m2 i j)))))
|
||||
vec)))
|
||||
|
||||
(defun + (&rest mat-list)
|
||||
(reduce #'add-mat mat-list))
|
||||
|
||||
(defun - (&rest mat-list)
|
||||
(reduce #'sub-mat mat-list))
|
||||
|
||||
(defun * (&rest mat-list)
|
||||
(reduce #'(lambda (a b)
|
||||
(cond
|
||||
((not (typep a 'simple-array)) (mul-scalar b a))
|
||||
((not (typep b 'simple-array)) (mul-scalar a b))
|
||||
((= (array-rank b) 1) (mul-vec a b))
|
||||
(t (mul-mat a b))))
|
||||
mat-list))
|
||||
(defun m* (&rest mat-list)
|
||||
(reduce #'mmul mat-list))
|
||||
|
|
|
|||
|
|
@ -1,53 +1,64 @@
|
|||
#|
|
||||
This file is a part of stoe project.
|
||||
Copyright (c) 2014 Renaud Casenave-Péré (renaud@casenave-pere.fr)
|
||||
Copyright (c) 2015 Renaud Casenave-Péré (renaud@casenave-pere.fr)
|
||||
|#
|
||||
|
||||
(in-package :cl-user)
|
||||
(defpackage stoe.maths.quaternion
|
||||
(:nicknames :quaternion :q)
|
||||
(:use :cl)
|
||||
(:shadow :* :conjugate)
|
||||
(:export :quaternion :quat
|
||||
(:use :cl :types :vector :matrix)
|
||||
(:export :quat
|
||||
:from-axis-and-angle
|
||||
:to-float33 :to-float44
|
||||
:* :conjugate))
|
||||
:to-float33 :to-float44))
|
||||
(in-package :stoe.maths.quaternion)
|
||||
|
||||
(deftype quaternion () '(simple-array single-float (4)))
|
||||
(defun make-quaternion (&rest attribs)
|
||||
(let ((q (make-instance 'quaternion
|
||||
:array (make-array (4) :element-type 'single-float))))
|
||||
(loop with i = 0
|
||||
for attr in attribs
|
||||
do (setf i (fill-vector q attr i)))
|
||||
q))
|
||||
|
||||
(defun quat (x y z w)
|
||||
(v:normalize (v:vec x y z w)))
|
||||
(defun from-attribs (x y z w)
|
||||
(let ((q (make-quaternion x y z w)))
|
||||
(normalize q)))
|
||||
|
||||
(defun from-axis-and-angle (vec angle)
|
||||
"Create a quaternion from an axis and an angle."
|
||||
(let ((vec (v:normalize vec))
|
||||
(let ((v (normalize vec))
|
||||
(sin (coerce (sin (/ angle 2)) 'single-float))
|
||||
(cos (coerce (cos (/ angle 2)) 'single-float)))
|
||||
(v:normalize (v:vec (v:* vec sin) cos))))
|
||||
(normalize (make-quaternion (v* v sin) cos))))
|
||||
|
||||
(defun conjugate (quat)
|
||||
(quat (v:- (v:x quat)) (v:- (v:y quat)) (v:- (v:z quat)) (v:w quat)))
|
||||
(defmacro quat (&rest attribs)
|
||||
(let ((len (length attribs)))
|
||||
(assert (or (= len 2) (= len 4)))
|
||||
(case len
|
||||
(4 `(from-attribs ,@attribs))
|
||||
(2 `(from-axis-and-angle ,@attribs)))))
|
||||
|
||||
(defun * (&rest quat-list)
|
||||
(v:normalize (reduce (lambda (q1 q2)
|
||||
(v:with-attributes ((ax x) (ay y) (az z) (aw w)) q1
|
||||
(v:with-attributes ((bx x) (by y) (bz z) (bw w)) q2
|
||||
(quat (cl:- (cl:+ (cl:* aw bx) (cl:* ax bw) (cl:* ay bz)) (cl:* az by))
|
||||
(cl:- (cl:+ (cl:* aw by) (cl:* ay bw) (cl:* az bx)) (cl:* ax bz))
|
||||
(cl:- (cl:+ (cl:* aw bz) (cl:* az bw) (cl:* ax by)) (cl:* ay bx))
|
||||
(cl:- (cl:* aw bw) (cl:* ax bx) (cl:* ay by) (cl:* az bz))))))
|
||||
quat-list)))
|
||||
(defun conjug (quat)
|
||||
(quat (- (x quat)) (- (y quat)) (- (z quat)) (w quat)))
|
||||
|
||||
(defun to-float33 (quat)
|
||||
(v:with-attributes (x y z w) quat
|
||||
(f33:mat (cl:- 1 (cl:* 2 y y) (cl:* 2 z z)) (cl:- (cl:* 2 x y) (cl:* 2 w z)) (cl:+ (cl:* 2 x z) (cl:* 2 w y))
|
||||
(cl:+ (cl:* 2 x y) (cl:* 2 w z)) (cl:- 1 (cl:* 2 x x) (cl:* 2 z z)) (cl:- (cl:* 2 y z) (cl:* 2 w x))
|
||||
(cl:- (cl:* 2 x z) (cl:* 2 w y)) (cl:+ (cl:* 2 y z) (cl:* 2 w x)) (cl:- 1 (cl:* 2 x x) (cl:* 2 y y)))))
|
||||
(defun to-f33 (quat)
|
||||
(with-swizzle (x y z w) quat
|
||||
(let ((2xx (* 2 x x)) (2yy (* 2 y y)) (2zz (* 2 z z))
|
||||
(2xy (* 2 x y)) (2xz (* 2 x z)) (2xw (* 2 x w))
|
||||
(2yz (* 2 y z)) (2yw (* 2 y w))
|
||||
(2zw (* 2 z w)))
|
||||
(mat (- 1 2yy 2zz) (- 2xy 2zw) (+ 2xz 2yw)
|
||||
(+ 2xy 2zw) (- 1 2xx 2zz) (- 2yz 2xw)
|
||||
(- 2xz 2yw) (+ 2yz 2xw) (- 1 2xx 2yy)))))
|
||||
|
||||
(defun to-float44 (quat)
|
||||
(v:with-attributes (x y z w) quat
|
||||
(f44:mat (cl:- 1 (cl:* 2 y y) (cl:* 2 z z)) (cl:- (cl:* 2 x y) (cl:* 2 w z)) (cl:+ (cl:* 2 x z) (cl:* 2 w y)) 0.0
|
||||
(cl:+ (cl:* 2 x y) (cl:* 2 w z)) (cl:- 1 (cl:* 2 x x) (cl:* 2 z z)) (cl:- (cl:* 2 y z) (cl:* 2 w x)) 0.0
|
||||
(cl:- (cl:* 2 x z) (cl:* 2 w y)) (cl:+ (cl:* 2 y z) (cl:* 2 w x)) (cl:- 1 (cl:* 2 x x) (cl:* 2 y y)) 0.0
|
||||
0.0 0.0 0.0 1.0)))
|
||||
(defun to-f44 (quat)
|
||||
(setf (mref (mat4 (to-f33 quat)) 3 3) 1.0))
|
||||
|
||||
(defun q* (&rest q-list)
|
||||
(normalize (reduce (lambda (q1 q2)
|
||||
(with-swizzle ((ax x) (ay y) (az z) (aw w)) q1
|
||||
(with-swizzle ((bx x) (by y) (bz z) (bw w)) q2
|
||||
(quat (- (+ (* aw bx) (* ax bw) (* ay bz)) (* az by))
|
||||
(- (+ (* aw by) (* ay bw) (* az bx)) (* ax bz))
|
||||
(- (+ (* aw bz) (* az bw) (* ax by)) (* ay bx))
|
||||
(- (* aw bw) (* ax bx) (* ay by) (* az bz))))))
|
||||
,q-list)))
|
||||
|
|
|
|||
71
src/maths/types.lisp
Normal file
71
src/maths/types.lisp
Normal file
|
|
@ -0,0 +1,71 @@
|
|||
#|
|
||||
This file is a part of stoe project.
|
||||
Copyright (c) 2015 Renaud Casenave-Péré (renaud@casenave-pere.fr)
|
||||
|#
|
||||
|
||||
(in-package :cl-user)
|
||||
(defpackage stoe.maths.types
|
||||
(:nicknames :types)
|
||||
(:use :cl)
|
||||
(:export :vect :array
|
||||
:int2 :int3 :int4
|
||||
:float2 :float3 :float4
|
||||
:matrix :dimensions
|
||||
:int22 :int33 :int44
|
||||
:float22 :float33 :float44
|
||||
:dimension :element-type))
|
||||
(in-package :stoe.maths.types)
|
||||
|
||||
(defclass vect ()
|
||||
((array :type (array * (*))
|
||||
:initarg :array
|
||||
:documentation "The internal representation of the vector")))
|
||||
|
||||
(defclass int2 (vect)
|
||||
((array :type (array 'fixnum (2)))))
|
||||
|
||||
(defclass int3 (vect)
|
||||
((array :type (array 'fixnum (3)))))
|
||||
|
||||
(defclass int4 (vect)
|
||||
((array :type (array 'fixnum (4)))))
|
||||
|
||||
(defclass float2 (vect)
|
||||
((array :type (array 'single-float (2)))))
|
||||
|
||||
(defclass float3 (vect)
|
||||
((array :type (array 'single-float (3)))))
|
||||
|
||||
(defclass float4 (vect)
|
||||
((array :type (array 'single-float (4)))))
|
||||
|
||||
(defclass quaternion (float4)
|
||||
())
|
||||
|
||||
(defclass matrix ()
|
||||
((dimensions :initarg :dimensions
|
||||
:documentation "The dimensions of the matrix")
|
||||
(array :type (array * (*))
|
||||
:initarg :array
|
||||
:documentation "The internal representation of the matrix")))
|
||||
|
||||
(defclass int22 (matrix)
|
||||
((array :type (array 'fixnum (4)))))
|
||||
|
||||
(defclass int33 (matrix)
|
||||
((array :type (array 'fixnum (9)))))
|
||||
|
||||
(defclass int44 (matrix)
|
||||
((array :type (array 'fixnum (16)))))
|
||||
|
||||
(defclass float22 (matrix)
|
||||
((array :type (array 'single-float (4)))))
|
||||
|
||||
(defclass float33 (matrix)
|
||||
((array :type (array 'single-float (9)))))
|
||||
|
||||
(defclass float44 (matrix)
|
||||
((array :type (array 'single-float (16)))))
|
||||
|
||||
(defgeneric dimension (x))
|
||||
(defgeneric element-type (x))
|
||||
27
src/maths/utils.lisp
Normal file
27
src/maths/utils.lisp
Normal file
|
|
@ -0,0 +1,27 @@
|
|||
#|
|
||||
This file is a part of stoe project.
|
||||
Copyright (c) 2015 Renaud Casenave-Péré (renaud@casenave-pere.fr)
|
||||
|#
|
||||
|
||||
(in-package :cl-user)
|
||||
(defpackage stoe.maths.utils
|
||||
(:use :cl)
|
||||
(:export :lerp :clamp
|
||||
:deg-to-rad :rad-to-deg))
|
||||
(in-package :stoe.maths.utils)
|
||||
|
||||
(defun lerp (a b ratio)
|
||||
"Linear interpolation of `a' and `b' based on `ratio'."
|
||||
(+ (* b ratio) (* a (- 1.0 ratio))))
|
||||
|
||||
(defun deg-to-rad (deg)
|
||||
"Convert an angle from degree to radian."
|
||||
(* deg (/ (* (coerce pi 'single-float) 2.0) 360.0)))
|
||||
|
||||
(defun rad-to-deg (rad)
|
||||
"Convert an angle from radian to degree."
|
||||
(/ rad (/ (* (coerce pi 'single-float) 2.0) 360.0)))
|
||||
|
||||
(defun clamp (number min max)
|
||||
"Clamp a `number' between `min' and `max'."
|
||||
(min max (max min number)))
|
||||
|
|
@ -1,116 +1,294 @@
|
|||
#|
|
||||
This file is a part of stoe project.
|
||||
Copyright (c) 2014 Renaud Casenave-Péré (renaud@casenave-pere.fr)
|
||||
Copyright (c) 2015 Renaud Casenave-Péré (renaud@casenave-pere.fr)
|
||||
|#
|
||||
|
||||
(in-package :cl-user)
|
||||
(defpackage stoe.maths.vector
|
||||
(:nicknames :vector :v)
|
||||
(:use :cl)
|
||||
(:shadow :+ :- :* :/ :length)
|
||||
(:export :vec :vec-int :x :y :z :w
|
||||
:swizzle :with-attributes
|
||||
:+ :- :* :/
|
||||
:lengthsq :length
|
||||
(:use :cl :alexandria :types)
|
||||
(:export :vref
|
||||
:with-swizzle
|
||||
:vec-null
|
||||
:make-vector
|
||||
:vec :vec2 :vec3 :vec4
|
||||
:v+ :v- :v* :v/
|
||||
:vlengthsq :vlength
|
||||
:normalize :safe-normalize))
|
||||
(in-package :stoe.maths.vector)
|
||||
|
||||
(defun make-vector (type components)
|
||||
(let ((dim (cl:length components)))
|
||||
(make-array dim :element-type type :initial-contents (loop for i in components
|
||||
collect (coerce i type)))))
|
||||
(defmethod dimension ((v vect)) (array-dimension (slot-value v 'array) 0))
|
||||
(defmethod dimension ((v int2)) 2)
|
||||
(defmethod dimension ((v int3)) 3)
|
||||
(defmethod dimension ((v int4)) 4)
|
||||
(defmethod dimension ((v float2)) 2)
|
||||
(defmethod dimension ((v float3)) 3)
|
||||
(defmethod dimension ((v float4)) 4)
|
||||
|
||||
(defun decompose (&rest components)
|
||||
"Decompose a list of potential vectors into a single list."
|
||||
(reduce #'append (mapcar (lambda (attr)
|
||||
(if (typep attr 'sequence)
|
||||
(coerce attr 'list)
|
||||
(list attr)))
|
||||
components)))
|
||||
(defmethod element-type ((v vect)) (array-element-type (slot-value v 'array)))
|
||||
(defmethod element-type ((v int2)) 'fixnum)
|
||||
(defmethod element-type ((v int3)) 'fixnum)
|
||||
(defmethod element-type ((v int4)) 'fixnum)
|
||||
(defmethod element-type ((v float2)) 'single-float)
|
||||
(defmethod element-type ((v float3)) 'single-float)
|
||||
(defmethod element-type ((v float4)) 'single-float)
|
||||
|
||||
(defmacro vec (&rest components)
|
||||
`(make-vector 'single-float (decompose ,@components)))
|
||||
(defun vref (v subscript)
|
||||
(aref (slot-value v 'array) subscript))
|
||||
(defun set-vref (v subscript x)
|
||||
(setf (aref (slot-value v 'array) subscript) x))
|
||||
(defsetf vref set-vref)
|
||||
|
||||
(defmacro vec-int (&rest components)
|
||||
`(make-vector 'fixnum (decompose ,@components)))
|
||||
(defgeneric fill-vector (v attr subscript))
|
||||
(defmethod fill-vector (v attr subscript)
|
||||
(setf (vref v subscript) attr)
|
||||
(1+ subscript))
|
||||
|
||||
(defun x (vec) (if (> (array-dimension vec 0) 0) (aref vec 0) (coerce 0 (array-element-type vec))))
|
||||
(defun y (vec) (if (> (array-dimension vec 0) 1) (aref vec 1) (coerce 0 (array-element-type vec))))
|
||||
(defun z (vec) (if (> (array-dimension vec 0) 2) (aref vec 2) (coerce 0 (array-element-type vec))))
|
||||
(defun w (vec) (if (> (array-dimension vec 0) 3) (aref vec 3) (coerce 0 (array-element-type vec))))
|
||||
(defmethod fill-vector (v (attr vect) subscript)
|
||||
(loop for i from 0 below (dimension attr)
|
||||
for j from subscript
|
||||
do (setf (vref v j) (vref attr i))
|
||||
finally (return (1+ j))))
|
||||
|
||||
(defmacro swizzle (vec attributes)
|
||||
(let* ((name (symbol-name attributes))
|
||||
(len (cl:length name)))
|
||||
`(make-array ,len :element-type (array-element-type ,vec)
|
||||
:initial-contents (list ,@(loop for x being the element of name
|
||||
collect `(,(intern (concatenate 'string `(,x)) 'stoe.maths.vector)
|
||||
,vec))))))
|
||||
(defun vect-type (dim type)
|
||||
(case dim
|
||||
(2 (case type (single-float 'float2) (fixnum 'int2) (otherwise 'vect)))
|
||||
(3 (case type (single-float 'float3) (fixnum 'int3) (otherwise 'vect)))
|
||||
(4 (case type (single-float 'float4) (fixnum 'int4) (otherwise 'vect)))
|
||||
(otherwise 'vect)))
|
||||
|
||||
(defmacro with-attributes (attr-list vec &body body)
|
||||
"Binds a list of variables with x y z w or some swizzled vector for use in `body'."
|
||||
(defun vec-null (dim type)
|
||||
(make-instance (vect-type dim type)
|
||||
:array (make-array dim :element-type type)))
|
||||
|
||||
(defun make-vector (dim type &rest attribs)
|
||||
(let ((v (vec-null dim type)))
|
||||
(loop with i = 0
|
||||
for attr in attribs
|
||||
do (setf i (fill-vector v attr i)))
|
||||
v))
|
||||
|
||||
(defun make-displaced-vector (array &optional (index 0) dim)
|
||||
(let ((dim (or dim (array-dimension array 0)))
|
||||
(type (array-element-type array)))
|
||||
(make-instance (vect-type dim type)
|
||||
:array (make-array dim :element-type type :displaced-to array
|
||||
:displaced-index-offset index))))
|
||||
|
||||
(defmacro vec (&rest attribs)
|
||||
(let ((dim (list '+ 0)) type)
|
||||
(loop for attr in attribs
|
||||
do (progn
|
||||
(unless type
|
||||
(setf type (cond
|
||||
((floatp attr) 'single-float)
|
||||
((integerp attr) 'fixnum))))
|
||||
(if (numberp attr)
|
||||
(setf (cadr dim) (1+ (cadr dim)))
|
||||
(setf dim (append dim (list `(dimension ,attr)))))))
|
||||
`(make-vector ,(if (eq (cddr dim) nil) (cadr dim) dim) ',type ,@attribs)))
|
||||
|
||||
(defmacro vec2 (&rest attribs)
|
||||
(let (type)
|
||||
(loop for attr in attribs
|
||||
do (unless type
|
||||
(setf type (cond
|
||||
((floatp attr) 'single-float)
|
||||
((integerp attr) 'fixnum)))))
|
||||
`(make-vector 2 ',type ,@attribs)))
|
||||
|
||||
(defmacro vec3 (&rest attribs)
|
||||
(let (type)
|
||||
(loop for attr in attribs
|
||||
do (unless type
|
||||
(setf type (cond
|
||||
((floatp attr) 'single-float)
|
||||
((integerp attr) 'fixnum)))))
|
||||
`(make-vector 3 ',type ,@attribs)))
|
||||
|
||||
(defmacro vec4 (&rest attribs)
|
||||
(let (type)
|
||||
(loop for attr in attribs
|
||||
do (unless type
|
||||
(setf type (cond
|
||||
((floatp attr) 'single-float)
|
||||
((integerp attr) 'fixnum)))))
|
||||
`(make-vector 4 ',type ,@attribs)))
|
||||
|
||||
(defmacro defswizzle (attribs)
|
||||
(labels ((index (attr)
|
||||
(case attr
|
||||
(#\X 0)
|
||||
(#\Y 1)
|
||||
(#\Z 2)
|
||||
(#\W 3)))
|
||||
(ref-vect (v dim x neutral)
|
||||
(if (or (numberp dim) (= (index x) 0))
|
||||
(if (or (= (index x) 0) (> dim (index x)))
|
||||
(list 'vref v (index x))
|
||||
neutral)
|
||||
`(if (> ,dim ,(index x))
|
||||
(vref ,v ,(index x))
|
||||
,neutral))))
|
||||
(let* ((name (symbol-name attribs))
|
||||
(len (length name)))
|
||||
`(progn
|
||||
(defgeneric ,attribs (v))
|
||||
(defmethod ,attribs ((v vect))
|
||||
,(if (< len 2)
|
||||
(ref-vect 'v '(dimension v) (char name 0) '(coerce 0 (element-type v)))
|
||||
`(make-vector ,len (element-type v)
|
||||
,@(loop for x across name
|
||||
collect (ref-vect 'v '(dimension v) x '(coerce 0 (element-type v)))))))
|
||||
,@(loop for cls in '(int2 int3 int4 float2 float3 float4)
|
||||
for type in '(fixnum fixnum fixnum single-float single-float single-float)
|
||||
for dim in '(2 3 4 2 3 4)
|
||||
for neutral in '(0 0 0 0.0 0.0 0.0)
|
||||
collect (list 'defmethod attribs `((v ,cls))
|
||||
(if (< len 2)
|
||||
(ref-vect 'v dim (char name 0) neutral)
|
||||
`(make-vector ,len ',type
|
||||
,@(loop for x across name
|
||||
collect (ref-vect 'v dim x neutral))))))
|
||||
(export ',attribs)))))
|
||||
|
||||
(defswizzle x)
|
||||
(defswizzle y)
|
||||
(defswizzle z)
|
||||
(defswizzle w)
|
||||
(defswizzle xy)
|
||||
(defswizzle xz)
|
||||
(defswizzle xw)
|
||||
(defswizzle yz)
|
||||
(defswizzle yw)
|
||||
(defswizzle zw)
|
||||
(defswizzle xyz)
|
||||
(defswizzle xyw)
|
||||
(defswizzle xzw)
|
||||
(defswizzle yzw)
|
||||
(defswizzle xyzw)
|
||||
|
||||
(defmacro with-swizzle (attr-list v &body body)
|
||||
"Binds a list of variables with x y z w or some swizzled vector for use in BODY"
|
||||
`(let ,(mapcar (lambda (attr)
|
||||
(let* ((var (if (listp attr) (car attr) attr))
|
||||
(sym (symbol-name (if (listp attr) (cadr attr) attr)))
|
||||
(sym-len (cl:length sym)))
|
||||
(if (> sym-len 1)
|
||||
(list var `(swizzle ,vec ,(intern sym 'vector)))
|
||||
(list var `(,(intern sym 'vector) ,vec)))))
|
||||
(let* ((var (if (listp attr) (first attr) attr))
|
||||
(sym (symbol-name (if (listp attr) (second attr) attr))))
|
||||
(list var `(,sym ,v))))
|
||||
attr-list)
|
||||
,@body))
|
||||
|
||||
(defun op-scalar (fun vec scalar)
|
||||
(map (type-of vec) #'(lambda (attr) (funcall fun attr scalar)) vec))
|
||||
(defgeneric vadd (v1 v2))
|
||||
(defmethod vadd ((v vect) (s number))
|
||||
(let ((vec (make-vector (dimension v) (element-type v))))
|
||||
(loop for i from 0 below (dimension v)
|
||||
do (setf (vref vec i) (+ (vref v i) s)))))
|
||||
|
||||
(defun op-vec (fun vec-a vec-b)
|
||||
(map (type-of vec-a) fun vec-a vec-b))
|
||||
(defmethod vadd ((s number) (v vect))
|
||||
(let ((vec (make-vector (dimension v) (element-type v))))
|
||||
(loop for i from 0 below (dimension v)
|
||||
do (setf (vref vec i) (+ s (vref v i))))))
|
||||
|
||||
(defun + (&rest vec-list)
|
||||
(reduce #'(lambda (a b)
|
||||
(cond
|
||||
((not (typep a 'simple-array)) (op-scalar #'cl:+ b a))
|
||||
((not (typep b 'simple-array)) (op-scalar #'cl:+ a b))
|
||||
(t (op-vec #'cl:+ a b))))
|
||||
vec-list))
|
||||
(defmethod vadd ((v1 vect) (v2 vect))
|
||||
(let ((v (make-vector (dimension v1) (element-type v1))))
|
||||
(loop for i from 0 below (dimension v1)
|
||||
do (setf (vref v i) (+ (vref v1 i) (vref v2 i))))))
|
||||
|
||||
(defun - (&rest vec-list)
|
||||
(if (= (cl:length vec-list) 1)
|
||||
(let ((vec (car vec-list)))
|
||||
(map (type-of vec) #'cl:- vec))
|
||||
(reduce #'(lambda (a b)
|
||||
(cond
|
||||
((not (typep a 'simple-array)) (op-scalar #'cl:- b a))
|
||||
((not (typep b 'simple-array)) (op-scalar #'cl:- a b))
|
||||
(t (op-vec #'cl:- a b))))
|
||||
vec-list)))
|
||||
(defmethod vadd ((v1 float2) (v2 float2))
|
||||
(make-vector 2 'single-float
|
||||
(+ (x v1) (x v2))
|
||||
(+ (y v1) (y v2))))
|
||||
|
||||
(defun * (&rest vec-list)
|
||||
(reduce #'(lambda (a b)
|
||||
(cond
|
||||
((not (typep a 'simple-array)) (op-scalar #'cl:* b a))
|
||||
((not (typep b 'simple-array)) (op-scalar #'cl:* a b))
|
||||
(t (op-vec #'cl:* a b))))
|
||||
vec-list))
|
||||
(defun v+ (&rest v-list)
|
||||
(reduce #'vadd v-list))
|
||||
|
||||
(defun / (&rest vec-list)
|
||||
(reduce #'(lambda (a b)
|
||||
(cond
|
||||
((not (typep a 'simple-array)) (op-scalar #'cl:/ b a))
|
||||
((not (typep b 'simple-array)) (op-scalar #'cl:/ a b))
|
||||
(t (op-vec #'cl:/ a b))))
|
||||
vec-list))
|
||||
(defgeneric vsub (v1 v2))
|
||||
(defmethod vsub ((v vect) (s number))
|
||||
(let ((vec (make-vector (dimension v) (element-type v))))
|
||||
(loop for i from 0 below (dimension v)
|
||||
do (setf (vref vec i) (- (vref v i) s)))))
|
||||
|
||||
(defun lengthsq (vec)
|
||||
(reduce #'cl:+ (map 'list #'(lambda (x) (cl:* x x)) vec)))
|
||||
(defmethod vsub ((s number) (v vect))
|
||||
(let ((vec (make-vector (dimension v) (element-type v))))
|
||||
(loop for i from 0 below (dimension v)
|
||||
do (setf (vref vec i) (- s (vref v i))))))
|
||||
|
||||
(defun length (vec)
|
||||
(sqrt (lengthsq vec)))
|
||||
(defmethod vsub ((v1 vect) (v2 vect))
|
||||
(let ((v (make-vector (dimension v1) (element-type v1))))
|
||||
(loop for i from 0 below (dimension v1)
|
||||
do (setf (vref v i) (- (vref v1 i) (vref v2 i))))))
|
||||
|
||||
(defun normalize (vec)
|
||||
(/ vec (length vec)))
|
||||
(defmethod vsub ((v1 float2) (v2 float2))
|
||||
(make-vector 2 'single-float
|
||||
(- (x v1) (x v2))
|
||||
(- (y v1) (y v2))))
|
||||
|
||||
(defun safe-normalize (vec &optional default)
|
||||
(let ((lensq (lengthsq vec)))
|
||||
(defun v- (&rest v-list)
|
||||
(reduce #'vsub v-list))
|
||||
|
||||
(defgeneric vmul (v1 v2))
|
||||
(defmethod vmul ((v vect) (s number))
|
||||
(let ((vec (make-vector (dimension v) (element-type v))))
|
||||
(loop for i from 0 below (dimension v)
|
||||
do (setf (vref vec i) (* (vref v i) s)))))
|
||||
|
||||
(defmethod vmul ((s number) (v vect))
|
||||
(let ((vec (make-vector (dimension v) (element-type v))))
|
||||
(loop for i from 0 below (dimension v)
|
||||
do (setf (vref vec i) (* s (vref v i))))))
|
||||
|
||||
(defun v* (&rest v-list)
|
||||
(reduce #'vmul v-list))
|
||||
|
||||
(defgeneric vdiv (v1 v2))
|
||||
(defmethod vdiv ((v vect) (s number))
|
||||
(let ((vec (make-vector (dimension v) (element-type v))))
|
||||
(loop for i from 0 below (dimension v)
|
||||
do (setf (vref vec i) (/ (vref v i) s)))))
|
||||
|
||||
(defmethod vdiv ((s number) (v vect))
|
||||
(let ((vec (make-vector (dimension v) (element-type v))))
|
||||
(loop for i from 0 below (dimension v)
|
||||
do (setf (vref vec i) (/ s (vref v i))))))
|
||||
|
||||
(defun v/ (&rest v-list)
|
||||
(reduce #'vdiv v-list))
|
||||
|
||||
(defgeneric dot (v1 v2))
|
||||
(defmethod dot ((v1 vect) (v2 vect))
|
||||
(loop for i below (dimension v1)
|
||||
sum (* (vref v1 i) (vref v2 i))))
|
||||
|
||||
(defgeneric cross (v1 v2))
|
||||
(defmethod cross ((v1 int3) (v2 int3))
|
||||
(let ((vec (make-vector 3 'fixnum)))
|
||||
(setf (vref vec 0) (- (* (vref v1 1) (vref v2 2))
|
||||
(* (vref v2 1) (vref v1 2))))
|
||||
(setf (vref vec 1) (- (* (vref v1 2) (vref v2 0))
|
||||
(* (vref v2 2) (vref v1 0))))
|
||||
(setf (vref vec 2) (- (* (vref v1 0) (vref v2 1))
|
||||
(* (vref v2 0) (vref v1 1))))))
|
||||
|
||||
(defmethod cross ((v1 float3) (v2 float3))
|
||||
(let ((vec (make-vector 3 'single-float)))
|
||||
(setf (vref vec 0) (- (* (vref v1 1) (vref v2 2))
|
||||
(* (vref v2 1) (vref v1 2))))
|
||||
(setf (vref vec 1) (- (* (vref v1 2) (vref v2 0))
|
||||
(* (vref v2 2) (vref v1 0))))
|
||||
(setf (vref vec 2) (- (* (vref v1 0) (vref v2 1))
|
||||
(* (vref v2 0) (vref v1 1))))))
|
||||
|
||||
(defgeneric vlengthsq (v))
|
||||
(defmethod vlengthsq ((v vect))
|
||||
(reduce #'+ (map 'list (lambda (x) (* x x)) (slot-value v 'array))))
|
||||
|
||||
(defun vlength (v)
|
||||
(sqrt (vlengthsq v)))
|
||||
|
||||
(defun normalize (v)
|
||||
(v/ v (vlength v)))
|
||||
|
||||
(defun safe-normalize (v &optional default)
|
||||
(let ((lensq (vlengthsq v)))
|
||||
(if (zerop lensq)
|
||||
(or default vec)
|
||||
(/ vec (sqrt lensq)))))
|
||||
(or default v)
|
||||
(normalize v))))
|
||||
|
|
|
|||
124
t/maths.lisp
124
t/maths.lisp
|
|
@ -1,68 +1,100 @@
|
|||
#|
|
||||
This file is a part of stoe project.
|
||||
Copyright (c) 2014 Renaud Casenave-Péré (renaud@casenave-pere.fr)
|
||||
Copyright (c) 2015 Renaud Casenave-Péré (renaud@casenave-pere.fr)
|
||||
|#
|
||||
|
||||
(in-package :cl-user)
|
||||
(defpackage stoe-test.maths
|
||||
(:use :cl
|
||||
:stoe
|
||||
:stoe.maths.types
|
||||
:stoe.maths.vector
|
||||
:stoe.maths.matrix
|
||||
:prove))
|
||||
(in-package :stoe-test.maths)
|
||||
|
||||
(defvar *v2* (vec 1 2))
|
||||
(defvar *v3* (vec 3 4 5))
|
||||
(defvar *v4* (vec 6 7 8 9))
|
||||
(defvar *v5* (vec 2.0 3.0))
|
||||
(defvar *v6* (vec 4.0 5.0 6.0))
|
||||
|
||||
(defvar *m22* (mat 1 2
|
||||
3 4))
|
||||
(defvar *m33* (mat 1 2 3
|
||||
4 5 6
|
||||
7 8 9))
|
||||
(defvar *m44* (mat 1 2 3 4
|
||||
5 6 7 8
|
||||
9 10 11 12
|
||||
13 14 15 16))
|
||||
(defvar *m23* (make-matrix '(2 3) 'fixnum 1 3 5 2 4 6))
|
||||
(defvar *m32* (make-matrix '(3 2) 'fixnum 1 4 2 5 3 6))
|
||||
|
||||
(plan 23)
|
||||
|
||||
(diag "Vector Constructor Tests")
|
||||
(is (v:vec 1 2 3) #(1.0 2.0 3.0) "Float Vector Constructor" :test #'equalp)
|
||||
(is (v:vec-int 1 2 3) #(1 2 3) "Integer Vector Constructor" :test #'equalp)
|
||||
(is (f2:vec 2 3) (v:vec 2 3) "Float2 Constructor" :test #'equalp)
|
||||
(is (f3:vec 2 3 4) (v:vec 2 3 4) "Float3 Constructor" :test #'equalp)
|
||||
(is (f4:vec 2 3 4 5) (v:vec 2 3 4 5) "Float4 Constructor" :test #'equalp)
|
||||
(diag "Vector constructor tests")
|
||||
(is (slot-value (vec 1 2 3) 'array) #(1 2 3)
|
||||
"Integer vector constructor" :test #'equalp)
|
||||
|
||||
(defvar *vector2* (f2:vec 2 3))
|
||||
(defvar *vector3* (f3:vec 4 5 6))
|
||||
(defvar *vector4* (f4:vec 7 8 9 10))
|
||||
(is (slot-value (vec 1.0 2.0 3.0) 'array) #(1.0 2.0 3.0)
|
||||
"Float vector constructor" :test #'equalp)
|
||||
|
||||
(diag "Swizzle Tests")
|
||||
(is (v:swizzle *vector4* xy) (f2:vec 7 8) "Swizzle f4:xy" :test #'equalp)
|
||||
(is (v:swizzle *vector2* xyz) (f3:vec 2 3 0) "Swizzle f2:xyz" :test #'equalp)
|
||||
(is (v:swizzle *vector3* xyz) *vector3* "Swizzle f3:xyz (identity)" :test #'equalp)
|
||||
(is (v:swizzle *vector4* wzyx) (f4:vec 10 9 8 7) "Swizzle f4:wzyx (reverse)" :test #'equalp)
|
||||
(is (v:swizzle *vector2* xyxy) (f4:vec 2 3 2 3)
|
||||
"Swizzle f2:xyxy (multiple attributes)" :test #'equalp)
|
||||
(is (vec2 2 3) (vec 2 3) "float2 constructor" :test (lambda (v1 v2)
|
||||
(eq (type-of v1)
|
||||
(type-of v2))))
|
||||
|
||||
(is (vec3 2 3 4) (vec 2 3 4) "float3 constructor" :test (lambda (v1 v2)
|
||||
(eq (type-of v1)
|
||||
(type-of v2))))
|
||||
|
||||
(is (vec4 2 3 4 5) (vec 2 3 4 5) "float4 constructor" :test (lambda (v1 v2)
|
||||
(eq (type-of v1)
|
||||
(type-of v2))))
|
||||
|
||||
(diag "Swizzle tests")
|
||||
(is (slot-value (xy *v4*) 'array) #(6 7) "f4.xy" :test #'equalp)
|
||||
(is (slot-value (xyz *v2*) 'array) #(1 2 0) "f2.xyz" :test #'equalp)
|
||||
(is (slot-value (xyz *v3*) 'array) #(3 4 5) "f3.xyz (identity)" :test #'equalp)
|
||||
(is (slot-value (wzyx *v4*) 'array) #(9 8 7 6) "f4.wzyx (reverse)" :test #'equalp)
|
||||
(is (slot-value (xyxy *v2*) 'array) #(1 2 1 2)
|
||||
"f2.xyxy (multiple attributes)" :test #'equalp)
|
||||
|
||||
(diag "Simple vector operations")
|
||||
(is (v:+ *vector2* (v:swizzle *vector4* xy)) #(9.0 11.0) "Add f2" :test #'equalp)
|
||||
(is (v:- *vector3* *vector3*) #(0.0 0.0 0.0) "Substract f3 to itself" :test #'equalp)
|
||||
(is (v:* *vector4* (v:swizzle *vector2* xyxy)) #(14.0 24.0 18.0 30.0) "Multiply f4" :test #'equalp)
|
||||
(is (v:/ *vector2* (v:swizzle *vector3* xz)) #(0.5 0.5) "Divide f2" :test #'equalp)
|
||||
(is (slot-value (v+ *v2* (xy *v4*)) 'array) #(7 9) "Add f2" :test #'equalp)
|
||||
(is (slot-value (v- *v3* *v3*) 'array) #(0 0 0)
|
||||
"Substract f3 to itself" :test #'equalp)
|
||||
(is (slot-value (v* *v4* (xyxy *v2*)) 'array) #(6 14 8 18)
|
||||
"Multiply f4" :test #'equalp)
|
||||
(is (slot-value (v/ *v5* (xz *v6*)) 'array) #(0.5 0.5) "Divide f2" :test #'equalp)
|
||||
|
||||
(diag "Simple vector / scalar operations")
|
||||
(is (v:+ *vector2* 3) #(5.0 6.0) "Add f2" :test #'equalp)
|
||||
(is (v:- *vector3* 1) #(3.0 4.0 5.0) "Substract f3" :test #'equalp)
|
||||
(is (v:* *vector4* 2) #(14.0 16.0 18.0 20.0) "Multiply f4" :test #'equalp)
|
||||
(is (v:/ *vector2* 5) #(0.4 0.6) "Divide f2" :test #'equalp)
|
||||
(is (slot-value (v+ *v2* 3) 'array) #(4 5) "Add f2" :test #'equalp)
|
||||
(is (slot-value (v- *v3* 1) 'array) #(2 3 4) "Substract f3" :test #'equalp)
|
||||
(is (slot-value (v* *v4* 2) 'array) #(12 14 16 18) "Multiply f4" :test #'equalp)
|
||||
(is (slot-value (v/ *v5* 5) 'array) #(0.4 0.6) "Divide f2" :test #'equalp)
|
||||
|
||||
(diag "Matrix Constructor Tests")
|
||||
(is (m:mat 1 2 3 4 5 6 7 8 9 10 11 12) #2A((1.0 2.0 3.0 4.0) (5.0 6.0 7.0 8.0) (9.0 10.0 11.0 12.0))
|
||||
"Matrix Constructor" :test #'equalp)
|
||||
(diag "Matrix constructor tests")
|
||||
(is (slot-value (mat 1 2 3 4 5 6 7 8 9 10 11 12) 'array)
|
||||
#(1 2 3 4 5 6 7 8 9 10 11 12) "Matrix constructor (array)" :test #'equalp)
|
||||
(is (slot-value (mat 1 2 3 4 5 6 7 8 9 10 11 12) 'dimensions) '(3 4)
|
||||
"Matrix constructor (dimensions)" :test #'equalp)
|
||||
|
||||
(defvar *matrix22* (f22:mat 1 2
|
||||
3 4))
|
||||
(defvar *matrix33* (f33:mat 1 2 3
|
||||
4 5 6
|
||||
7 8 9))
|
||||
(defvar *matrix44* (f44:mat 1 2 3 4
|
||||
5 6 7 8
|
||||
9 10 11 12
|
||||
13 14 15 16))
|
||||
|
||||
(is *matrix22* #2a((1.0 2.0) (3.0 4.0)) "Matrix22 Constructor" :test #'equalp)
|
||||
(is *matrix33* #2a((1.0 2.0 3.0) (4.0 5.0 6.0) (7.0 8.0 9.0)) "Matrix33 Constructor" :test #'equalp)
|
||||
(is *matrix44* #2a((1.0 2.0 3.0 4.0) (5.0 6.0 7.0 8.0) (9.0 10.0 11.0 12.0) (13.0 14.0 15.0 16.0))
|
||||
"Matrix44 Constructor" :test #'equalp)
|
||||
|
||||
(diag "Simple Matrix Operations")
|
||||
(is (m:+ *matrix22* (f22:mat-ident)) #2a((2.0 2.0) (3.0 5.0)) "Add f22" :test #'equalp)
|
||||
(diag "Simple matrix operations")
|
||||
(is (slot-value (m+ *m22* (mat-id 2 'fixnum)) 'array)
|
||||
#(2 2
|
||||
3 5) "Add f22" :test #'equalp)
|
||||
(is (slot-value (m- *m33* (mat 9 8 7
|
||||
6 5 4
|
||||
3 2 1)) 'array)
|
||||
#(-8 -6 -4
|
||||
-2 0 2
|
||||
4 6 8) "Substract f33" :test #'equalp)
|
||||
(is (slot-value (m* *m44* (mat 1 0 0 0
|
||||
0 1 0 0
|
||||
0 0 1 0
|
||||
0 0 0 1)) 'array)
|
||||
#(1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16) "Multiply f44 by id" :test #'equalp)
|
||||
(is (slot-value (m* *m23* *m32) 'array)
|
||||
#(9 19 29 12 26 40 15 33 51) "Multiply f23 by f32" :test #'equalp)
|
||||
|
||||
(finalize)
|
||||
|
|
|
|||
Loading…
Add table
Reference in a new issue