% 3d.matrix.inc

% linear and affine transformations ---------------------

% a matrix is [[...]..[...]]
% a vector is [...]

% -------------------------------------------------------
% Arguments: v t
% Returns v + t on stack

/vectortranslate {
4 dict begin
/v exch def
/t exch def
/N v length def
[
0 1 N 1 sub {
/i exch def
v i get t i get add
} for
]
end
} def

% -----------------------------------------------------------
% Arguments: u v
% Returns u.v on stack

/dotproduct {
4 dict begin
/u exch def
/v exch def
/n u length def
0
0 1 n 1 sub {
/i exch def
u i get v i get mul add
} for
end
} def

% -----------------------------------------------------------
% Arguments: M v
% Returns M.v on stack

/matrixvector {
8 dict begin
/v exch def
/m exch def
/r m length def

[

0 1 r 1 sub {
m exch get
v
dotproduct
} for

]
end
} def

% -----------------------------------------------------------
% Arguments: n
% Returns n x n identity matrix on stack

/identitymatrix {
3 dict begin
[
[
n { 0 } for
]
]
/I exch def
0 1 n 1 sub {
/i exch def
I i 0 get i 0 put
} for
} def

% -----------------------------------------------------------
% Arguments: M = [L T] v
% M is an affine map
% L = matrix
% T, v = vectors
% returns Lv + T

/affinemap {
2 dict begin
/v exch def
/M exch def
M 0 get
v matrixvector 
M 1 get vectortranslate
end
} def

%------------------------------------------------
% Arguments: matrix M 
% returns transpose of M

/transpose {
3 dict begin
/m exch def
/r m length def
/c m 0 get length def
[
0 1 c 1 sub {
/i exch def
[
0 1 r 1 sub {
/j exch def
m j get i get
} for
]
} for
]
end
} def

%------------------------------------------------
% Arguments: matrices m n 
% Returns the matrix m x n 

/matrixmul {
8 dict begin
/n exch transpose def
/m exch def
/r m length def
/c n length def

[

0 1 c 1 sub {
n exch get
m exch
matrixvector 
} for

]
transpose
end
} def

% vector algebra --------------------------------

% Arguments: vector v
% Returns |v| 

/vectorlength {
3 dict begin
/v exch def
/n v length def
0
0 1 n 1 sub {
v exch get dup mul add
} for
sqrt
end
} def

%------------------------------------------------
% Argument: v
% Returns v/|v| 
% normalized so length = 1

/normalized {
3 dict begin
/v exch def
/r v vectorlength def
/n v length def
[
0 1 n 1 sub {
v exch get r div
} for
]
end
} def

%-----------------------------------------------------------
% Arguments: P[3] Q[3]
% Returns P x Q

/crossproduct {
2 dict begin
/Q exch def
/P exch def

[
P 1 get
Q 2 get
mul
P 2 get
Q 1 get
mul
sub

Q 0 get
P 2 get
mul
Q 2 get
P 0 get
mul
sub

P 0 get
Q 1 get
mul
P 1 get
Q 0 get
mul
sub
]
end
} def

% rotations ---------------------------------------------

% Arguments: axis[3] theta v[3]
% Returns rotation of v through theta around axis

/3rotate {
8 dict begin
/v exch def
/theta exch def
/axis exch normalized def
/r axis v dotproduct def
/v0 
[
axis 0 get r mul
axis 1 get r mul
axis 2 get r mul
] def
/vperp [
v 0 get v0 0 get sub
v 1 get v0 1 get sub
v 2 get v0 2 get sub
] def

/vstar 
axis vperp crossproduct
def

[
v0 0 get
vperp 0 get theta cos mul 
vstar 0 get theta sin mul 
add add
v0 1 get
vperp 1 get theta cos mul
vstar 1 get theta sin mul
add add
v0 2 get
vperp 2 get theta cos mul
vstar 2 get theta sin mul
add add
]
end
} def

% ---------------------------------------------------------
% Arguments: axis[3] theta
% Returns the matrix of the rotation 

/rotation-matrix {
5 dict begin
/theta exch def
/axis exch normalized def
/c0 
axis theta [1 0 0] 3rotate
def
/c1
axis theta [0 1 0] 3rotate
def
/c2 
axis theta [0 0 1] 3rotate
def
[
[
c0 0 get
c1 0 get
c2 0 get
]
[
c0 1 get
c1 1 get
c2 1 get
]
[
c0 2 get
c1 2 get
c2 2 get
]
]
dup /M exch def
end
} def

% -------------------------------------------------------
% Arguments: v u
% Returns r_u v
% Assumes |u| = 1

/reflect {
3 dict begin
/u exch def
/v exch def
/c u v dotproduct 2 mul def
[
v 0 get u 0 get c mul sub
v 1 get u 1 get c mul sub
v 2 get u 2 get c mul sub
]
end
} def

% -----------------------------------------------------
% Arguments: [m1 t1] [m2 t2]
% Concats two 3D affine transformations
% Returns the composition

/affineconcat {

4 dict begin
aload pop
/T2 exch def
/M2 exch def
aload pop
/T1 exch def
/M1 exch def
[
M1 
M2 
matrixmul
M1 
T2
matrixvector
T1 
vectortranslate
]
end
} def

% -------------------------------------------------------------