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