extra/math/vectors/homogeneous/homogeneous.factor

   1 ! Copyright (C) 2009 Joe Groff.
   2 ! See https://factorcode.org/license.txt for BSD license.
   3 USING: kernel math math.vectors sequences ;
   4 IN: math.vectors.homogeneous
   5
   6 : (homogeneous-xyz) ( h -- xyz )
   7     but-last ; inline
   8
   9 : (homogeneous-w) ( h -- w )
  10     last ; inline
  11
  12 : h+ ( a b -- c )
  13     2dup [ (homogeneous-w) ] bi@ over =
  14     [ [ [ (homogeneous-xyz) ] bi@ v+ ] dip suffix ] [
  15         drop
  16         [ [ (homogeneous-xyz) ] [ (homogeneous-w)   ] bi* v*n    ]
  17         [ [ (homogeneous-w)   ] [ (homogeneous-xyz) ] bi* n*v v+ ]
  18         [ [ (homogeneous-w)   ] [ (homogeneous-w)   ] bi* * suffix ] 2tri
  19     ] if ;
  20
  21 : n*h ( n h -- nh )
  22     [ (homogeneous-xyz) n*v ] [ (homogeneous-w) suffix ] bi ;
  23
  24 : h*n ( h n -- nh )
  25     swap n*h ;
  26
  27 : hneg ( h -- -h )
  28     -1.0 swap n*h ;
  29
  30 : h- ( a b -- c )
  31     hneg h+ ;
  32
  33 : v>h ( v -- h )
  34     1.0 suffix ;
  35
  36 : h>v ( h -- v )
  37     [ (homogeneous-xyz) ] [ (homogeneous-w) ] bi v/n ;