jamshred: collision detection half working (half broken)

extra/jamshred/oint/oint-tests.factor

@@ -2,3 +2,7 @@ USING: jamshred.oint tools.test ;
 IN: jamshred.oint-tests
 
 [ { 0 -1 -1 } ] [ { 0 1 -1 } { 0 -1 0 } reflect ] unit-test
+[ { 0 1 0 } ] [ { 1 1 0 } { 1 0 0 } proj-perp ] unit-test
+[ { 1 0 0 } ] [ { 1 1 0 } { 0 1 0 } proj-perp ] unit-test
+[ { 1/2 -1/2 0 } ] [ { 1 0 0 } { 1 1 0 } proj-perp ] unit-test
+[ { -1/2 1/2 0 } ] [ { 0 1 0 } { 1 1 0 } proj-perp ] unit-test

extra/jamshred/oint/oint.factor

@@ -9,6 +9,7 @@ IN: jamshred.oint
 ! segment's location and orientation are given by an oint.
 
 TUPLE: oint location forward up left ;
+C: <oint> oint
 
 : rotation-quaternion ( theta axis -- quaternion )
     swap 2 / dup cos swap sin rot n*v first3 rect> >r rect> r> 2array ;
@@ -48,13 +49,13 @@ TUPLE: oint location forward up left ;
     #! the scalar projection of v1 onto v2
     tuck v. swap norm / ;
 
+: proj-perp ( u v -- w )
+    dupd proj v- ;
+
 : perpendicular-distance ( oint oint -- distance )
     tuck distance-vector swap 2dup left>> scalar-projection abs
     -rot up>> scalar-projection abs + ;
 
