% matrix.inc

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

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

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

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

/vectortranslate {
vector-translate
} def

/vector-translate {
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 {
dot-product
} def

/dot-product {
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 {
matrix-vector
} def

/matrix-vector {
8 dict begin
/v exch def
/m exch def
/r m length def

[

0 1 r 1 sub {
m exch get
v
dot-product
} for

]
end
} def

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

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

/identitymatrix {
identity-matrix 
} def

/identity-matrix {
4 dict begin
/n exch def
/I 
[
n {
[
n { 0 } repeat
]
} repeat
]
def

0 1 n 1 sub {
/i exch def
I i get i 1 put
} for
I
end
} def

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

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

/affinemap {
affine-map
} def

/affine-map {
2 dict begin
/v exch def
/M exch def
M 0 get
v matrix-vector 
M 1 get vector-translate
end
} def

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

% Arguments: matrix M 
% returns transpose of M

/transpose {
6 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 {
matrix-mul
} def

/matrix-mul {
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
matrix-vector 
} for

]
transpose
end
} def

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

% Arguments: vector v
% Returns |v| 

/vectorlength {
vector-length
} def

/vector-length {
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 vector-length 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 {
cross-product
} def

/cross-product {
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 dot-product 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 cross-product
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
]
]
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 dot-product 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

% v c

/vectorscale {
vector-scale
} def

/vector-scale {
4 dict begin
/c exch def
/v exch def
/N v length def

[
0 1 N 1 sub {
v exch get
c mul
} for
]
end
} def

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

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

/affineconcat {
affine-concat
} def

/affine-concat {

4 dict begin
aload pop
/T2 exch def
/M2 exch def
aload pop
/T1 exch def
/M1 exch def
[
M1 
M2 
matrix-mul
M1 
T2
matrix-vector
T1 
vector-translate
]
end
} def

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

/acos {
1 dict begin
/x exch def
1 x dup mul sub sqrt x atan
end
} def

/degrees-to-radians {
180 div pi mul
} def

% Arguments: u v
% Returns the angle between them in degrees

/angle-between {
2 dict begin
/v exch def
/u exch def
u v dot-product
u vector-length div v vector-length div
acos 
end
} def

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

% Arguments: m[3][3]
% Returns determinant of m

/3x3-determinant {
1 dict begin
/m exch def

m 0 get 0 get 
m 1 get 1 get mul
m 2 get 2 get mul

m 0 get 1 get
m 1 get 2 get mul
m 2 get 0 get mul

add

m 0 get 2 get
m 1 get 0 get mul
m 2 get 1 get mul

add

m 0 get 2 get
m 1 get 1 get mul
m 2 get 0 get mul

sub

m 0 get 0 get
m 1 get 2 get mul
m 2 get 1 get mul

sub

m 0 get 1 get
m 1 get 0 get mul
m 2 get 2 get mul

sub

end
} def

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

% Arguments: m[3][3]
% Returns inverse of m

/3x3-inverse {
2 dict begin
/m exch def
/d m 3x3-determinant def
[
[
% [0][0]
m 1 get 1 get
m 2 get 2 get mul
m 2 get 1 get 
m 1 get 2 get mul
sub
d div

% [0][1]
m 2 get 1 get 
m 0 get 2 get mul
m 0 get 1 get
m 2 get 2 get mul
sub
d div

% [0][2]
m 0 get 1 get
m 1 get 2 get mul
m 1 get 1 get 
m 0 get 2 get mul
sub
d div
]

[
% [1][0]
m 2 get 0 get 
m 1 get 2 get mul
m 1 get 0 get
m 2 get 2 get mul
sub
d div

% [1][1]
m 0 get 0 get 
m 2 get 2 get mul
m 0 get 2 get
m 2 get 0 get mul
sub
d div

% [1][2]
m 1 get 0 get 
m 0 get 2 get mul
m 0 get 0 get
m 1 get 2 get mul
sub
d div
]

[
% [2][0]
m 1 get 0 get
m 2 get 1 get mul
m 2 get 0 get 
m 1 get 1 get mul
sub
d div

% [2][1]
m 0 get 1 get
m 2 get 0 get mul
m 2 get 1 get 
m 0 get 0 get mul
sub
d div

% [2][2]
m 0 get 0 get
m 1 get 1 get mul
m 1 get 0 get 
m 0 get 1 get mul
sub
d div
]

]
end
} def

% m

/2x2-determinant {
1 dict begin
/m exch def
	m 0 get 0 get 
	m 1 get 1 get mul
	m 0 get 1 get 
	m 1 get 0 get mul
	sub
end
} def

/2x2-inverse {
2 dict begin
/m exch def
/d m 2x2-determinant def
[
[ m 1 get 1 get     d div m 0 get 1 get neg d div]
[ m 1 get 0 get neg d div m 1 get 1 get     d div]
]
end
} def

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

false {
/M [
[1 -1][3 1]
] def
M 2x2-inverse
M matrix-mul ==

} if