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

(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 (dimension 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)
  (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) (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 (dimension v) (element-type v))))
    (loop for i from 0 below (dimension v)
       do (setf (vref vec i) (+ (vref v i) s)))
    vec))

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

(defmethod vadd ((v1 vect) (v2 vect))
  (let ((vec (make-vector (dimension v1) (element-type v1))))
    (loop for i from 0 below (dimension 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 (dimension v) (element-type v))))
    (loop for i from 0 below (dimension v)
       do (setf (vref vec i) (- (vref v i) s)))
    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))))
    vec))

(defmethod vsub ((v1 vect) (v2 vect))
  (let ((vec (make-vector (dimension v1) (element-type v1))))
    (loop for i from 0 below (dimension 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)))
    (reduce #'vsub v-list :initial-value (vec-null (dimension v) (element-type v)))))

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

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

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

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

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