matrix.lisp

   1
   2 (in-package :cl-tuples)
   3
   4 (def-tuple-type matrix22
   5         :tuple-element-type fast-float
   6         :initial-element 0.0f0
   7         :elements (e00 e01
   8                    e10 e11))
   9
  10 (export-tuple-operations matrix22)
  11
  12 (def-tuple-type matrix33
  13         :tuple-element-type fast-float
  14         :initial-element 0.0f0
  15         :elements (e00 e01 e02
  16                                    e10 e11 e12
  17                                    e20 e21 e22))
  18
  19 (export-tuple-operations matrix33)
  20
  21 (def-tuple-type matrix44
  22         :tuple-element-type fast-float
  23         :initial-element 0.0f0
  24         :elements (e00 e01 e02 e03
  25                                    e10 e11 e12 e13
  26                                    e20 e21 e22 e23
  27                                    e30 e31 e32 e33))
  28
  29 (export-tuple-operations matrix44)
  30
  31 (def-tuple-op transpose-matrix22*
  32     ((mat22 matrix22
  33                                  (e00 e01
  34                                           e10 e11)))
  35         "Return the transpose of the matrix"
  36         (:return matrix22
  37                          (matrix22-values*
  38                           e00 e10
  39                           e01 e11)))
  40
  41 (defmacro matrix-dot (dimension row col)
  42   "Generate the symbols required for a dot product between the row and column of a matrix, assuming accessor symbol is e<row><col>"
  43   (labels
  44           ((make-matrix-element-symbol (mat row col)
  45                  (intern (string-upcase (format nil "e~A~A~A" mat row col)) :cl-tuples)))
  46         (let
  47                 ((col-sym-names
  48                   (loop for row from 0 below dimension
  49                          collect (make-matrix-element-symbol 0 row col)))
  50                  (row-sym-names
  51                   (loop for col from 0 below dimension
  52                          collect (make-matrix-element-symbol 1 row col))))
  53           `(+
  54                 ,@(loop
  55                          for col-sym in col-sym-names
  56                          for row-sym in row-sym-names
  57                          collect `(* ,col-sym ,row-sym))))))
  58
  59
  60 (def-tuple-op transform-vertex2d*
  61         ((mat matrix33 (e00 e01 e02 e10 e11 e12 e20 e21 e22))
  62          (vert vector2d (x y w)))
  63   (:return vertex2d
  64                    (vertex2d-values*
  65                         (+ (* x e00) (* y e01) (* w e02))
  66                         (+ (* x e10) (* y e11) (* w e12))
  67                         (+ (* x e20) (* y e21) (* w e22)))))
  68
  69
  70 (def-tuple-op transform-vector2d*
  71         ((mat matrix33 (e00 e01 e02 e10 e11 e12 e20 e21 e22))
  72          (vec vector2d (x y)))
  73   (:return vector2d
  74                    (vector2d-values*
  75                         (+ (* x e00) (* y e01))
  76                         (+ (* x e10) (* y e11)))))
  77
  78
  79 (def-tuple-op matrix33-product*
  80         ((m0 matrix33 (e000 e001 e002 e010 e011 e012 e020 e021 e022))
  81          (m1 matrix33 (e100 e101 e102 e110 e111 e112 e120 e121 e122)))
  82   (:return matrix33
  83                    (matrix33-values*
  84                         (matrix-dot 3 0 0)
  85                         (matrix-dot 3 0 1)
  86                         (matrix-dot 3 0 2)
  87
  88                         (matrix-dot 3 1 0)
  89                         (matrix-dot 3 1 1)
  90                         (matrix-dot 3 1 2)
  91
  92                         (matrix-dot 3 2 0)
  93                         (matrix-dot 3 2 1)
  94                         (matrix-dot 3 2 2))))
  95
  96
  97 (def-tuple-op transform-vertex3d*
  98         ((mat matrix44
  99                   (e00 e01 e02 e03
 100                            e10 e11 e12 e13
 101                            e20 e21 e22 e23
 102                            e30 e31 e32 e33))
 103          (vert vertex3d (x y z w)))
 104   (:return vertex3d
 105                    (vertex3d-values*
 106                         (+ (* x e00) (* y e01) (* z e02) (* w e03))
 107                         (+ (* x e10) (* y e11) (* z e12) (* w e13))
 108                         (+ (* x e20) (* y e21) (* z e22) (* w e23))
 109                         (+ (* x e30) (* y e31) (* z e32) (* w e33)))))
 110
 111 (def-tuple-op transform-vector3d*
 112         ((mat matrix33
 113                   (e00 e01 e02
 114                            e10 e11 e12
 115                            e20 e21 e22))
 116          (vect vector3d (x y z)))
 117   (:return vector3d
 118                    (vector3d-values*
 119                         (+ (* x e00) (* y e01) (* z e02) )
 120                         (+ (* x e10) (* y e11) (* z e12) )
 121                         (+ (* x e20) (* y e21) (* z e12) ))))
 122
 123 (def-tuple-op transpose-matrix33*
 124     ((mat33 matrix33 (e00 e01 e02 e10 e11 e12 e20 e21 e22)))
 125         "Return the transpose of the matrix"
 126         (:return matrix33
 127                          (matrix33-values*
 128                           e00 e10 e20
 129                           e01 e11 e21
 130                           e02 e12 e22)))
 131
 132 (def-tuple-op print-matrix33*
 133         ((mat matrix44 (e00 e01 e02 e03
 134                                                 e10 e11 e12 e13
 135                                                 e20 e21 e22 e23
 136                                                 e30 e31 e32 e33)))
 137   "Print a 3x3 matrix in a useful format."
 138   (:return (values)
 139                    (format t "~A ~A ~A ~A ~%" e00 e01 e02)
 140                    (format t "~A ~A ~A ~A ~%" e10 e11 e12)
 141                    (format t "~A ~A ~A ~A ~%" e20 e21 e22)))
 142
 143 (def-tuple-op matrix44-product*
 144         ((m0 matrix44 (e000 e001 e002 e003 e010 e011 e012 e013  e020 e021 e022 e023  e030 e031 e032 e033))
 145          (m1 matrix44 (e100 e101 e102 e103 e110 e111 e112 e113  e120 e121 e122 e123  e130 e131 e132 e133)))
 146   (:return matrix44
 147                    (matrix44-values*
 148                         (matrix-dot 4 0 0)
 149                         (matrix-dot 4 0 1)
 150                         (matrix-dot 4 0 2)
 151                         (matrix-dot 4 0 3)
 152
 153                         (matrix-dot 4 1 0)
 154                         (matrix-dot 4 1 1)
 155                         (matrix-dot 4 1 2)
 156                         (matrix-dot 4 1 3)
 157
 158                         (matrix-dot 4 2 0)
 159                         (matrix-dot 4 2 1)
 160                         (matrix-dot 4 2 2)
 161                         (matrix-dot 4 2 3)
 162
 163                         (matrix-dot 4 3 0)
 164                         (matrix-dot 4 3 1)
 165                         (matrix-dot 4 3 2)
 166                         (matrix-dot 4 3 3))))
 167
 168 (def-tuple-op identity-matrix22*
 169     ()
 170   (:return matrix22
 171            (matrix22-key-values
 172             e00 1.0
 173             e11 1.0)))
 174
 175 (def-tuple-op identity-matrix33*
 176     ()
 177   (:return matrix33
 178            (matrix33-key-values
 179             e00 1.0
 180             e11 1.0
 181             e22 1.0)))
 182
 183 (def-tuple-op identity-matrix44*
 184     ()
 185   (:return matrix44
 186            (matrix44-key-values
 187             e00 1.0
 188             e11 1.0
 189             e22 1.0
 190             e33 1.0)))
 191
 192 (def-tuple-op translation-matrix44*
 193         ((tx fast-float)
 194          (ty fast-float)
 195          (tz fast-float))
 196   "Return a matrix that represents a translation transformation"
 197   (:return matrix44
 198                    (matrix44-values*
 199                         1.0f0 0.0f0 0.0f0 tx
 200                         0.0f0 1.0f0 0.0f0 ty
 201                         0.0f0 0.0f0 1.0f0 tz
 202                         0.0f0 0.0f0 0.0f0 1.0f0)))
 203
 204 (def-tuple-op scaling-matrix44*
 205     ((sx #1=fast-float)
 206      (sy #1#)
 207      (sz #1#))
 208   (:return matrix44
 209            (matrix44-key-values
 210             e00 sx
 211             e11 sy
 212             e22 sz
 213             e33 1.0)))
 214
 215 (def-tuple-op vertex3d-translation-matrix44*
 216         ((vert vertex3d (tx ty tz tw)))
 217   "Return a matrix that represents a translation transformation"
 218   (:return matrix44
 219                    (matrix44-values*
 220                         1.0f0 0.0f0 0.0f0 tx
 221                         0.0f0 1.0f0 0.0f0 ty
 222                         0.0f0 0.0f0 1.0f0 tz
 223                         0.0f0 0.0f0 0.0f0 1.0f0)))
 224
 225 (def-tuple-op rotatex-matrix33*
 226     ((rotation fast-float))
 227   "Return a matrix for rotating around the x axis."
 228   (:return matrix33
 229            (let* ((sin (sin rotation))
 230                   (-sin (- sin))
 231                   (cos (cos rotation)))
 232              (matrix33-values*
 233               1.0 0.0 0.0
 234               0.0 cos -sin
 235               0.0 sin cos))))
 236
 237 (def-tuple-op rotatex-matrix44*
 238     ((rotation fast-float))
 239   "Return a matrix for rotating around the x axis."
 240   (:return matrix44
 241            (matrix33-matrix44*
 242             (rotatex-matrix33* rotation))))
 243
 244 (def-tuple-op rotatey-matrix33*
 245     ((rotation fast-float))
 246   "Return a matrix for rotating around the y axis."
 247   (:return matrix33
 248            (let* ((sin (sin rotation))
 249                   (-sin (- sin))
 250                   (cos (cos rotation)))
 251              (matrix33-values*
 252               cos  0.0 sin
 253               0.0  1.0 0.0
 254               -sin 0.0 cos))))
 255
 256 (def-tuple-op rotatey-matrix44*
 257     ((rotation fast-float))
 258   "Return a matrix for rotating around the y axis."
 259   (:return matrix44
 260            (matrix33-matrix44*
 261             (rotatey-matrix33* rotation))))
 262
 263 (def-tuple-op rotatez-matrix33*
 264     ((rotation fast-float))
 265   "Return a matrix for rotating around the z axis."
 266   (:return matrix33
 267            (let* ((sin (sin rotation))
 268                   (-sin (- sin))
 269                   (cos (cos rotation)))
 270              (matrix33-values*
 271               cos -sin 0.0
 272               sin cos  0.0
 273               0.0 0.0  1.0))))
 274
 275 (def-tuple-op rotatez-matrix44*
 276     ((rotation fast-float))
 277   "Return a matrix for rotating around the z axis."
 278   (:return matrix44
 279            (matrix33-matrix44*
 280             (rotatez-matrix33* rotation))))
 281
 282 (def-tuple-op transpose-matrix44*
 283         ((mat44 matrix44
 284                         (e00 e01 e02 e03
 285                                  e10 e11 e12 e13
 286                                  e20 e21 e22 e23
 287                                  e30 e31 e32 e33)))
 288   "Return the transpose of the matrix"
 289   (:return matrix44
 290                    (matrix44-values*
 291                         e00 e10 e20 e30
 292                         e01 e11 e21 e31
 293                         e02 e12 e22 e32
 294                         e03 e13 e23 e33)))
 295
 296 (def-tuple-op make-test-matrix44*
 297         ()
 298   "Return a matrix for testing purposes"
 299   (:return matrix44
 300                    (matrix44-values*
 301                         1.0f0  2.0f0  3.0f0  4.0f0
 302                         5.0f0  6.0f0  7.0f0  8.0f0
 303                         9.0f0  10.0f0 11.0f0 12.0f0
 304                         13.0f0 14.0f0 15.0f0 16.0f0)))
 305
 306
 307 (def-tuple-op print-matrix44*
 308         ((mat matrix44 (e00 e01 e02 e03
 309                                                 e10 e11 e12 e13
 310                                                 e20 e21 e22 e23
 311                                                 e30 e31 e32 e33)))
 312   "Print a matrix in a useful format."
 313   (:return (values)
 314                    (format t "~A ~A ~A ~A ~%" e00 e01 e02 e03)
 315                    (format t "~A ~A ~A ~A ~%" e10 e11 e12 e13)
 316                    (format t "~A ~A ~A ~A ~%" e20 e21 e22 e23)
 317                    (format t "~A ~A ~A ~A ~%" e30 e31 e32 e33)))
 318
 319
 320 (def-tuple-op matrix44-matrix33*
 321         ((mat44 matrix44
 322                         (e00 e01 e02 e03
 323                                  e10 e11 e12 e13
 324                                  e20 e21 e22 e23
 325                                  e30 e31 e32 e33)))
 326   "Convert a 4x4 matrix to a 3x3 matrix"
 327   (:return matrix33
 328                    (matrix33-values* e00 e01 e02 e10 e11 e12 e20 e21 e22)))
 329
 330 (def-tuple-op matrix33-matrix44*
 331         ((mat3 matrix33
 332                    (e00 e01 e02
 333                                 e10 e11 e12
 334                                 e20 e21 e22)))
 335   "Convert a 3x3 matrix to a 4x4 matrix"
 336   (:return matrix44
 337                    (matrix44-values* e00 e01 e02 0.0f0
 338                                           e10 e11 e12 0.0f0
 339                                           e20 e21 e22 0.0f0
 340                                           0.0f0 0.0f0 0.0f0 1.0f0)))
 341
 342 (def-tuple-op vector3d-matrix3d*
 343         ((zvec vector3d (zx zy zz))
 344          (yvec vector3d (yx yy yz)))
 345   "Construct a rotation matrix from 2 vectors"
 346   (:return matrix33
 347                    (with-vector3d
 348                            (vector3d-cross
 349                                 zvec yvec)
 350                            (xx xy xz)
 351                          (matrix33*
 352                           (xx yx zx
 353                                   xy yy zy
 354                                   xz yz zz)))))
 355
 356
 357 (def-tuple-op matrix22-determinant*
 358     ((mat matrix22 #.(tuple-elements 'matrix22)))
 359   (:return fast-float
 360            (- (* e00 e11)
 361               (* e01 e10))))
 362
 363 (def-tuple-op matrix33-determinant*
 364     ((mat matrix33 #.(tuple-elements 'matrix33)))
 365   (:return fast-float
 366            (- (+ (* e00 e11 e22)
 367                  (* e01 e12 e20)
 368                  (* e02 e10 e21))
 369               (* e02 e11 e20)
 370               (* e01 e10 e22)
 371               (* e00 e12 e21))))
 372
 373 (def-tuple-op matrix44-determinant*
 374     ((mat matrix44 #.(tuple-elements 'matrix44)))
 375   (:return fast-float
 376            (let ((t0 (* e00 e22))
 377                  (t1 (* e11 e33))
 378                  (t2 (* e01 e23))
 379                  (t3 (* e12 e30))
 380                  (t4 (* e03 e21))
 381                  (t5 (* e10 e32))
 382                  (t6 (* e13 e31))
 383                  (t7 (* e02 e20)))
 384              (- (+ (* t0 t1)
 385                    (* t2 t3)
 386                    (* t7 t6)
 387                    (* t4 t5))
 388                 (* t3 t4)
 389                 (* t7 t1)
 390                 (* t2 t5)
 391                 (* t0 t6)))))
 392
 393 (def-tuple-op matrix22-scale*
 394     ((x fast-float)
 395      (mat matrix22 #1=#.(tuple-elements 'matrix22)))
 396   (:return matrix22 (multiply-arguments matrix22-values* x #1#)))
 397
 398 (def-tuple-op matrix33-scale*
 399     ((x fast-float)
 400      (mat matrix33 #1=#.(tuple-elements 'matrix33)))
 401   (:return matrix33 (multiply-arguments matrix33-values* x #1#)))
 402
 403 (def-tuple-op matrix44-scale*
 404     ((x fast-float)
 405      (mat matrix44 #1=#.(tuple-elements 'matrix44)))
 406   (:return matrix44 (multiply-arguments matrix44-values* x #1#)))
 407
 408 (def-tuple-op cofactor-matrix22*
 409     ((mat matrix22 #.(tuple-elements 'matrix22)))
 410   (:return matrix22
 411            (matrix22-values*
 412             e11 e10
 413             e01 e00)))
 414
 415 (def-tuple-op cofactor-matrix33*
 416     ((mat matrix33 #.(tuple-elements 'matrix33)))
 417   (:return matrix33
 418            (macrolet ((cofactors ()
 419                         `(matrix33-values*
 420                           ,@(matrix-cofactors 3))))
 421              (cofactors))))
 422
 423 (def-tuple-op cofactor-matrix44*
 424     ((mat matrix44 #.(tuple-elements 'matrix44)))
 425   (:return matrix44
 426            (macrolet ((cofactors ()
 427                         `(matrix44-values*
 428                           ,@(matrix-cofactors 4))))
 429              (cofactors))))
 430
 431 (def-tuple-op inverted-matrix22*
 432     ((mat matrix22 #.(tuple-elements 'matrix22)))
 433   (:return matrix22
 434            (matrix22-scale*
 435             (matrix22-determinant* mat)
 436             (transpose-matrix22*
 437              (cofactor-matrix22* mat)))))
 438
 439 (def-tuple-op inverted-matrix33*
 440     ((mat matrix33 #.(tuple-elements 'matrix33)))
 441   (:return matrix33
 442            (matrix33-scale*
 443             (matrix33-determinant* mat)
 444             (transpose-matrix33*
 445              (cofactor-matrix33* mat)))))
 446
 447 (def-tuple-op inverted-matrix44*
 448     ((mat matrix44 #.(tuple-elements 'matrix44)))
 449   (:return matrix44
 450            (matrix44-scale*
 451             (matrix44-determinant* mat)
 452             (transpose-matrix44*
 453              (cofactor-matrix44* mat)))))