extra/jamshred/oint/oint.factor

   1 ! Copyright (C) 2007, 2008 Alex Chapman
   2 ! See http://factorcode.org/license.txt for BSD license.
   3 USING: accessors arrays kernel math math.functions math.vectors
   4 random sequences ;
   5 IN: jamshred.oint
   6
   7 ! An oint is a point with three linearly independent unit vectors
   8 ! given relative to that point. In jamshred a player's location and
   9 ! direction are given by the player's oint. Similarly, a tunnel
  10 ! segment's location and orientation are given by an oint.
  11
  12 TUPLE: oint location forward up left ;
  13 C: <oint> oint
  14
  15 : rotation-quaternion ( theta axis -- quaternion )
  16     swap 2 / dup cos swap sin rot n*v first3 rect> [ rect> ] dip 2array ;
  17
  18 <PRIVATE
  19
  20 ! inline old math.quaternions to get this to work, eww.
  21
  22 : ** ( x y -- z ) conjugate * ; inline
  23
  24 : 2q ( u v -- u' u'' v' v'' ) [ first2 ] bi@ ; inline
  25
  26 : q*a ( u v -- a ) 2q swapd ** [ * ] dip - ; inline
  27
  28 : q*b ( u v -- b ) 2q [ ** swap ] dip * + ; inline
  29
  30 : q* ( u v -- u*v )
  31     [ q*a ] [ q*b ] 2bi 2array ;
  32
  33 : v>q ( v -- q )
  34     first3 rect> [ 0 swap rect> ] dip 2array ;
  35
  36 : q>v ( q -- v )
  37     first2 [ imaginary-part ] dip >rect 3array ;
  38
  39 : qconjugate ( u -- u' )
  40     first2 [ conjugate ] [ neg  ] bi* 2array ;
  41
  42 : qrecip ( u -- 1/u )
  43     qconjugate dup norm-sq v/n ;
  44
  45 PRIVATE>
  46
  47 : rotate-vector ( q qrecip v -- v )
  48     v>q swap q* q* q>v ;
  49
  50 : rotate-oint ( oint theta axis -- )
  51     rotation-quaternion dup qrecip pick
  52     [ forward>> rotate-vector >>forward ]
  53     [ up>> rotate-vector >>up ]
  54     [ left>> rotate-vector >>left ] 3tri drop ;
  55
  56 : left-pivot ( oint theta -- )
  57     over left>> rotate-oint ;
  58
  59 : up-pivot ( oint theta -- )
  60     over up>> rotate-oint ;
  61
  62 : forward-pivot ( oint theta -- )
  63     over forward>> rotate-oint ;
  64
  65 : random-float+- ( n -- m )
  66     ! find a random float between -n/2 and n/2
  67     dup 10000 * >integer random 10000 / swap 2 / - ;
  68
  69 : random-turn ( oint theta -- )
  70     2 / 2dup random-float+- left-pivot random-float+- up-pivot ;
  71
  72 : location+ ( v oint -- )
  73     [ location>> v+ ] [ location<< ] bi ;
  74
  75 : go-forward ( distance oint -- )
  76     [ forward>> n*v ] [ location+ ] bi ;
  77
  78 : distance-vector ( oint oint -- vector )
  79     [ location>> ] bi@ swap v- ;
  80
  81 : distance ( oint oint -- distance )
  82     distance-vector norm ;
  83
  84 : scalar-projection ( v1 v2 -- n )
  85     ! the scalar projection of v1 onto v2
  86     [ vdot ] [ norm ] bi / ;
  87
  88 : proj-perp ( u v -- w )
  89     dupd proj v- ;
  90
  91 : perpendicular-distance ( oint oint -- distance )
  92     [ distance-vector ] keep 2dup left>> scalar-projection abs
  93     -rot up>> scalar-projection abs + ;
  94
  95 :: reflect ( v n -- v' )
  96     ! bounce v on a surface with normal n
  97     v v n vdot n n vdot / 2 * n n*v v- ;
  98
  99 : half-way ( p1 p2 -- p3 )
 100     over v- 2 v/n v+ ;
 101
 102 : half-way-between-oints ( o1 o2 -- p )
 103     [ location>> ] bi@ half-way ;