forked from psilord/option-9
-
Notifications
You must be signed in to change notification settings - Fork 0
/
pvec.lisp
787 lines (666 loc) · 28.4 KB
/
pvec.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
(in-package :option-9)
#+(or (not option-9-optimize-pvec) option-9-debug)
(declaim (optimize (safety 3) (space 0) (speed 0) (debug 3)))
(defparameter *pvec-tol* (as-double-float 10d-10))
(eval-when (:compile-toplevel :load-toplevel :execute)
(deftype pvec () `(simple-array double-float (3)))
(defstruct (pvec
(:type (vector double-float))
(:constructor make-pvec)
(:constructor pvec (&optional x y z)))
(x 0.0d0 :type double-float)
(y 0.0d0 :type double-float)
(z 0.0d0 :type double-float))
(defmacro with-pvec-accessors ((prefix-symbol pvec) &body body)
`(with-accessors
((,(make-accessor-symbol prefix-symbol "X") pvec-x)
(,(make-accessor-symbol prefix-symbol "Y") pvec-y)
(,(make-accessor-symbol prefix-symbol "Z") pvec-z))
,pvec
,@body))
(defmacro with-multiple-pvec-accessors (sbinds &body body)
(if (null sbinds)
`(progn ,@body)
`(with-pvec-accessors ,(car sbinds)
(with-multiple-pvec-accessors ,(cdr sbinds) ,@body))))
;; NOTE: I must use the pprint-dispatch table to emit nicely formatted
;; pvecs because they aren't a CLASS due to the defstruct definition
;; I am using. So PRINT-OBJECT doesn't work on PVEC types.
(set-pprint-dispatch
'pvec #'(lambda (str pobj)
(with-pvec-accessors (p pobj)
(print-unreadable-object (pobj str)
(format str "[~A ~A ~A]" px py pz))))))
;;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
(declaim (ftype (function (pvec pvec) pvec) vect-copy-into))
(declaim (inline vect-copy-into))
(defun vect-copy-into (dst src)
"Copy the SRC into the DST."
#+option-9-optimize-pvec (declare (optimize (speed 3) (safety 0)))
(with-multiple-pvec-accessors ((d dst) (s src))
(psetf dx sx
dy sy
dz sz))
dst)
(declaim (ftype (function (pvec pvec) pvec) vcopyi))
(declaim (inline vcopyi))
(defun vcopyi (dst src)
"Shortname for VECT-COPY-INTO."
#+option-9-optimize-pvec (declare (optimize (speed 3) (safety 0)))
(vect-copy-into dst src))
;;; ;;;;;;;;
(declaim (ftype (function (pvec) pvec) vect-copy))
(declaim (inline vect-copy))
(defun vect-copy (pv)
"Return a newly allocated copy of PV."
#+option-9-optimize-pvec (declare (optimize (speed 3) (safety 0)))
(vcopyi (pvec) pv))
(declaim (ftype (function (pvec) pvec) vcopy))
(declaim (inline vcopy))
(defun vcopy (pv)
"Sortname for VECT-COPY."
#+option-9-optimize-pvec (declare (optimize (speed 3) (safety 0)))
(vect-copy pv))
;;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
(declaim (ftype (function (pvec) pvec) vect-zero-into))
(declaim (inline vect-zero-into))
(defun vect-zero-into (pv)
"Zero each element of PV."
#+option-9-optimize-pvec (declare (optimize (speed 3) (safety 0)))
(with-pvec-accessors (p pv)
(psetf px 0.0d0
py 0.0d0
pz 0.0d0))
pv)
(declaim (ftype (function (pvec) pvec) vzeroi))
(declaim (inline vzeroi))
(defun vzeroi (pv)
"Shortname for VECT-ZERO-INTO."
#+option-9-optimize-pvec (declare (optimize (speed 3) (safety 0)))
(vect-zero-into pv))
;;; ;;;;;;;;
(declaim (ftype (function () pvec) vect-zero))
(declaim (inline vect-zero))
(defun vect-zero ()
"Allocate a vector with all elements being 0d0 and return it."
#+option-9-optimize-pvec (declare (optimize (speed 3) (safety 0)))
(pvec))
(declaim (ftype (function () pvec) vzero))
(declaim (inline vzero))
(defun vzero ()
"Shortname for VECT-ZERO."
#+option-9-optimize-pvec (declare (optimize (speed 3) (safety 0)))
(vect-zero))
;;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
(declaim (ftype (function (pvec double-float double-float double-float) pvec)
vect-set-into))
(declaim (inline vect-set-into))
(defun vect-set-into (pv x y z)
"Assign X Y Z into PV."
#+option-9-optimize-pvec (declare (optimize (speed 3) (safety 0)))
(with-pvec-accessors (p pv)
(psetf px (as-double-float x)
py (as-double-float y)
pz (as-double-float z)))
pv)
(declaim (ftype (function (pvec double-float double-float double-float) pvec)
vseti))
(declaim (inline vseti))
(defun vseti (pv x y z)
"Shortname for VECT-SET-INTO."
#+option-9-optimize-pvec (declare (optimize (speed 3) (safety 0)))
(vect-set-into pv x y z))
;;; ;;;;;;;;
(declaim (ftype (function (double-float double-float double-float) pvec)
vect-set))
(declaim (inline vect-set))
(defun vect-set (x y z)
"Allocate and return a new pvec and assign its :x :y and :z elements
with X Y and Z."
#+option-9-optimize-pvec (declare (optimize (speed 3) (safety 0)))
(pvec x y z))
(declaim (ftype (function (double-float double-float double-float) pvec)
vset))
(declaim (inline vset))
(defun vset (x y z)
"Shortname for VECT-SET."
#+option-9-optimize-pvec (declare (optimize (speed 3) (safety 0)))
(vect-set x y z))
;;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
(defun vect-get (pv &key (multiple-value t))
"Return either VALUES of the x, y, and z elements, or as a list,
depending on the value of the keyword argument :MULTIPLE-VALUE, which
defaults to T."
#+option-9-optimize-pvec (declare (optimize (speed 3) (safety 0)))
(with-pvec-accessors (p pv)
(if multiple-value
(values
(as-double-float px)
(as-double-float py)
(as-double-float pz))
(list
(as-double-float px)
(as-double-float py)
(as-double-float pz)))))
(defun vget (pv &key (multiple-value t))
"Shortname for VECT-GET."
#+option-9-optimize-pvec (declare (optimize (speed 3) (safety 0)))
(vect-get pv :multiple-value multiple-value))
;;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
(declaim (ftype (function (pvec pvec &key
(:min-val double-float)
(:max-val double-float))
pvec) vect-clamp-into))
(declaim (inline vect-clamp-into))
(defun vect-clamp-into (dst src &key (min-val least-negative-double-float)
(max-val most-positive-double-float))
"Clamp all elements from SRC between (inclusive) MIN-VAL and MAX-VAL
values into DST."
#+option-9-optimize-pvec (declare (optimize (speed 3) (safety 0)))
(with-multiple-pvec-accessors ((d dst) (s src))
(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 dx (clamp sx)
dy (clamp sy)
dz (clamp sz))
dst)))
(declaim (ftype (function (pvec pvec &key
(:min-val double-float)
(:max-val double-float))
pvec) vclampi))
(declaim (inline vclampi))
(defun vclampi (dst src &key (min-val least-negative-double-float)
(max-val most-positive-double-float))
"Shortname for VECT-CLAMP-INTO."
#+option-9-optimize-pvec (declare (optimize (speed 3) (safety 0)))
(vect-clamp-into dst src :min-val min-val :max-val max-val))
;;; ;;;;;;;;
(declaim (ftype (function (pvec &key
(:min-val double-float)
(:max-val double-float))
pvec) vect-clamp))
(declaim (inline vect-clamp))
(defun vect-clamp (src &key (min-val least-negative-double-float)
(max-val most-positive-double-float))
"Allocate a new pvec and return the clamped SRC in it."
#+option-9-optimize-pvec (declare (optimize (speed 3) (safety 0)))
(vclampi (pvec) src :min-val min-val :max-val max-val))
(declaim (ftype (function (pvec &key
(:min-val double-float)
(:max-val double-float))
pvec) vclamp))
(declaim (inline vclamp))
(defun vclamp (src &key (min-val least-negative-double-float)
(max-val most-positive-double-float))
"Shortname for VECT-CLAMP."
#+option-9-optimize-pvec (declare (optimize (speed 3) (safety 0)))
(vect-clamp src :min-val min-val :max-val max-val))
;;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
(declaim (ftype (function (pvec pvec) pvec) vect-negate-into))
(declaim (inline vect-negate-into))
(defun vect-negate-into (dst src)
"Negate the vector in SRC and store into DST. Return DST."
#+option-9-optimize-pvec (declare (optimize (speed 3) (safety 0)))
(with-multiple-pvec-accessors ((d dst) (s src))
(psetf dx (as-double-float (- sx))
dy (as-double-float (- sy))
dz (as-double-float (- sz))))
dst)
(declaim (ftype (function (pvec pvec) pvec) vnegi))
(declaim (inline vnegi))
(defun vnegi (dst src)
"Shortname for VECT-NEGATE-INTO."
#+option-9-optimize-pvec (declare (optimize (speed 3) (safety 0)))
(vect-negate-into dst src))
;;; ;;;;;;;;
(declaim (ftype (function (pvec) pvec) vect-negate))
(declaim (inline vect-negate))
(defun vect-negate (src)
"Store the negation of SRC into a newly created pvec and return it."
#+option-9-optimize-pvec (declare (optimize (speed 3) (safety 0)))
(vnegi (pvec) src))
(declaim (ftype (function (pvec) pvec) vneg))
(declaim (inline vneg))
(defun vneg (src)
"Shortname for VECT-NEGATE."
#+option-9-optimize-pvec (declare (optimize (speed 3) (safety 0)))
(vect-negate src))
;;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
(declaim (ftype (function (pvec pvec) pvec) vect-stabilize-into))
(declaim (inline vect-stabilize-into))
(defun vect-stabilize-into (dst src)
"If any element in SRC is < than *pvec-tol*, force it to 0.0d0 when
storing it into into DST."
#+option-9-optimize-pvec (declare (optimize (speed 3) (safety 0)))
(with-multiple-pvec-accessors ((d dst) (s src))
(macrolet ((stabilize (slot)
`(if (< (as-double-float (abs ,slot))
(as-double-float *pvec-tol*))
0d0
,slot)))
(psetf dx (stabilize sx)
dy (stabilize sy)
dz (stabilize sz))))
dst)
(declaim (ftype (function (pvec pvec) pvec) vstabi))
(declaim (inline vstabi))
(defun vstabi (dst src)
"Shortname for VECT-STABILIZE-INTO."
#+option-9-optimize-pvec (declare (optimize (speed 3) (safety 0)))
(vect-stabilize-into dst src))
;;; ;;;;;;;;
(declaim (ftype (function (pvec) pvec) vect-stabilize))
(declaim (inline vect-stabilize))
(defun vect-stabilize (src)
"Allocate a new pvec and return the stabilized PV in it."
#+option-9-optimize-pvec (declare (optimize (speed 3) (safety 0)))
(vstabi (pvec) src))
(declaim (ftype (function (pvec) pvec) vstab))
(declaim (inline vstab))
(defun vstab (src)
"Shortname for VECT-STABILIZE."
#+option-9-optimize-pvec (declare (optimize (speed 3) (safety 0)))
(vect-stabilize src))
;;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
(declaim (ftype (function (pvec) double-float) vect-norm))
(declaim (inline vect-norm))
(defun vect-norm (src)
"Compute the double-float Euclidean magnitude of SRC and return it."
#+option-9-optimize-pvec (declare (optimize (speed 3) (safety 0)))
;; And remove the compiler note about the boxed double-float I'm trying
;; to return.
#+(and :option-9-optimize-pvec :sbcl)
(declare (sb-ext:muffle-conditions sb-ext:compiler-note))
(with-pvec-accessors (s src)
(the double-float
(sqrt (as-double-float
(+ (* sx sx)
(* sy sy)
(* sz sz)))))))
(declaim (ftype (function (pvec) double-float) vnorm))
(declaim (inline vnorm))
(defun vnorm (src)
"Shortname for VECT-NORM."
#+option-9-optimize-pvec (declare (optimize (speed 3) (safety 0)))
;; And remove the compiler note about the boxed double-float I'm trying
;; to return.
#+(and :option-9-optimize-pvec :sbcl)
(declare (sb-ext:muffle-conditions sb-ext:compiler-note))
(vect-norm src))
;;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
(declaim (ftype (function (pvec pvec) pvec) vect-normalize-into))
(declaim (inline vect-normalize-into))
(defun vect-normalize-into (dst src)
"Normalize SRC and put result into DST. Return DST."
#+option-9-optimize-pvec (declare (optimize (speed 3) (safety 0)))
(let ((inv-mag (the double-float (/ (vnorm src)))))
(with-multiple-pvec-accessors ((d dst) (s src))
(psetf dx (as-double-float (* sx inv-mag))
dy (as-double-float (* sy inv-mag))
dz (as-double-float (* sz inv-mag))))
dst))
(declaim (ftype (function (pvec pvec) pvec) vnormalizei))
(declaim (inline vnormalizei))
(defun vnormalizei (dst src)
"Shortname for VECT-NORMALIZE-INTO."
#+option-9-optimize-pvec (declare (optimize (speed 3) (safety 0)))
(vect-normalize-into dst src))
;;; ;;;;;;;;
(declaim (ftype (function (pvec) pvec) vect-normalize))
(declaim (inline vect-normalize))
(defun vect-normalize (src)
"Allocate a new pvec that contains the normalization of SRC and return it."
#+option-9-optimize-pvec (declare (optimize (speed 3) (safety 0)))
(vnormalizei (pvec) src))
(declaim (ftype (function (pvec) pvec) vnormalize))
(declaim (inline vnormalize))
(defun vnormalize (src)
"Shortname for VECT-NORMALIZE."
#+option-9-optimize-pvec (declare (optimize (speed 3) (safety 0)))
(vect-normalize src))
;;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
(declaim (ftype (function (pvec) t) vect-zero-p))
(defun vect-zero-p (src)
"If all elements are < *pvec-tol* return NIL, otherwise PV."
#+option-9-optimize-pvec (declare (optimize (speed 3) (safety 0)))
(with-pvec-accessors (s src)
(let* ((is-zero-p (and (< (the double-float (abs sx))
(the double-float *pvec-tol*))
(< (the double-float (abs sy))
(the double-float *pvec-tol*))
(< (the double-float (abs sz))
(the double-float *pvec-tol*)))))
(if is-zero-p
src
nil))))
(declaim (ftype (function (pvec) t) vzerop))
(defun vzerop (src)
"Shortname for VECT-ZERO-P."
#+option-9-optimize-pvec (declare (optimize (speed 3) (safety 0)))
(vect-zero-p src))
;;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
(declaim (ftype (function (pvec pvec pvec) pvec) vect-cross-into))
(declaim (inline vect-cross-into))
(defun vect-cross-into (pvn pvu pvv)
"Perform the cross product of PVU x PVV and store into PVN. Return PVN."
#+option-9-optimize-pvec (declare (optimize (speed 3) (safety 0)))
(with-multiple-pvec-accessors ((n pvn) (u pvu) (v pvv))
(psetf nx (as-double-float (- (* uy vz) (* uz vy)))
ny (as-double-float (- (* uz vx) (* ux vz)))
nz (as-double-float (- (* ux vy) (* uy vx)))))
pvn)
(declaim (ftype (function (pvec pvec pvec) pvec) vcrossi))
(declaim (inline vcrossi))
(defun vcrossi (pvn pvu pvv)
"Shortname for VECT-CROSS-INTO."
#+option-9-optimize-pvec (declare (optimize (speed 3) (safety 0)))
(vect-cross-into pvn pvu pvv))
;;; ;;;;;;;;
(declaim (ftype (function (pvec pvec) pvec) vect-cross))
(declaim (inline vect-cross))
(defun vect-cross (pvu pvv)
"Do a cross product between PVU and PVB and return a new pvec of it."
#+option-9-optimize-pvec (declare (optimize (speed 3) (safety 0)))
(vcrossi (pvec) pvu pvv))
(declaim (ftype (function (pvec pvec) pvec) vcross))
(declaim (inline vcross))
(defun vcross (pvu pvv)
"Shortname of VECT-CROSS."
#+option-9-optimize-pvec (declare (optimize (speed 3) (safety 0)))
(vect-cross pvu pvv))
;;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
(declaim (ftype (function (pvec pvec pvec) pvec) vect-add-into))
(declaim (inline vect-add-into))
(defun vect-add-into (pvd pva pvb)
"Vector add PVA and PVB and store into PVD."
#+option-9-optimize-pvec (declare (optimize (speed 3) (safety 0)))
(with-multiple-pvec-accessors ((r pvd) (a pva) (b pvb))
(psetf rx (as-double-float (+ ax bx))
ry (as-double-float (+ ay by))
rz (as-double-float (+ az bz))))
pvd)
(declaim (ftype (function (pvec pvec pvec) pvec) vaddi))
(declaim (inline vaddi))
(defun vaddi (pvd pva pvb)
"Shortname for VECT-ADD-INTO."
#+option-9-optimize-pvec (declare (optimize (speed 3) (safety 0)))
(vect-add-into pvd pva pvb))
;;; ;;;;;;;;
(declaim (ftype (function (pvec pvec) pvec) vect-add))
(declaim (inline vect-add))
(defun vect-add (pva pvb)
"Add two vectors PVA and PVB and return a new pvec with the result."
#+option-9-optimize-pvec (declare (optimize (speed 3) (safety 0)))
(vaddi (pvec) pva pvb))
(declaim (ftype (function (pvec pvec) pvec) vadd))
(declaim (inline vadd))
(defun vadd (pva pvb)
"Shortname for VECT-ADD."
#+option-9-optimize-pvec (declare (optimize (speed 3) (safety 0)))
(vect-add pva pvb))
;;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
(declaim (ftype (function (pvec pvec pvec) pvec) vect-sub-into))
(declaim (inline vect-sub-into))
(defun vect-sub-into (pvd pva pvb)
"Subtract PVB from PVA and store into PVD. Return PVD."
#+option-9-optimize-pvec (declare (optimize (speed 3) (safety 0)))
(with-multiple-pvec-accessors ((r pvd) (a pva) (b pvb))
(psetf rx (as-double-float (- ax bx))
ry (as-double-float (- ay by))
rz (as-double-float (- az bz))))
pvd)
(declaim (ftype (function (pvec pvec pvec) pvec) vsubi))
(declaim (inline vsubi))
(defun vsubi (pvd pva pvb)
"Shortname for VECT-SUB-INTO."
#+option-9-optimize-pvec (declare (optimize (speed 3) (safety 0)))
(vect-sub-into pvd pva pvb))
;;; ;;;;;;;;
(declaim (ftype (function (pvec pvec) pvec) vect-sub))
(declaim (inline vect-sub))
(defun vect-sub (pva pvb)
"Subtract PVB from PVA and return result in a new pvec."
#+option-9-optimize-pvec (declare (optimize (speed 3) (safety 0)))
(vsubi (pvec) pva pvb))
(declaim (ftype (function (pvec pvec) pvec) vsub))
(declaim (inline vsub))
(defun vsub (pva pvb)
"Shortname for VECT-SUB."
#+option-9-optimize-pvec (declare (optimize (speed 3) (safety 0)))
(vect-sub pva pvb))
;;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
(declaim (ftype (function (pvec pvec pvec) pvec) vect-vect-into))
(declaim (inline vect-vect-into))
(defun vect-vect-into (pvd pva pvb)
"Create a vector from point PVA to point PVB and stored into PVD. This
is just a conveniently named vector subtraction in a well known context."
#+option-9-optimize-pvec (declare (optimize (speed 3) (safety 0)))
(vsubi pvd pvb pva))
(declaim (ftype (function (pvec pvec pvec) pvec) vvecti))
(declaim (inline vvecti))
(defun vvecti (pvd pva pvb)
"Shortname for VECT-VECT-INTO."
#+option-9-optimize-pvec (declare (optimize (speed 3) (safety 0)))
(vect-vect-into pvd pva pvb))
;;; ;;;;;;;;
(declaim (ftype (function (pvec pvec) pvec) vect-vect))
(declaim (inline vect-vect))
(defun vect-vect (pva pvb)
"Compute a vector from the point represented in PVA to the point
represented in PVB and return the new pvec vector. This is just a
conveniently named vector subtraction in a well known context."
#+option-9-optimize-pvec (declare (optimize (speed 3) (safety 0)))
(vvecti (pvec) pva pvb))
(declaim (ftype (function (pvec pvec) pvec) vvect))
(declaim (inline vvect))
(defun vvect (pva pvb)
"Shortname for VECT-VECT."
#+option-9-optimize-pvec (declare (optimize (speed 3) (safety 0)))
(vect-vect pva pvb))
;;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
(declaim (ftype (function (pvec pvec double-float) pvec) vect-scale-into))
(declaim (inline vect-scale-into))
(defun vect-scale-into (dst src scale-factor)
"Multiply SRC by the DOUBLE-FLOAT SCALE-FACTOR and store into DST."
#+option-9-optimize-pvec (declare (optimize (speed 3) (safety 0)))
(with-multiple-pvec-accessors ((d dst) (s src))
(psetf dx (as-double-float (* sx scale-factor))
dy (as-double-float (* sy scale-factor))
dz (as-double-float (* sz scale-factor)))
dst))
(declaim (ftype (function (pvec pvec double-float) pvec) vscalei))
(declaim (inline vscalei))
(defun vscalei (dst src scale-factor)
"Shortname for VECT-SCALE-INTO."
#+option-9-optimize-pvec (declare (optimize (speed 3) (safety 0)))
(vect-scale-into dst src scale-factor))
;;; ;;;;;;;;
(declaim (ftype (function (pvec double-float) pvec) vect-scale))
(declaim (inline vect-scale))
(defun vect-scale (src scale-factor)
"Allocate and return a new pvec that contains SRC scaled by the
DOUBLE-FLOAT SCALE-FACTOR."
#+option-9-optimize-pvec (declare (optimize (speed 3) (safety 0)))
(vscalei (pvec) src scale-factor))
(declaim (ftype (function (pvec double-float) pvec) vscale))
(declaim (inline vscale))
(defun vscale (src scale-factor)
"Shortname for VECT-SCALE."
#+option-9-optimize-pvec (declare (optimize (speed 3) (safety 0)))
(vect-scale src scale-factor))
;;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
(declaim (ftype (function (pvec pvec) double-float) vect-dot))
(declaim (inline vect-dot))
(defun vect-dot (pva pvb)
"Compute the dot product between the two vectors and return it."
#+option-9-optimize-pvec (declare (optimize (speed 3) (safety 0)))
#+(and :option-9-optimize-pvec :sbcl)
(declare (sb-ext:muffle-conditions sb-ext:compiler-note))
(with-multiple-pvec-accessors ((a pva) (b pvb))
(as-double-float
(+ (* ax bx)
(* ay by)
(* az bz)))))
(declaim (ftype (function (pvec pvec) double-float) vdot))
(declaim (inline vdot))
(defun vdot (pva pvb)
"Shortname for VEC-DOT."
#+option-9-optimize-pvec (declare (optimize (speed 3) (safety 0)))
(vect-dot pva pvb))
;;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
(declaim (ftype (function (pvec pvec) double-float) vect-angle))
(defun vect-angle (pva pvb)
"Compute and return the angle in radians between the two vectors."
#+option-9-optimize-pvec (declare (optimize (speed 3) (safety 0)))
#+(and :option-9-optimize-pvec :sbcl)
(declare (sb-ext:muffle-conditions sb-ext:compiler-note))
(let ((dot (vdot pva pvb))
(denom (* (vnorm pva) (vnorm pvb))))
(if (zerop denom)
0d0
(as-double-float (acos (/ dot denom))))))
(declaim (ftype (function (pvec pvec) double-float) vangle))
(declaim (inline vangle))
(defun vangle (pva pvb)
"Shortname for VECT-ANGLE."
#+option-9-optimize-pvec (declare (optimize (speed 3) (safety 0)))
(vect-angle pva pvb))
;;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
(declaim (ftype (function (pvec pvec &key (:sqrt t))
(values double-float double-float))
vect-dist))
(defun vect-dist (pva pvb &key (sqrt t))
"Assume PVA and PVB are points in 3 dimensions and compute the distance
between them. If :SQRT is T, the default, then return the Euclidean distance
as the first value and the non-normalized distance as the second. If :SQRT is
NIL, then do not compute the Euclidean distance and instead return the
non-normalized distance for both values."
#+option-9-optimize-pvec (declare (optimize (speed 3) (safety 0)))
(with-multiple-pvec-accessors ((a pva) (b pvb))
(let* ((factor-1 (as-double-float (- bx ax)))
(factor-2 (as-double-float (- by ay)))
(factor-3 (as-double-float (- bz az)))
(non-normalized (as-double-float (+ (* factor-1 factor-1)
(* factor-2 factor-2)
(* factor-3 factor-3)))))
(values
(the double-float
(if sqrt
(sqrt (as-double-float non-normalized))
(as-double-float non-normalized)))
(as-double-float non-normalized)))))
(declaim (ftype (function (pvec pvec &key (:sqrt t))
(values double-float double-float))
vdist))
(declaim (inline vdist))
(defun vdist (pva pvb &key (sqrt t))
"Shortname for VECT-DIST."
#+option-9-optimize-pvec (declare (optimize (speed 3) (safety 0)))
(vect-dist pva pvb :sqrt sqrt))
;;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
(declaim (ftype (function (pvec &key (:span keyword)) pvec)
vect-rand-into))
(defun vect-rand-into (dst &key (span :xy))
"Into DST place a normalized vector which is randomly oriented in
the vector space defined by the :SPAN keyword. :SPAN may be one of:
:X, :Y, :Z, :XZ, :XY, :YZ, :XYZ."
#+option-9-optimize-pvec (declare (optimize (speed 3) (safety 0)))
(flet ((rand-one-dim ()
(* (random-sign) 1d0))
(rand-two-dim ()
;; parametric description of a circle centered at zero radius one.
(let ((t0 (random (* 2d0 pi))))
(values (sin t0) (cos t0))))
(rand-three-dim ()
(let ((longitude (random (* 2d0 pi)))
(colatitude (random (* 1d0 pi))))
(values (* 1d0 (cos longitude) (sin colatitude))
(* 1d0 (sin longitude) (sin colatitude))
(* 1d0 (cos colatitude))))))
(ecase span
((:x)
(vseti dst (rand-one-dim) 0d0 0d0))
((:y)
(vseti dst 0d0 (rand-one-dim) 0d0))
((:z)
(vseti dst 0d0 0d0 (rand-one-dim)))
((:xy)
(multiple-value-bind (x y) (rand-two-dim)
(vseti dst x y 0d0)))
((:xz)
(multiple-value-bind (x z) (rand-two-dim)
(vseti dst x 0d0 z)))
((:yz)
(multiple-value-bind (y z) (rand-two-dim)
(vseti dst 0d0 y z)))
((:xyz)
(multiple-value-bind (x y z) (rand-three-dim)
(vseti dst x y z))))
;; already normalized by virtue of construction.
dst))
(declaim (ftype (function (pvec &key (:span keyword)) pvec)
vrandi))
(declaim (inline vrandi))
(defun vrandi (dst &key (span :xy))
"Shortname for VECT-RAND-INTO."
#+option-9-optimize-pvec (declare (optimize (speed 3) (safety 0)))
(vect-rand-into dst :span span))
;;; ;;;;;;;;
(declaim (ftype (function (&key (:span keyword)) pvec)
vect-rand))
(declaim (inline vect-rand))
(defun vect-rand (&key (span :xy))
"Return a newly allocated normalized vector which is randomly
oriented in the vector space defined by the :SPAN keyword. :SPAN may
be one of:
:X, :Y, :Z, :XZ, :XY, :YZ, :XYZ."
#+option-9-optimize-pvec (declare (optimize (speed 3) (safety 0)))
(vrandi (pvec) :span span))
(declaim (ftype (function (&key (:span keyword)) pvec)
vrand))
(declaim (inline vrand))
(defun vrand (&key (span :xy))
"Shortname for VECT-RAND-INTO."
#+option-9-optimize-pvec (declare (optimize (speed 3) (safety 0)))
(vect-rand :span span))
;;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
(declaim (ftype (function (double-float double-float double-float) double-float)
dlerp))
(declaim (inline dlerp))
(defun dlerp (a b interp)
"Perform a linear interpolation from double-float A to double-float B
with double-float INTERP being the interpolant value. Return the interpolated
value as a double-float."
#+option-9-optimize-pvec (declare (optimize (speed 3) (safety 0)))
(as-double-float
(+ (* (- 1d0 interp) a) (* interp b))))
(defun vect-interpolate-into (dst from to interp &key (interp-func #'dlerp))
"Interpolate from pvec FROM to pvec TO by INTERP. Put result into pvec DST
and return it."
#+option-9-optimize-pvec (declare (optimize (speed 3) (safety 0)))
(with-multiple-pvec-accessors ((d dst) (f from) (e to))
(psetf dx (funcall interp-func fx ex interp)
dy (funcall interp-func fy ey interp)
dz (funcall interp-func fz ez interp)))
dst)
(defun vinterpi (dst from to interp &key (interp-func #'dlerp))
"Shortname for VECT-INTERPOLATE-INTO."
#+option-9-optimize-pvec (declare (optimize (speed 3) (safety 0)))
(vect-interpolate-into dst from to interp :interp-func interp-func))
;;; ;;;;;;;;
(defun vect-interpolate (from to interp &key (interp-func #'dlerp))
#+option-9-optimize-pvec (declare (optimize (speed 3) (safety 0)))
(vect-interpolate-into (pvec) from to interp :interp-func interp-func))
(defun vinterp (from to interp &key (interp-func #'dlerp))
#+option-9-optimize-pvec (declare (optimize (speed 3) (safety 0)))
(vect-interpolate from to interp :interp-func interp-func))
;;; ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;