% - Vector calculations (good in any number of dimensions) --------- /matrixinc true def /dot-product { 1 dict begin /v exch def 0 0 % u s i 3 2 roll { % s i u[i] v % s i u[i] v 2 index get mul % s i u[i]*v[i] 3 2 roll % i u[i]*v[i] s add exch 1 add % s i } forall pop end } def /vector-scale { 1 dict begin /c exch def [ exch { % s i u[i] c mul % s i u[i] v } forall ] end } def /vector-add { 1 dict begin /v exch def [ exch 0 % u i exch { % i u[i] v % i u[i] v 2 index get add % i u[i]+v[i] exch 1 add % i } forall pop ] end } def /vector-sub { 1 dict begin /v exch def [ exch 0 % u i exch { % i u[i] v % i u[i] v 2 index get sub % i u[i]+v[i] exch 1 add % i } forall pop ] end } def % [x y z ... ] => r % watch out for overflow /vector-length { 1 dict begin dup % find maximum entry /max 0 def { % max abs dup max gt { % if abs gt max /max exch def } { pop } ifelse } forall max 0 ne { 0 exch { % 0 v[i] max div dup mul add } forall sqrt max mul } { pop 0 } ifelse end } def % v => v/|v| /normalized { 1 dict begin dup % v v vector-length /r exch def [ exch { r div } forall ] end } def % u v % u0 u1 u2 % v0 v1 v2 /cross-product { 2 dict begin /v exch def /u exch def [ u 1 get v 2 get mul v 1 get u 2 get mul sub v 0 get u 2 get mul u 0 get v 2 get mul sub u 0 get v 1 get mul v 0 get u 1 get mul sub ] end } def % -------------------------------------------------------------- % axis A => [ x1 y1 z1 x2 y2 z2 x3 y3 z3 ] = columns of the matrix /rotation-matrix3d { 8 dict begin dup cos /c exch def sin /s exch def /a exch def /r a vector-length def /a [ a 0 get r div a 1 get r div a 2 get r div ] def [ % e = [1 0 0] etc. % e0 = (e.a)a, e# = e - e0, e* = a x e = a x e0 + a x e# = a x e# /x a 0 get def /e0 [a 0 get x mul a 1 get x mul a 2 get x mul] def /e# [1 e0 0 get sub e0 1 get neg e0 2 get neg] def % [a0 a1 a2] % [ 1 0 0] /e* [0 a 2 get a 1 get neg ] def e# 0 get c mul e* 0 get s mul add e0 0 get add e# 1 get c mul e* 1 get s mul add e0 1 get add e# 2 get c mul e* 2 get s mul add e0 2 get add /x a 1 get def /e0 [a 0 get x mul a 1 get x mul a 2 get x mul] def /e# [e0 0 get neg 1 e0 1 get sub e0 2 get neg] def % [a0 a1 a2] % [ 0 1 0] /e* [a 2 get neg 0 a 0 get ] def e# 0 get c mul e* 0 get s mul add e0 0 get add e# 1 get c mul e* 1 get s mul add e0 1 get add e# 2 get c mul e* 2 get s mul add e0 2 get add /x a 2 get def /e0 [a 0 get x mul a 1 get x mul a 2 get x mul] def /e# [e0 0 get neg e0 1 get neg 1 e0 2 get sub] def % [a0 a1 a2] % [ 0 0 1] /e* [a 1 get a 0 get neg 0 ] def e# 0 get c mul e* 0 get s mul add e0 0 get add e# 1 get c mul e* 1 get s mul add e0 1 get add e# 2 get c mul e* 2 get s mul add e0 2 get add ] end } def % square matrices /matrix-mul { 8 dict begin /B exch def /A exch def /N A length sqrt round cvi def /n N 1 sub def % 0 1 % n n+1 % 2n 2n+1 [ /i 0 def N { % i = initial index of the row /j 0 def N { % j = initial index of the column /k i def /ell j def 0 N { A k get B ell get mul add /k k 1 add def /ell ell N add def } repeat /j j 1 add def } repeat /i i N add def } repeat ] end } def % a square matrix m x m % [i, j] = n*i + j /transpose { 4 dict begin /M exch def /n M length sqrt round cvi def [ /k 0 def n { /i k def n { M i get /i i n add def } repeat /k k 1 add def } repeat ] end } def /3x3-det { 1 dict begin /m exch def m 0 get m 4 get mul m 8 get mul m 1 get m 5 get mul m 6 get mul add m 2 get m 3 get mul m 7 get mul add m 2 get m 4 get mul m 6 get mul sub m 1 get m 3 get mul m 8 get mul sub m 0 get m 5 get mul m 7 get mul sub end } def /3x3-inverse { 2 dict begin /m exch def /d m 3x3-det def [ m 4 get m 8 get mul m 5 get m 7 get mul sub d div m 2 get m 7 get mul m 1 get m 8 get mul sub d div m 1 get m 5 get mul m 4 get m 2 get mul sub d div m 5 get m 6 get mul m 3 get m 8 get mul sub d div m 0 get m 8 get mul m 2 get m 6 get mul sub d div m 2 get m 3 get mul m 0 get m 5 get mul sub d div m 3 get m 7 get mul m 6 get m 4 get mul sub d div m 1 get m 6 get mul m 0 get m 7 get mul sub d div m 0 get m 4 get mul m 1 get m 3 get mul sub d div ] end } def /acos { dup dup % x x x mul 1 sub neg % x 1-x^2 sqrt exch atan } def % u v /angle-between { dup vector-length % u v |v| 3 1 roll % |v| u v 1 index % |v| u v u dot-product % |v| u u.v exch vector-length % |v| u.v |u| div exch div acos } def