% - matrix3d.inc --------------------------------------------------

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

% v c

/vector-scale-3d {
2 dict begin
/c exch def
/v exch def
[
v 0 get c mul
v 1 get c mul
v 2 get c mul
]
end
} def

% u v

/vector-add-3d {
3 dict begin
/v exch def
/u exch def
[
u 0 get v 0 get add
u 1 get v 1 get add
u 2 get v 2 get add
]
end
} def

/vector-sub-3d {
3 dict begin
/v exch def
/u exch def
[
u 0 get v 0 get sub
u 1 get v 1 get sub
u 2 get v 2 get sub
]
end
} def

/dot-product-3d {
2 dict begin
/v exch def
/u exch def
  u 0 get v 0 get mul
  u 1 get v 1 get mul add
  u 2 get v 2 get mul add
end
} def

% [u0 u1 u2]
% [v0 v1 v2]

/cross-product {
2 dict begin
/v exch def
/u exch def
[
  u 1 get v 2 get mul
  u 2 get v 1 get mul sub
  u 2 get v 0 get mul
  u 0 get v 2 get mul sub
  u 0 get v 1 get mul
  u 1 get v 0 get mul sub
]
end
} def

% u

/normalize-3d {
2 dict begin
/u exch def
/r 
  u 0 get dup mul
  u 1 get dup mul add
  u 2 get dup mul add
  sqrt 
def
[
  u 0 get r div
  u 1 get r div
  u 2 get r div
]
end
} def

/vector-length-3d {
1 dict begin
/v exch def
v 0 get dup mul
v 1 get dup mul add
v 2 get dup mul add
sqrt
end
} def

% - matrix algebra ----------------------------------------------

% M u

/transform-3d {
3 dict begin
/u exch def
/M exch def
[
0 1 2 {
  /i exch def
  /m M i get def
    m 0 get u 0 get mul
    m 1 get u 1 get mul add
    m 2 get u 2 get mul add
} for
]
end
} def

% a 3d matrix = 3 rows of 3-vectors

% [M v] u => Mu + v

/affine-transform-3d {
6 dict begin
/u exch def
aload pop 
/v exch def
/M exch def
[
0 1 2 {
  /i exch def
  /m M i get def
  v i get
  0 1 2 {
    /j exch def
    m j get u j get mul
    add
  } for
} for
]
end
} def

/identity-matrix-3d [
[1 0 0]
[0 1 0]
[0 0 1]
]
def

% a b

/matrix-mul-3d {
4 dict begin
/b exch def
/a exch def
[
  0 1 2 {
    /i exch def
    [
      0 1 2 {
        /k exch def
        0
        0 1 2 {
          /j exch def
          a i get j get b j get k get mul add
        } for
      } for
    ]
  } for
]
end
} def

/transpose-3d {
1 dict begin
/m exch def
[
  [
    m 0 get 0 get
    m 1 get 0 get
    m 2 get 0 get
  ]
  [
    m 0 get 1 get
    m 1 get 1 get
    m 2 get 1 get
  ]
  [
    m 0 get 2 get
    m 1 get 2 get
    m 2 get 2 get
  ]
]
end
} def

% a x v = axis nagle vector

/rotate-3d {
9 dict begin
/v exch def
/x exch def
/a exch normalize-3d def
/sc v a dot-product-3d def
/v0 [ 
  a 0 get sc mul
  a 1 get sc mul
  a 2 get sc 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 a vperp cross-product def
/c x cos def
/s x sin def
[
  v0 0 get 
  vperp 0 get c mul add 
  vstar 0 get s mul add
  v0 1 get 
  vperp 1 get c mul add 
  vstar 1 get s mul add
  v0 2 get 
  vperp 2 get c mul add 
  vstar 2 get s mul add
]
end
} def

% a x

/rotation-matrix-3d {
3 dict begin
/x exch def
/a exch def
/m [
  a x [1 0 0] rotate-3d 
  a x [0 1 0] rotate-3d
  a x [0 0 1] rotate-3d
] def
[
  [
    m 0 get 0 get 
    m 1 get 0 get
    m 2 get 0 get
  ]
  [
    m 0 get 1 get 
    m 1 get 1 get
    m 2 get 1 get
  ]
  [
    m 0 get 2 get 
    m 1 get 2 get
    m 2 get 2 get
  ]
]
end
} def

% - end of matrix.inc ---------------------------------------