-: proj-perp ( v u -- w )
-    dupd proj v- ;
-
 :: reflect ( v n -- v' )
     #! bounce v on a surface with normal n
     v v n v. n n v. / 2 * n n*v v- ;

extra/jamshred/player/player.factor

@@ -27,7 +27,6 @@ TUPLE: player < oint name tunnel nearest-segment last-move ;
 
 : player-speed ( player -- speed )
     drop max-speed ;
-    ! dup nearest-segment>> fraction-from-wall sq max-speed * ;
 
 : distance-to-move ( player -- distance )
     [ player-speed ] [ last-move>> millis dup >r swap - 1000 / * r> ]
@@ -35,14 +34,9 @@ TUPLE: player < oint name tunnel nearest-segment last-move ;
 
 DEFER: (move-player)
 
-USE: morse
 : ?bounce ( distance-remaining player -- )
     over 0 > [
-        "e" play-as-morse
+        [ dup nearest-segment>> bounce ] [ (move-player) ] bi
-        [ dup nearest-segment>> bounce ]
-        ! [ (move-player) ] ! uncomment when bounce works...
-        [ 2drop ]
-        bi
     ] [
         2drop
     ] if ;
@@ -50,14 +44,11 @@ USE: morse
 : move-player-distance ( distance-remaining player distance -- distance-remaining player )
     pick min tuck over go-forward [ - ] dip ;
 
-USE: prettyprint
-USE: io.streams.string
 : (move-player) ( distance-remaining player -- )
     over 0 <= [
         2drop
     ] [
         dup dup nearest-segment>> distance-to-collision
-        [ dup . ] with-string-writer jamshred-log
         move-player-distance ?bounce
     ] if ;
 

extra/jamshred/tunnel/tunnel-tests.factor

@@ -3,8 +3,8 @@
 USING: jamshred.oint jamshred.tunnel kernel sequences tools.test ;
 IN: jamshred.tunnel.tests
 
-[ 0 ] [ T{ segment T{ oint f { 0 0 0 } } 0 }
+[ 0 ] [ T{ segment f { 0 0 0 } f f f 0 }
-        T{ segment T{ oint f { 1 1 1 } } 1 }
+        T{ segment f { 1 1 1 } f f f 1 }
         T{ oint f { 0 0 0.25 } }
         nearer-segment segment-number ] unit-test
 
@@ -15,3 +15,30 @@ IN: jamshred.tunnel.tests
 [ 3 ] [ <straight-tunnel> T{ oint f { 0 0 -3.25 } } 0 nearest-segment-forward segment-number ] unit-test
 
 [ F{ 0 0 0 } ] [ <straight-tunnel> T{ oint f { 0 0 -0.25 } } over first nearest-segment oint-location ] unit-test
+
+: test-segment-oint ( -- oint )
+    { 0 0 0 } { 0 0 -1 } { 0 1 0 } { -1 0 0 } <oint> ;
+
+[ { -1 0 0 } ] [ test-segment-oint { 1 0 0 } vector-to-centre ] unit-test
+[ { 1 0 0 } ] [ test-segment-oint { -1 0 0 } vector-to-centre ] unit-test
+[ { 0 -1 0 } ] [ test-segment-oint { 0 1 0 } vector-to-centre ] unit-test
+[ { 0 1 0 } ] [ test-segment-oint { 0 -1 0 } vector-to-centre ] unit-test
+[ { -1 0 0 } ] [ test-segment-oint { 1 0 -1 } vector-to-centre ] unit-test
+[ { 1 0 0 } ] [ test-segment-oint { -1 0 -1 } vector-to-centre ] unit-test
+[ { 0 -1 0 } ] [ test-segment-oint { 0 1 -1 } vector-to-centre ] unit-test
+[ { 0 1 0 } ] [ test-segment-oint { 0 -1 -1 } vector-to-centre ] unit-test
+
+: simplest-straight-ahead ( -- oint segment )
+    { 0 0 0 } { 0 0 -1 } { 0 1 0 } { -1 0 0 } <oint>
+    initial-segment ;
+
+[ { 0 0 0 } ] [ simplest-straight-ahead sideways-heading ] unit-test
+[ { 0 0 0 } ] [ simplest-straight-ahead sideways-relative-location ] unit-test
+
+: simple-collision-up ( -- oint segment )
+    { 0 0 0 } { 0 1 0 } { 0 0 1 } { -1 0 0 } <oint>
+    initial-segment ;
+
+[ { 0 1 0 } ] [ simple-collision-up sideways-heading ] unit-test
+[ { 0 0 0 } ] [ simple-collision-up sideways-relative-location ] unit-test
+[ { 0 1 0 } ] [ simple-collision-up collision-vector ] unit-test

extra/jamshred/tunnel/tunnel.factor

@@ -1,6 +1,6 @@
 ! Copyright (C) 2007 Alex Chapman
 ! See http://factorcode.org/license.txt for BSD license.
-USING: accessors arrays float-arrays kernel jamshred.oint locals math math.functions math.constants math.matrices math.order math.ranges math.vectors random sequences vectors ;
+USING: accessors arrays combinators float-arrays kernel jamshred.oint locals math math.functions math.constants math.matrices math.order math.ranges math.vectors math.quadratic random sequences vectors ;
 IN: jamshred.tunnel
 
 : n-segments ( -- n ) 5000 ; inline
@@ -24,7 +24,7 @@ C: <segment> segment
     dup [ / pi 2 * * ] curry map ;
 
 : segment-number++ ( segment -- )
-    dup segment-number 1+ swap set-segment-number ;
+    [ number>> 1+ ] keep (>>number) ;
 
 : random-color ( -- color )
     { 100 100 100 } [ random 100 / >float ] map { 1.0 } append ;
@@ -47,8 +47,8 @@ C: <segment> segment
 : default-segment-radius ( -- r ) 1 ;
 
 : initial-segment ( -- segment )
-        F{ 0 0 0 } F{ 0 0 -1 } F{ 0 1 0 } F{ -1 0 0 }
+    F{ 0 0 0 } F{ 0 0 -1 } F{ 0 1 0 } F{ -1 0 0 }
-        0 random-color default-segment-radius <segment> ;
+    0 random-color default-segment-radius <segment> ;
 
 : random-segments ( n -- segments )
     initial-segment 1vector swap (random-segments) ;
@@ -97,65 +97,52 @@ C: <segment> segment
     [ nearest-segment-forward ] 3keep
     nearest-segment-backward r> nearer-segment ;
 
-: distance-from-centre ( oint segment -- distance )
+: vector-to-centre ( seg loc -- v )
-    perpendicular-distance ;
+    over location>> swap v- swap forward>> proj-perp ;
 
-: distance-from-wall ( oint segment -- distance )
+: distance-from-centre ( seg loc -- distance )
-    tuck distance-from-centre swap segment-radius swap - ;
+    vector-to-centre norm ;
 
-: fraction-from-centre ( oint segment -- fraction )
+: wall-normal ( seg oint -- n )
-    tuck distance-from-centre swap segment-radius / ;
+    location>> vector-to-centre normalize ;
 
-: fraction-from-wall ( oint segment -- fraction )
+: from ( seg loc -- radius d-f-c )
+    dupd location>> distance-from-centre [ radius>> ] dip ;
+
+: distance-from-wall ( seg loc -- distance ) from - ;
+: fraction-from-centre ( seg loc -- fraction ) from / ;
+: fraction-from-wall ( seg loc -- fraction )
     fraction-from-centre 1 swap - ;
 
-: sideways-heading ( oint segment -- v )
-    [ forward>> ] bi@ proj-perp ;
-
-! : facing-nearest-wall? ( oint segment -- ? )
-!     [ [ location>> ] bi@ distance ]
-!     [ sideways-heading ]
-!     [ [ location>> ] bi@ [ v+ ] dip distance ] tri < ;
-
-! : distance-to-collision ( oint segment -- distance )
-! ! TODO: this isn't right. If oint is facing away from the wall then it should return a much bigger distance...
-!     #! distance on the oint's heading to the segment wall
-!     facing-nearest-wall? [
-!         [ sideways-heading norm ]
-!         [ distance-from-wall ] 2bi swap /
-!     ] [
-!     ] if ;
-
-USING: jamshred.log prettyprint io.streams.string ;
-
 : distant 10 ; inline
 
-:: (collision-coefficient) ( -2b sqrt(b^2-2ac) 2a -- c )
+:: (collision-coefficient) ( -b sqrt(b^2-4ac) 2a -- c )
-    sqrt(b^2-2ac) complex? [
+    sqrt(b^2-4ac) complex? [
         distant
     ] [
-        -2b sqrt(b^2-2ac) + 2a /
+        -b sqrt(b^2-4ac) + 2a /
-        -2b sqrt(b^2-2ac) - 2a / max ! the -ve answer is behind us
+        -b sqrt(b^2-4ac) - 2a / max ! the -ve answer is behind us
     ] if ;
 
 :: collision-coefficient ( v w -- c )
     [let* | a [ v dup v. ]
             b [ v w v. 2 * ]
             c [ w dup v. v dup v. - ] |
-        b neg b sq a c * 4 * - sqrt a 2 * (collision-coefficient) ] ;
+        c b a quadratic [ real-part ] bi@ max ] ;
 
+: sideways-heading ( oint segment -- v )
+    [ forward>> ] bi@ proj-perp ;
+
+: sideways-relative-location ( oint segment -- loc )
+    [ [ location>> ] bi@ v- ] keep forward>> proj-perp ;
+
+: collision-vector ( oint segment -- v )
+        dupd [ sideways-heading ] [ sideways-relative-location ] 2bi
+        collision-coefficient swap forward>> n*v ;
+
+USING: prettyprint jamshred.log io.streams.string ;
 : distance-to-collision ( oint segment -- distance )
-    [ sideways-heading ] [ [ location>> ] bi@ v- collision-coefficient ]
+    collision-vector norm [ dup . ] with-string-writer jamshred-log ;
-    [ drop forward>> n*v norm ] 2tri ;
-
-:: (wall-normal) ( seg loc -- n )
-    [let* | back [ loc seg location>> v- ]
-           back-proj [ back seg forward>> proj ]
-           perp-point [ loc back-proj v- ] |
-        perp-point seg location>> v- normalize ] ;
-
-: wall-normal ( segment oint -- n )
-    location>> (wall-normal) ;
 
 : bounce-forward ( segment oint -- )
     [ wall-normal ] [ forward>> swap reflect ] [ (>>forward) ] tri ;