stoe/maths/vector.lisp

#|
  This file is a part of stoe project.
  Copyright (c) 2015 Renaud Casenave-Péré (renaud@casenave-pere.fr)
|#

(uiop:define-package :stoe/maths/vector
    (:use :cl :alexandria :stoe/maths/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)

(defmethod dimensions ((v vect)) (array-dimension (slot-value v 'array) 0))
(defmethod dimensions ((v int2)) 2)
(defmethod dimensions ((v int3)) 3)
(defmethod dimensions ((v int4)) 4)
(defmethod dimensions ((v float2)) 2)
(defmethod dimensions ((v float3)) 3)
(defmethod dimensions ((v float4)) 4)

(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)

(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)

(defgeneric fill-vector (v attr subscript))
(defmethod fill-vector (v attr subscript)
  (setf (vref v subscript) attr)
  (1+ subscript))

(defmethod fill-vector (v (attr vect) subscript)
  (loop for i from 0 below (dimensions attr)
     for j from subscript
     do (setf (vref v j) (vref attr i))
     finally (return (1+ j))))

(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)))

(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)
  (once-only ((attrib (first 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 `(dimensions ,attr)))))))
     `(make-vector ,(if (eq (cddr dim) nil) (cadr dim) dim)
                   ,(if type
                        `',type
                        `(element-type ,attrib))
                   ,attrib ,@(rest attribs)))))

(defmacro vec2 (&rest attribs)
  (once-only ((attrib (first attribs)))
    (let (type)
      (loop for attr in attribs
         do (unless type
              (setf type (cond
                           ((floatp attr) 'single-float)
                           ((integerp attr) 'fixnum)))))
      `(make-vector 2 ,(if type
                           `',type
                           `(element-type ,attrib))
                    ,@attribs))))

(defmacro vec3 (&rest attribs)
  (once-only ((attrib (first attribs)))
    (let (type)
      (loop for attr in attribs
         do (unless type
              (setf type (cond
                           ((floatp attr) 'single-float)
                           ((integerp attr) 'fixnum)))))
      `(make-vector 3 ,(if type
                           `',type
                           `(element-type ,attrib))
                    ,@attribs))))

(defmacro vec4 (&rest attribs)
  (once-only ((attrib (first attribs)))
    (let (type)
      (loop for attr in attribs
         do (unless type
              (setf type (cond
                           ((floatp attr) 'single-float)
                           ((integerp attr) 'fixnum)))))
      `(make-vector 4 ,(if type
                           `',type
                           `(element-type ,attrib))
                    ,@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 '(dimensions v) (char name 0) '(coerce 0 (element-type v)))
               `(make-vector ,len (element-type v)
                             ,@(loop for x across name
                                  collect (ref-vect 'v '(dimensions 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)
(defswizzle xyxy)
(defswizzle wzyx)

(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) (first attr) attr))
                          (sym (if (listp attr) (second attr) attr)))
                     (list var `(,sym ,v))))
                 attr-list)
     ,@body))

(defgeneric vadd (v1 v2))
(defmethod vadd ((v vect) (s number))
  (let ((vec (make-vector (dimensions v) (element-type v))))
    (loop for i from 0 below (dimensions v)
       do (setf (vref vec i) (+ (vref v i) s)))
    vec))

(defmethod vadd ((s number) (v vect))
  (let ((vec (make-vector (dimensions v) (element-type v))))
    (loop for i from 0 below (dimensions v)
       do (setf (vref vec i) (+ s (vref v i))))
    vec))

(defmethod vadd ((v1 vect) (v2 vect))
  (let ((vec (make-vector (dimensions v1) (element-type v1))))
    (loop for i from 0 below (dimensions v1)
       do (setf (vref vec i) (+ (vref v1 i) (vref v2 i))))
    vec))

(defmethod vadd ((v1 float2) (v2 float2))
  (make-vector 2 'single-float
               (+ (x v1) (x v2))
               (+ (y v1) (y v2))))

(defun v+ (&rest v-list)
  (reduce #'vadd v-list))

(defgeneric vsub (v1 v2))
(defmethod vsub ((v vect) (s number))
  (let ((vec (make-vector (dimensions v) (element-type v))))
    (loop for i from 0 below (dimensions v)
       do (setf (vref vec i) (- (vref v i) s)))
    vec))

(defmethod vsub ((s number) (v vect))
  (let ((vec (make-vector (dimensions v) (element-type v))))
    (loop for i from 0 below (dimensions v)
       do (setf (vref vec i) (- s (vref v i))))
    vec))

(defmethod vsub ((v1 vect) (v2 vect))
  (let ((vec (make-vector (dimensions v1) (element-type v1))))
    (loop for i from 0 below (dimensions v1)
       do (setf (vref vec i) (- (vref v1 i) (vref v2 i))))
    vec))

(defmethod vsub ((v1 float2) (v2 float2))
  (make-vector 2 'single-float
               (- (x v1) (x v2))
               (- (y v1) (y v2))))

(defun v- (&rest v-list)
  (let ((v (first v-list))
         (r (rest v-list)))
    (if (null r)
        (vsub (vec-null (dimensions v) (element-type v)) v)
        (reduce #'vsub v-list))))

(defgeneric vmul (v1 v2))
(defmethod vmul ((v vect) (s number))
  (let ((vec (make-vector (dimensions v) (element-type v))))
    (loop for i from 0 below (dimensions v)
       do (setf (vref vec i) (* (vref v i) s)))
    vec))

(defmethod vmul ((s number) (v vect))
  (let ((vec (make-vector (dimensions v) (element-type v))))
    (loop for i from 0 below (dimensions v)
       do (setf (vref vec i) (* s (vref v i))))
    vec))

(defun v* (&rest v-list)
  (reduce #'vmul v-list))

(defgeneric vdiv (v1 v2))
(defmethod vdiv ((v vect) (s number))
  (let ((vec (make-vector (dimensions v) (element-type v))))
    (loop for i from 0 below (dimensions v)
       do (setf (vref vec i) (/ (vref v i) s)))
    vec))

(defmethod vdiv ((s number) (v vect))
  (let ((vec (make-vector (dimensions v) (element-type v))))
    (loop for i from 0 below (dimensions v)
       do (setf (vref vec i) (/ s (vref v i))))
    vec))

(defun v/ (&rest v-list)
  (reduce #'vdiv v-list))

(defgeneric dot (v1 v2))
(defmethod dot ((v1 vect) (v2 vect))
  (loop for i below (dimensions 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 v)
        (normalize v))))