-
Notifications
You must be signed in to change notification settings - Fork 4
/
pmat.lisp
1711 lines (1461 loc) · 68.6 KB
/
pmat.lisp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
766
767
768
769
770
771
772
773
774
775
776
777
778
779
780
781
782
783
784
785
786
787
788
789
790
791
792
793
794
795
796
797
798
799
800
801
802
803
804
805
806
807
808
809
810
811
812
813
814
815
816
817
818
819
820
821
822
823
824
825
826
827
828
829
830
831
832
833
834
835
836
837
838
839
840
841
842
843
844
845
846
847
848
849
850
851
852
853
854
855
856
857
858
859
860
861
862
863
864
865
866
867
868
869
870
871
872
873
874
875
876
877
878
879
880
881
882
883
884
885
886
887
888
889
890
891
892
893
894
895
896
897
898
899
900
901
902
903
904
905
906
907
908
909
910
911
912
913
914
915
916
917
918
919
920
921
922
923
924
925
926
927
928
929
930
931
932
933
934
935
936
937
938
939
940
941
942
943
944
945
946
947
948
949
950
951
952
953
954
955
956
957
958
959
960
961
962
963
964
965
966
967
968
969
970
971
972
973
974
975
976
977
978
979
980
981
982
983
984
985
986
987
988
989
990
991
992
993
994
995
996
997
998
999
1000
(in-package :option-9)
;;; NOTE: TODO: Change all long function names (and docstrings) to
;;; match the commented names by the short names. Also, make the last
;;; few functions starting with pm-create-view-into conform to the new
;;; naming method. Change all references to this API to use the
;;; correct (once they are written) long or short function names as
;;; appropriate. Do the same type of nomenclature change to the
;;; pvec.lisp library too.
;; This matrix library contains function to operate on both generic 4x4
;; matricies and also "transformation matricies" which are 3x3
;; rotation matricies embedded into the upper left hand corner of a 4x
;; matrix incuding a 4x1 matrix representing the translation in the
;; 4th column of the 4x4 matrix.
#+(or (not option-9-optimize-pmat) option-9-debug)
(declaim (optimize (safety 3) (space 0) (speed 0) (debug 3)))
(eval-when (:compile-toplevel :load-toplevel :execute)
(deftype pmat () `(simple-array double-float (16)))
(defstruct (pmat
(:type (vector double-float))
(:constructor make-pmat)
(:constructor pmat (&optional m00 m01 m02 m03
m10 m11 m12 m13
m20 m21 m22 m23
m30 m31 m32 m33)))
(m00 0.0d0 :type double-float)
(m01 0.0d0 :type double-float)
(m02 0.0d0 :type double-float)
(m03 0.0d0 :type double-float)
(m10 0.0d0 :type double-float)
(m11 0.0d0 :type double-float)
(m12 0.0d0 :type double-float)
(m13 0.0d0 :type double-float)
(m20 0.0d0 :type double-float)
(m21 0.0d0 :type double-float)
(m22 0.0d0 :type double-float)
(m23 0.0d0 :type double-float)
(m30 0.0d0 :type double-float)
(m31 0.0d0 :type double-float)
(m32 0.0d0 :type double-float)
(m33 0.0d0 :type double-float))
(defmacro with-pmat-accessors ((prefix-symbol pmat) &body body)
`(with-accessors ((,(make-accessor-symbol prefix-symbol "00") pmat-m00)
(,(make-accessor-symbol prefix-symbol "01") pmat-m01)
(,(make-accessor-symbol prefix-symbol "02") pmat-m02)
(,(make-accessor-symbol prefix-symbol "03") pmat-m03)
(,(make-accessor-symbol prefix-symbol "10") pmat-m10)
(,(make-accessor-symbol prefix-symbol "11") pmat-m11)
(,(make-accessor-symbol prefix-symbol "12") pmat-m12)
(,(make-accessor-symbol prefix-symbol "13") pmat-m13)
(,(make-accessor-symbol prefix-symbol "20") pmat-m20)
(,(make-accessor-symbol prefix-symbol "21") pmat-m21)
(,(make-accessor-symbol prefix-symbol "22") pmat-m22)
(,(make-accessor-symbol prefix-symbol "23") pmat-m23)
(,(make-accessor-symbol prefix-symbol "30") pmat-m30)
(,(make-accessor-symbol prefix-symbol "31") pmat-m31)
(,(make-accessor-symbol prefix-symbol "32") pmat-m32)
(,(make-accessor-symbol prefix-symbol "33") pmat-m33))
,pmat
,@body))
(defmacro with-multiple-pmat-accessors (sbinds &body body)
(if (null sbinds)
`(progn ,@body)
`(with-pmat-accessors ,(car sbinds)
(with-multiple-pmat-accessors ,(cdr sbinds) ,@body)))))
(defun matrix-print (str obj)
"Define a pretty printer for nicely formatted pmats.
NOTE: I must use the pprint-dispatch table instead of PRINT-OBJECT
because pmats aren't a CLASS due to the defstruct definition I am using."
(print-unreadable-object (obj str)
(with-pmat-accessors (m obj)
(format str
"[~A ~A ~A ~A]~% [~A ~A ~A ~A]~% [~A ~A ~A ~A]~% [~A ~A ~A ~A]"
m00 m01 m02 m03
m10 m11 m12 m13
m20 m21 m22 m23
m30 m31 m32 m33))))
;; Priority of 1 given incase other arrays of type single-float (16) are
;; pretty printed. This will enable ours to take precedence.
(set-pprint-dispatch 'pmat 'matrix-print 1)
;;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
(defun pmat-aref (mat row col)
;; linearize the row/column index into a linear index
(the double-float(aref mat (+ (* row 4) col))))
(defun (setf pmat-aref) (new-val mat row col)
;; linearize the row/column index into a linear index.
(setf (the double-float (aref mat (+ (* row 4) col)))
(the double-float new-val)))
;;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
(declaim (ftype (function (pmat) pmat) matrix-identity-into))
(defun matrix-identity-into (dst)
"Fill the matrix DST with an identity matrix."
#+option-9-optimize-pmat (declare (optimize (speed 3) (safety 0)))
(with-pmat-accessors (d dst)
(psetf d00 1.0d0 d01 0.0d0 d02 0.0d0 d03 0.0d0
d10 0.0d0 d11 1.0d0 d12 0.0d0 d13 0.0d0
d20 0.0d0 d21 0.0d0 d22 1.0d0 d23 0.0d0
d30 0.0d0 d31 0.0d0 d32 0.0d0 d33 1.0d0))
dst)
(declaim (ftype (function (pmat) pmat) mii))
(declaim (inline mii))
(defun mii (dst) ;; matrix-identity-into
"Shortname for MATRIX-IDENTITY-INTO."
(matrix-identity-into dst))
;;; ;;;;;;;;
(declaim (ftype (function () pmat) matrix-identity))
(defun matrix-identity ()
"Return a newly allocated identity matrix."
#+option-9-optimize-pmat (declare (optimize (speed 3) (safety 0)))
(matrix-identity-into (pmat)))
(declaim (ftype (function () pmat) mi))
(declaim (inline mi))
(defun mi () ;; matrix-identity
"Shortname for MATRIX-IDENTITY."
#+option-9-optimize-pmat (declare (optimize (speed 3) (safety 0)))
(matrix-identity))
;;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
(declaim (ftype (function (pmat pmat) pmat) matrix-copy-into))
(defun matrix-copy-into (dst src)
"Copy SRC into DST and return DST. DST and SRC may be EQ."
#+option-9-optimize-pmat (declare (optimize (speed 3) (safety 0)))
(with-multiple-pmat-accessors ((d dst) (s src))
(psetf d00 s00 d01 s01 d02 s02 d03 s03
d10 s10 d11 s11 d12 s12 d13 s13
d20 s20 d21 s21 d22 s22 d23 s23
d30 s30 d31 s31 d32 s32 d33 s33))
dst)
(declaim (ftype (function (pmat pmat) pmat) mcpi))
(declaim (inline mcpi))
(defun mcpi (dst src) ;; matrix-copy-into
"Shortname for MATRIX-COPY-INTO."
#+option-9-optimize-pmat (declare (optimize (speed 3) (safety 0)))
(matrix-copy-into dst src))
;;; ;;;;;;;;
(declaim (ftype (function (pmat) pmat) matrix-copy))
(defun matrix-copy (src)
"Return a newly allocated pmat into which SRC was copied."
#+option-9-optimize-pmat (declare (optimize (speed 3) (safety 0)))
(matrix-copy-into (mi) src))
(declaim (ftype (function (pmat) pmat) mcp))
(declaim (inline mcp))
(defun mcp (src) ;; matrix-copy
"Shortname for MATRIX-COPY."
#+option-9-optimize-pmat (declare (optimize (speed 3) (safety 0)))
(matrix-copy src))
;;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
(declaim (ftype (function (pmat pmat &key
(:min-val double-float)
(:max-val double-float))
pmat) matrix-clamp-into))
(defun matrix-clamp-into (dst src &key (min-val least-negative-double-float)
(max-val most-positive-double-float))
"Read all values from the SRC matrix and clamp them between MIN-VAL and
MAX-VAL. DST and SRC may be EQ. Return DST."
#+option-9-optimize-pmat (declare (optimize (speed 3) (safety 0)))
(with-multiple-pmat-accessors ((d dst) (s src))
;; This macro isn't entirely lexically/once-only safe, so don't abuse it.
(macrolet ((clamp (read-place)
`(cond
((< (as-double-float ,read-place)
(as-double-float min-val))
(as-double-float min-val))
((> (as-double-float ,read-place)
(as-double-float max-val))
(as-double-float max-val))
(t
,read-place))))
(psetf d00 (clamp s00)
d01 (clamp s01)
d02 (clamp s02)
d03 (clamp s03)
d10 (clamp s10)
d11 (clamp s11)
d12 (clamp s12)
d13 (clamp s13)
d20 (clamp s20)
d21 (clamp s21)
d22 (clamp s22)
d23 (clamp s23)
d30 (clamp s30)
d31 (clamp s31)
d32 (clamp s32)
d33 (clamp s33)))
dst))
(declaim (ftype (function (pmat pmat &key
(:min-val double-float)
(:max-val double-float))
pmat) mci))
(declaim (inline mci))
(defun mci (dst src &key (min-val least-negative-double-float)
(max-val most-positive-double-float)) ;; matrix-clamp-into
"Shortname for MATRIX-CLAMP-INTO."
#+option-9-optimize-pmat (declare (optimize (speed 3) (safety 0)))
(matrix-clamp-into dst src :min-val min-val :max-val max-val))
;;; ;;;;;;;;
(declaim (ftype (function (pmat &key
(:min-val double-float)
(:max-val double-float))
pmat) matrix-clamp))
(defun matrix-clamp (src &key (min-val least-negative-double-float)
(max-val most-positive-double-float))
"Return a newly allocated matrix that contains the clamped values
from SRC."
#+option-9-optimize-pmat (declare (optimize (speed 3) (safety 0)))
(let ((m (matrix-copy src)))
(matrix-clamp-into m m :min-val min-val :max-val max-val)))
(declaim (ftype (function (pmat &key
(:min-val double-float)
(:max-val double-float))
pmat) mc))
(declaim (inline mc))
(defun mc (src &key (min-val least-negative-double-float)
(max-val most-positive-double-float)) ;; matrix-clamp
"Shortname for MATRIX-CLAMP."
#+option-9-optimize-pmat (declare (optimize (speed 3) (safety 0)))
(matrix-clamp src :min-val min-val :max-val max-val))
;;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
(defun matrix-test-into (dst)
"Construct a test pattern into the 4x4 matrix DST."
#+option-9-optimize-pmat (declare (optimize (speed 3) (safety 0)))
(with-multiple-pmat-accessors ((d dst))
(psetf d00 1d0 d01 2d0 d02 3d0 d03 4d0
d10 5d0 d11 6d0 d12 7d0 d13 8d0
d20 9d0 d21 10d0 d22 11d0 d23 12d0
d30 13d0 d31 14d0 d32 15d0 d33 16d0))
dst)
(defun mtsti (dst) ;; matrix-test-into
"Shortname for MATRIX-TEST-INTO."
#+option-9-optimize-pmat (declare (optimize (speed 3) (safety 0)))
(matrix-test-into dst))
;;; ;;;;;;;;
(defun matrix-test ()
"Return a newly allocated 4x4 matrix that contains a test pattern."
#+option-9-optimize-pmat (declare (optimize (speed 3) (safety 0)))
(let ((dst (pmat)))
(matrix-test-into dst)))
(defun mtst () ;; matrix-test
"Shortname for MATRIX-TEST."
#+option-9-optimize-pmat (declare (optimize (speed 3) (safety 0)))
(matrix-test))
;;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
(declaim (ftype (function (pmat pmat) pmat) matrix-transpose-into))
(declaim (inline matrix-transpose-into))
(defun matrix-transpose-into (dst src)
"Transpose the 4x4 SRC and store it into DST. SRC and DST may be EQ"
#+option-9-optimize-pmat (declare (optimize (speed 3) (safety 0)))
(unless (eq dst src)
(matrix-copy-into dst src))
(with-pmat-accessors (d dst)
(rotatef d10 d01)
(rotatef d20 d02)
(rotatef d30 d03)
(rotatef d21 d12)
(rotatef d31 d13)
(rotatef d32 d23))
dst)
(declaim (ftype (function (pmat pmat) pmat) mtpi))
(declaim (inline mtpi))
(defun mtpi (dst src) ;; matrix-transpose-into
"Shortname for MATRIX-TRANSPOSE-INTO."
#+option-9-optimize-pmat (declare (optimize (speed 3) (safety 0)))
(matrix-transpose-into dst src))
;;; ;;;;;;;;
(declaim (ftype (function (pmat) pmat) matrix-transpose))
(declaim (inline matrix-transpose))
(defun matrix-transpose (src)
"Return a newly allocated 4x4 matrix which is the transpose of SRC"
#+option-9-optimize-pmat (declare (optimize (speed 3) (safety 0)))
(matrix-transpose-into (mi) src))
(declaim (ftype (function (pmat) pmat) mtp))
(declaim (inline mtp))
(defun mtp (src) ;; matrix-transpose
"Shortname for MATRIX-TRANSPOSE."
#+option-9-optimize-pmat (declare (optimize (speed 3) (safety 0)))
(matrix-transpose src))
;;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;; NOTE: This looks scary in that I might be modifying RESULT while
;; still reading from it. Not so. The semantics of PSETF are that all
;; subforms are evaluated and then the assignments happen in any
;; order. So, no assignments can happen before the subforms are
;; finished being evaluated. Hence, I don't have to worry if result EQ
;; mat0 or mat1. It is the same for other uses of PSETF in this file.
(declaim (ftype (function (pmat pmat pmat) pmat) matrix-multiply-into))
(defun matrix-multiply-into (dst mat0 mat1)
"Perform matrix multiply of MAT0 * MAT1 and store into DST. DST can
be EQ to MAT0 or MAT1."
#+option-9-optimize-pmat (declare (optimize (speed 3) (safety 0)))
(with-multiple-pmat-accessors ((d dst) (a mat0) (b mat1))
(psetf d00 (as-double-float
(+ (* a00 b00) (* a01 b10) (* a02 b20) (* a03 b30)))
d10 (as-double-float
(+ (* a10 b00) (* a11 b10) (* a12 b20) (* a13 b30)))
d20 (as-double-float
(+ (* a20 b00) (* a21 b10) (* a22 b20) (* a23 b30)))
d30 (as-double-float
(+ (* a30 b00) (* a31 b10) (* a32 b20) (* a33 b30)))
d01 (as-double-float
(+ (* a00 b01) (* a01 b11) (* a02 b21) (* a03 b31)))
d11 (as-double-float
(+ (* a10 b01) (* a11 b11) (* a12 b21) (* a13 b31)))
d21 (as-double-float
(+ (* a20 b01) (* a21 b11) (* a22 b21) (* a23 b31)))
d31 (as-double-float
(+ (* a30 b01) (* a31 b11) (* a32 b21) (* a33 b31)))
d02 (as-double-float
(+ (* a00 b02) (* a01 b12) (* a02 b22) (* a03 b32)))
d12 (as-double-float
(+ (* a10 b02) (* a11 b12) (* a12 b22) (* a13 b32)))
d22 (as-double-float
(+ (* a20 b02) (* a21 b12) (* a22 b22) (* a23 b32)))
d32 (as-double-float
(+ (* a30 b02) (* a31 b12) (* a32 b22) (* a33 b32)))
d03 (as-double-float
(+ (* a00 b03) (* a01 b13) (* a02 b23) (* a03 b33)))
d13 (as-double-float
(+ (* a10 b03) (* a11 b13) (* a12 b23) (* a13 b33)))
d23 (as-double-float
(+ (* a20 b03) (* a21 b13) (* a22 b23) (* a23 b33)))
d33 (as-double-float
(+ (* a30 b03) (* a31 b13) (* a32 b23) (* a33 b33)))))
dst)
(declaim (ftype (function (pmat pmat pmat) pmat) mmi))
(declaim (inline mmi))
(defun mmi (dst mat0 mat1) ;; matrix-multiply-into
"Shortname for MATRIX-MULTIPLY-INTO."
#+option-9-optimize-pmat (declare (optimize (speed 3) (safety 0)))
(matrix-multiply-into dst mat0 mat1))
;;; ;;;;;;;;
(declaim (ftype (function (pmat pmat) pmat) matrix-multiply))
(defun matrix-multiply (mat0 mat1)
"Perform matrix multiply of MAT0 * MAT1 and return new pmat of result."
#+option-9-optimize-pmat (declare (optimize (speed 3) (safety 0)))
(matrix-multiply-into (pmat) mat0 mat1))
(declaim (ftype (function (pmat pmat) pmat) mm))
(declaim (inline mm))
(defun mm (mat0 mat1) ;; matrix-multiply
"Shortname for MATRIX-MULTIPLY."
#+option-9-optimize-pmat (declare (optimize (speed 3) (safety 0)))
(matrix-multiply mat0 mat1))
;;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
(declaim (ftype (function (pmat double-float pvec) pmat)
matrix-rotate-around-into))
(defun matrix-rotate-around-into (rotation angle axis)
"Store a computed rotation matrix with a (0 0 0) translation vector
into the ROTATION transformation matrix that will rotate around the
vector AXIS by the specified ANGLE. This assumes a right handed
coordinate system. Similar to glRotate()."
#+option-9-optimize-pmat (declare (optimize (speed 3) (safety 0)))
(let ((norm-axis (vnormalize axis))
(c (as-double-float (cos angle)))
(s (as-double-float (sin angle))))
#+option-9-optimize-pmat
(declare (type double-float c s))
(with-pvec-accessors (a norm-axis)
(let* ((1-c (as-double-float (- 1d0 c)))
(xs (as-double-float (* ax s)))
(ys (as-double-float (* ay s)))
(zs (as-double-float (* az s)))
(xx (as-double-float (* ax ax)))
(yy (as-double-float (* ay ay)))
(zz (as-double-float (* az az)))
(xy (as-double-float (* ax ay)))
(xz (as-double-float (* ax az)))
(yz (as-double-float (* ay az)))
(xx*[1-c] (as-double-float (* xx 1-c)))
(yy*[1-c] (as-double-float (* yy 1-c)))
(zz*[1-c] (as-double-float (* zz 1-c)))
(xy*[1-c] (as-double-float (* xy 1-c)))
(xz*[1-c] (as-double-float (* xz 1-c)))
(yz*[1-c] (as-double-float (* yz 1-c))))
#+option-9-optimize-pmat
(declare (type double-float 1-c xs ys zs xx yy zz xy xz yz
xx*[1-c] yy*[1-c] zz*[1-c] xy*[1-c] xz*[1-c] yz*[1-c]))
(with-pmat-accessors (r rotation)
(psetf r00 (as-double-float (+ xx*[1-c] c))
r10 (as-double-float (+ xy*[1-c] zs))
r20 (as-double-float (- xz*[1-c] ys))
r30 0d0
r01 (as-double-float (- xy*[1-c] zs))
r11 (as-double-float (+ yy*[1-c] c))
r21 (as-double-float (+ yz*[1-c] xs))
r31 0d0
r02 (as-double-float (+ xz*[1-c] ys))
r12 (as-double-float (- yz*[1-c] xs))
r22 (as-double-float (+ zz*[1-c] c))
r32 0d0
r03 0d0
r13 0d0
r23 0d0
r33 1d0)))))
rotation)
(declaim (ftype (function (pmat double-float pvec) pmat)
mrai))
(declaim (inline mrai))
(defun mrai (rotation angle axis);; matrix-rotate-around-into
"Shortname for PM-TRFM-ROTATE-AROUND."
#+option-9-optimize-pmat (declare (optimize (speed 3) (safety 0)))
(matrix-rotate-around-into rotation angle axis))
;;; ;;;;;;;;
(declaim (ftype (function (double-float pvec) pmat)
matrix-rotate-around))
(defun matrix-rotate-around (angle axis)
"Allocate and return a transformation matrix with a (0 0 0)
translation vector that will rotate around the vector AXIS by the
specified ANGLE. This assumes a right handed coordinate system."
#+option-9-optimize-pmat (declare (optimize (speed 3) (safety 0)))
(matrix-rotate-around-into (matrix-identity) angle axis))
(declaim (ftype (function (double-float pvec) pmat)
mra))
(declaim (inline mra))
(defun mra (angle axis) ;; matrix-rotate-around
"Shortname for PM-TRFM-ROTATE-AROUND."
#+option-9-optimize-pmat (declare (optimize (speed 3) (safety 0)))
(matrix-rotate-around angle axis))
;;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
(declaim (ftype (function (pmat pvec) pmat) matrix-scale-into))
(declaim (inline matrix-scale-into))
(defun matrix-scale-into (dst pvec)
"Store a transformation matrix into DST that scales the axes by the
respective PVEC amounts. Return DST. Similar to glScale()."
#+option-9-optimize-pmat (declare (optimize (speed 3) (safety 0)))
(matrix-identity-into dst)
(with-pmat-accessors (d dst)
(with-pvec-accessors (p pvec)
(psetf d00 px
d11 py
d22 pz)))
dst)
(declaim (ftype (function (pmat pvec) pmat) msci))
(declaim (inline msci))
(defun msci (dst pvec) ;; matrix-scale-into
"Shortname for MATRIX-SCALE-INTO."
#+option-9-optimize-pmat (declare (optimize (speed 3) (safety 0)))
(matrix-scale-into dst pvec))
;;; ;;;;;;;;
(declaim (ftype (function (pvec) pmat) matrix-scale))
(defun matrix-scale (pvec)
"Return a newly allocated transformation matrix that represents a scaling
in each axis denoted by PVEC. Similar to glScale()."
#+option-9-optimize-pmat (declare (optimize (speed 3) (safety 0)))
(matrix-scale-into (matrix-identity) pvec))
(declaim (ftype (function (pvec) pmat) msc))
(declaim (inline msc))
(defun msc (pvec) ;; matrix-scale
"Shortname for MATRIX-SCALE."
#+option-9-optimize-pmat (declare (optimize (speed 3) (safety 0)))
(matrix-scale pvec))
;;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
(declaim (ftype (function (pmat pmat double-float) pmat) matrix-scale-by-into))
(declaim (inline matrix-scale-by-into))
(defun matrix-scale-by-into (dst src scalar)
"Store a matrix into DST that is SRC scaled by the scalar
SCALAR. Return DST. DST may be EQ to SRC."
#+option-9-optimize-pmat (declare (optimize (speed 3) (safety 0)))
(with-multiple-pmat-accessors ((d dst) (s src))
(psetf d00 (as-double-float (* s00 scalar))
d10 (as-double-float (* s10 scalar))
d20 (as-double-float (* s20 scalar))
d30 (as-double-float (* s30 scalar))
d01 (as-double-float (* s01 scalar))
d11 (as-double-float (* s11 scalar))
d21 (as-double-float (* s21 scalar))
d31 (as-double-float (* s31 scalar))
d02 (as-double-float (* s02 scalar))
d12 (as-double-float (* s12 scalar))
d22 (as-double-float (* s22 scalar))
d32 (as-double-float (* s32 scalar))
d03 (as-double-float (* s03 scalar))
d13 (as-double-float (* s13 scalar))
d23 (as-double-float (* s23 scalar))
d33 (as-double-float (* s33 scalar))))
dst)
(declaim (ftype (function (pmat pmat double-float) pmat) mscbi))
(declaim (inline msci))
(defun mscbi (dst src scalar) ;; matrix-scale-by-into
"Shortname for MATRIX-SCALE-BY-INTO."
#+option-9-optimize-pmat (declare (optimize (speed 3) (safety 0)))
(matrix-scale-by-into dst src scalar))
;;; ;;;;;;;;
(declaim (ftype (function (pmat double-float) pmat) matrix-scale-by))
(defun matrix-scale-by (src scalar)
"Return a newly allocated identity matrix into which is stored SRC
which has been scaled by the scalar SCALAR."
#+option-9-optimize-pmat (declare (optimize (speed 3) (safety 0)))
(matrix-scale-by-into (pmat) src scalar))
(declaim (ftype (function (pmat double-float) pmat) mscb))
(declaim (inline mscb))
(defun mscb (src scalar) ;; matrix-scale-by
"Shortname for MATRIX-SCALE-BY."
#+option-9-optimize-pmat (declare (optimize (speed 3) (safety 0)))
(matrix-scale-by src scalar))
;;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
(declaim (ftype (function (pmat pvec) pmat) matrix-translate-into))
(declaim (inline matrix-translate-into))
(defun matrix-translate-into (dst pvec)
"Store a transformation matrix into DST that translates the
coordinate system by the respective PVEC amounts. Return DST. Similar
to glTranslate()"
#+option-9-optimize-pmat (declare (optimize (speed 3) (safety 0)))
(matrix-identity-into dst)
(with-pmat-accessors (d dst)
(with-pvec-accessors (p pvec)
(psetf d03 px
d13 py
d23 pz)))
dst)
(declaim (ftype (function (pmat pvec) pmat) mtri))
(declaim (inline mtri))
(defun mtri (dst pvec) ;; matrix-translate-into
"Shortname for MATRIX-TRANSLATE-INTO."
#+option-9-optimize-pmat (declare (optimize (speed 3) (safety 0)))
(matrix-translate-into dst pvec))
;;; ;;;;;;;;
(declaim (ftype (function (pvec) pmat) matrix-translate))
(defun matrix-translate (pvec)
"Return a newly allocated transformation matrix that translates the
coordinate system by the respective PVEC amounts. Similar
to glTranslate()"
#+option-9-optimize-pmat (declare (optimize (speed 3) (safety 0)))
(matrix-translate-into (matrix-identity) pvec))
(declaim (ftype (function (pvec) pmat) mtr))
(declaim (inline mtr))
(defun mtr (pvec) ;; matrix-translate
"Shortname for MATRIX-TRANSLATE."
#+option-9-optimize-pmat (declare (optimize (speed 3) (safety 0)))
(matrix-translate pvec))
;;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;; WARNING: This is special to orthonormal basis transformation matricies.
(declaim (ftype (function (pmat pmat) pmat) matrix-invert-trfm-into))
(declaim (inline matrix-invert-trfm-into))
(defun matrix-invert-trfm-into (dst src)
"Invert (specifically an) orthonormal transformation matrix (with a
rotation operator in the upper left 3x3 matrix and a translation
vector in the 4x1 column on the right) SRC and store into DST. DST can
be EQ with SRC. This means 1) store the transpose the 3x3 rotation
matrix contained in the upper left hand of the transformation matrix,
2) store the application of the inverted rotation to the negation of
the 4x1 translation column. Return the DST. This function
will not invert arbitrary 4x4 matricies."
#+option-9-optimize-pmat (declare (optimize (speed 3) (safety 0)))
(unless (eq dst src)
(matrix-copy-into dst src))
(with-pmat-accessors (d dst)
;; Transpose the upper left square 3x3 portion of the rotation matrix
(rotatef d10 d01)
(rotatef d20 d02)
(rotatef d21 d12)
;; Invert the translation by applying the inverted rotation to the
;; negated translation
(psetf d03 (as-double-float
(+ (* d00 (- d03)) (* d01 (- d13)) (* d02 (- d23))))
d13 (as-double-float
(+ (* d10 (- d03)) (* d11 (- d13)) (* d12 (- d23))))
d23 (as-double-float
(+ (* d20 (- d03)) (* d21 (- d13)) (* d22 (- d23))))))
dst)
(declaim (ftype (function (pmat pmat) pmat) minvti))
(declaim (inline minvti))
(defun minvti (dst src) ;; matrix-invert-transform-into
"Shortname for MATRIX-INVERT-TRFM-INTO."
#+option-9-optimize-pmat (declare (optimize (speed 3) (safety 0)))
(matrix-invert-trfm-into dst src))
;;; ;;;;;;;;
(declaim (ftype (function (pmat) pmat) matrix-invert-trfm))
(defun matrix-invert-trfm (src)
"Invert the transformation matrix SRC."
#+option-9-optimize-pmat (declare (optimize (speed 3) (safety 0)))
(matrix-invert-trfm-into (pmat) src))
(declaim (ftype (function (pmat) pmat) minvt))
(declaim (inline minvt))
(defun minvt (src) ;; matrix-invert-transform
"Shortname for MATRIX-INVERT-TRFM."
#+option-9-optimize-pmat (declare (optimize (speed 3) (safety 0)))
(matrix-invert-trfm src))
;;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;; TODO: In order to compile this with optimizations on, I need to:
;; (setf *inline-expansion-limit* 1024) somehow only for this function,
;; maybe EVAL-WHEN?
(declaim (ftype (function (pmat pmat) (values pmat t)) matrix-invert-into))
(defun matrix-invert-into (dst src)
"Invert an arbitrary 4x4 matrix in SRC and put the result into DST.
Return the values of DST and T if the inversion happened, or an identity
matrix and NIL if it couldn't happen."
#+option-9-optimize-pmat (declare (optimize (speed 3) (safety 0)))
;; Hrm, I hope the compiler optimizes this nicely.
;; Gotten from:
;; http://www.cg.info.hiroshima-cu.ac.jp/~miyazaki/knowledge/teche23.html
(with-pmat-accessors (s src)
(let ((det-s
(as-double-float
(- (+ (* s00 s11 s22 s33) (* s00 s12 s23 s31) (* s00 s13 s21 s32)
(* s01 s10 s23 s32) (* s01 s12 s20 s33) (* s01 s13 s22 s30)
(* s02 s10 s21 s33) (* s02 s11 s23 s30) (* s02 s13 s20 s31)
(* s03 s10 s22 s31) (* s03 s11 s20 s32) (* s03 s12 s21 s30))
(* s00 s11 s23 s32) (* s00 s12 s21 s33) (* s00 s13 s22 s31)
(* s01 s10 s22 s33) (* s01 s12 s23 s30) (* s01 s13 s20 s32)
(* s02 s10 s23 s31) (* s02 s11 s20 s33) (* s02 s13 s21 s30)
(* s03 s10 s21 s32) (* s03 s11 s22 s30) (* s03 s12 s20 s31)))))
#+option-9-optimize-pmat (declare (type double-float det-s))
;; Bail if the determinent is 0 or too close to it.
(when (< (as-double-float det-s) (as-double-float *pvec-tol*))
(matrix-identity-into dst)
(return-from matrix-invert-into (values dst NIL)))
;; The determinent exists, so compute the inverse into dst.
(with-pmat-accessors (d dst)
(psetf d00 (as-double-float
(/ (- (+ (* s11 s22 s33) (* s12 s23 s31) (* s13 s21 s32))
(* s11 s23 s32) (* s12 s21 s33) (* s13 s22 s31))
det-s))
d01 (as-double-float
(/ (- (+ (* s01 s23 s32) (* s02 s21 s33) (* s03 s22 s31))
(* s01 s22 s33) (* s02 s23 s31) (* s03 s21 s32))
det-s))
d02 (as-double-float
(/ (- (+ (* s01 s12 s33) (* s02 s13 s31) (* s03 s11 s32))
(* s01 s13 s32) (* s02 s11 s33) (* s03 s12 s31))
det-s))
d03 (as-double-float
(/ (- (+ (* s01 s13 s22) (* s02 s11 s23) (* s03 s12 s21))
(* s01 s12 s23) (* s02 s13 s21) (* s03 s11 s22))
det-s))
d10 (as-double-float
(/ (- (+ (* s10 s23 s32) (* s12 s20 s33) (* s13 s22 s30))
(* s10 s22 s33) (* s12 s23 s30) (* s13 s20 s32))
det-s))
d11 (as-double-float
(/ (- (+ (* s00 s22 s33) (* s02 s23 s30) (* s03 s20 s32))
(* s00 s23 s32) (* s02 s20 s33) (* s03 s22 s30))
det-s))
d12 (as-double-float
(/ (- (+ (* s00 s13 s32) (* s02 s10 s33) (* s03 s12 s30))
(* s00 s12 s33) (* s02 s13 s30) (* s03 s10 s32))
det-s))
d13 (as-double-float
(/ (- (+ (* s00 s12 s23) (* s02 s13 s20) (* s03 s10 s22))
(* s00 s13 s22) (* s02 s10 s23) (* s03 s12 s20))
det-s))
d20 (as-double-float
(/ (- (+ (* s10 s21 s33) (* s11 s23 s30) (* s13 s20 s31))
(* s10 s23 s31) (* s11 s20 s33) (* s13 s21 s30))
det-s))
d21 (as-double-float
(/ (- (+ (* s00 s23 s31) (* s01 s20 s33) (* s03 s21 s30))
(* s00 s21 s33) (* s01 s23 s30) (* s03 s20 s31))
det-s))
d22 (as-double-float
(/ (- (+ (* s00 s11 s33) (* s01 s13 s30) (* s03 s10 s31))
(* s00 s13 s31) (* s01 s10 s33) (* s03 s11 s30))
det-s))
d23 (as-double-float
(/ (- (+ (* s00 s13 s21) (* s01 s10 s23) (* s03 s11 s20))
(* s00 s11 s23) (* s01 s13 s20) (* s03 s10 s21))
det-s))
d30 (as-double-float
(/ (- (+ (* s10 s22 s31) (* s11 s20 s32) (* s12 s21 s30))
(* s10 s21 s32) (* s11 s22 s30) (* s12 s20 s31))
det-s))
d31 (as-double-float
(/ (- (+ (* s00 s21 s32) (* s01 s22 s30) (* s02 s20 s31))
(* s00 s22 s31) (* s01 s20 s32) (* s02 s21 s30))
det-s))
d32 (as-double-float
(/ (- (+ (* s00 s12 s31) (* s01 s10 s32) (* s02 s11 s30))
(* s00 s11 s32) (* s01 s12 s30) (* s02 s10 s31))
det-s))
d33 (as-double-float
(/ (- (+ (* s00 s11 s22) (* s01 s12 s20) (* s02 s10 s21))
(* s00 s12 s21) (* s01 s10 s22) (* s02 s11 s20))
det-s))))
(values dst t))))
(declaim (ftype (function (pmat pmat) (values pmat t)) minvi))
(declaim (inline minvi))
(defun minvi (dst src) ;; matrix-invert-into
"Shortname for MATRIX-INVERT-INTO."
#+option-9-optimize-pmat (declare (optimize (speed 3) (safety 0)))
(matrix-invert-into dst src))
;;; ;;;;;;;;
(declaim (ftype (function (pmat) (values pmat t)) matrix-invert))
(defun matrix-invert (src)
"Allocate a new matrix and place into it the inverse of the arbitrary
4x4 matrix SRC. Return the values of a inverted matrix and T if
the inversion was possible, or an identity matrix and NIL if it wasn't
possible."
#+option-9-optimize-pmat (declare (optimize (speed 3) (safety 0)))
(matrix-invert-into (mi) src))
(declaim (ftype (function (pmat) (values pmat t)) minv))
(declaim (inline minv))
(defun minv (src) ;; matrix-invert
"Shortname for MATRIX-INVERT."
#+option-9-optimize-pmat (declare (optimize (speed 3) (safety 0)))
(matrix-invert src))
;;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;; TODO Work in progress.
(declaim (ftype (function (pmat double-float double-float double-float
double-float double-float double-float) pmat)
matrix-perspective-projection-into))
(declaim (inline matrix-perspective-projection-into))
(defun matrix-perspective-projection-into (dst left right bottom top near far)
"Store a perspective projection matrix into DST described by LEFT, RIGHT,
BOTTOM, TOP, NEAR, and FAR. Similar to glFrustum()."
#+option-9-optimize-pmat (declare (optimize (speed 3) (safety 0)))
(with-pmat-accessors (d dst)
(psetf d00 (/ (* 2d0 near) (- right left))
d10 0d0
d20 0d0
d30 0d0
d01 0d0
d11 (/ (* 2d0 near) (- top bottom))
d21 0d0
d31 0d0
d02 (/ (+ right left) (- right left))
d12 (/ (+ top bottom) (- top bottom))
d22 (- (/ (+ far near) (- far near)))
d32 -1d0
d03 0d0
d13 0d0
d23 (- (/ (* 2d0 far near) (- far near)))
d33 0d0)
dst))
(declaim (ftype (function (pmat double-float double-float double-float
double-float double-float double-float) pmat)
mppi))
(declaim (inline mppi))
(defun mppi (dst left right bottom top near far)
"Shortname for MATRIX-PERSPECTIVE-PROJECTION-INTO."
#+option-9-optimize-pmat (declare (optimize (speed 3) (safety 0)))
(matrix-perspective-projection-into dst left right bottom top near far))
;;; ;;;;;;;;
(defun matrix-perspective-projection (left right bottom top near far)
"Allocate and return a perspective projection matrix described by
LEFT, RIGHT, BOTTOM, TOP, NEAR, and FAR. Similar to glFrustum()."
#+option-9-optimize-pmat (declare (optimize (speed 3) (safety 0)))
(matrix-perspective-projection-into (pmat) left right bottom top near far))
(declaim (ftype (function (double-float double-float double-float double-float
double-float double-float) pmat)
mpp))
(declaim (inline mpp))
(defun mpp (left right bottom top near far)
"Shortname for MATRIX-PERSPECTIVE-PROJECTION."
#+option-9-optimize-pmat (declare (optimize (speed 3) (safety 0)))
(matrix-perspective-projection left right bottom top near far))
;;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
(declaim (ftype (function (pmat double-float double-float double-float
double-float double-float double-float) pmat)
matrix-orthographic-projection-into))
(declaim (inline matrix-orthographic-projection-into))
(defun matrix-orthographic-projection-into (dst left right bottom top near far)
"Store an orthographics projection matrix into DST described by the
LEFT, RIGHT, BOTTOM, TOP, NEAR, and FAR arguments. Similar to glOrtho()."
#+option-9-optimize-pmat (declare (optimize (speed 3) (safety 0)))
(with-pmat-accessors (d dst)
(psetf d00 (as-double-float (/ 2d0 (- right left)))
d10 0d0
d20 0d0
d30 0d0
d01 0d0
d11 (as-double-float (/ 2d0 (- top bottom)))
d21 0d0
d31 0d0
d02 0d0
d12 0d0
d22 (as-double-float (- (/ 2d0 (- far near))))
d32 0d0
d03 (as-double-float (- (/ (+ right left) (- right left))))
d13 (as-double-float (- (/ (+ top bottom) (- top bottom))))
d23 (as-double-float (- (/ (+ far near) (- far near))))
d33 1d0)
dst))
(declaim (ftype (function (pmat double-float double-float double-float
double-float double-float double-float) pmat)
mopi))
(declaim (inline mopi))
(defun mopi (dst left right bottom top near far)
"Shortname for MATRIX-ORTHOGRAPHIC-PROJECTION-INTO."
#+option-9-optimize-pmat (declare (optimize (speed 3) (safety 0)))
(matrix-orthographic-projection-into dst left right bottom top near far))
;;; ;;;;;;;;
(declaim (ftype (function (double-float double-float double-float double-float
double-float double-float) pmat)
matrix-orthographic-projection))
(declaim (inline matrix-orthographic-projection))
(defun matrix-orthographic-projection (left right bottom top near far)
"Allocate and return an orthographic projection matrix defined by
LEFT, RIGHT, BOTTOM, TOP, NEAR, and FAR."
#+option-9-optimize-pmat (declare (optimize (speed 3) (safety 0)))
(matrix-orthographic-projection-into (pmat) left right bottom top near far))
(declaim (ftype (function (double-float double-float double-float double-float
double-float double-float) pmat)
mop))
(declaim (inline mop))
(defun mop (left right bottom top near far)
"Shortname for MATRIX-ORTHOGRAPHIC-PROJECTION."
#+option-9-optimize-pmat (declare (optimize (speed 3) (safety 0)))
(matrix-orthographic-projection left right bottom top near far))
;;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
(declaim (ftype (function ((simple-array float (16)) pmat)
(simple-array float (16)))
matrix-convert-to-opengl-into))
(declaim (inline matrix-convert-to-opengl-into))
(defun matrix-convert-to-opengl-into (ogl src)
"Convert the MAT matrix into OGL, which is a column-major OpenGL
matrix represented as a (simple-array float (16)), and then
return OGL. Precision is lost going from double-float to float here."
#+option-9-optimize-pmat (declare (optimize (speed 3) (safety 0)))
(declare (type (simple-array float (16)) ogl))
(with-pmat-accessors (s src)
(psetf (aref ogl 0) (float s00 1.0)
(aref ogl 1) (float s10 1.0)
(aref ogl 2) (float s20 1.0)
(aref ogl 3) (float s30 1.0)
(aref ogl 4) (float s01 1.0)
(aref ogl 5) (float s11 1.0)
(aref ogl 6) (float s21 1.0)
(aref ogl 7) (float s31 1.0)
(aref ogl 8) (float s02 1.0)
(aref ogl 9) (float s12 1.0)
(aref ogl 10) (float s22 1.0)
(aref ogl 11) (float s32 1.0)
(aref ogl 12) (float s03 1.0)
(aref ogl 13) (float s13 1.0)
(aref ogl 14) (float s23 1.0)
(aref ogl 15) (float s33 1.0)))
ogl)
(declaim (ftype (function ((simple-array float (16)) pmat)
(simple-array float (16)))
mctoi))
(declaim (inline mctoi))
(defun mctoi (ogl src) ;; matrix-convert-to-opengl-into
"Shortname for MATRIX-CONVERT-TO-OPENGL-INTO."
#+option-9-optimize-pmat (declare (optimize (speed 3) (safety 0)))
(matrix-convert-to-opengl-into ogl src))
;;; ;;;;;;;;
(declaim (ftype (function (pmat) (simple-array float (16)))
matrix-convert-to-opengl))
(declaim (inline matrix-convert-to-opengl))
(defun matrix-convert-to-opengl (src)
"Convert the SRC matrix into newly allocated column-major ordered
simple-array float (16) suitable for OpenGL and return it. Precision is lost
going form double-float to float here."
#+option-9-optimize-pmat (declare (optimize (speed 3) (safety 0)))
(let ((ogl (make-array 16 :element-type 'float
:initial-element 0.0)))
(declare (type (simple-array float (16)) ogl))
(matrix-convert-to-opengl-into ogl src)
ogl))
(declaim (ftype (function (pmat) (simple-array float (16))) mcto))
(declaim (inline mcto))