USING: jamshred.oint jamshred.tunnel kernel sequences tools.test ;
IN: jamshred.tunnel.tests
-[ 0 ] [ T{ segment T{ oint f { 0 0 0 } } 0 }
- T{ segment T{ oint f { 1 1 1 } } 1 }
+[ 0 ] [ T{ segment f { 0 0 0 } f f f 0 }
+ T{ segment f { 1 1 1 } f f f 1 }
T{ oint f { 0 0 0.25 } }
nearer-segment segment-number ] unit-test
[ 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
! 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
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 ;
: default-segment-radius ( -- r ) 1 ;
: initial-segment ( -- segment )
- F{ 0 0 0 } F{ 0 0 -1 } F{ 0 1 0 } F{ -1 0 0 }
- 0 random-color default-segment-radius <segment> ;
+ F{ 0 0 0 } F{ 0 0 -1 } F{ 0 1 0 } F{ -1 0 0 }
+ 0 random-color default-segment-radius <segment> ;
: random-segments ( n -- segments )
initial-segment 1vector swap (random-segments) ;
[ nearest-segment-forward ] 3keep
nearest-segment-backward r> nearer-segment ;
-: distance-from-centre ( oint segment -- distance )
- perpendicular-distance ;
+: vector-to-centre ( seg loc -- v )
+ over location>> swap v- swap forward>> proj-perp ;
-: distance-from-wall ( oint segment -- distance )
- tuck distance-from-centre swap segment-radius swap - ;
+: distance-from-centre ( seg loc -- distance )
+ vector-to-centre norm ;
-: fraction-from-centre ( oint segment -- fraction )
- tuck distance-from-centre swap segment-radius / ;
+: wall-normal ( seg oint -- n )
+ location>> vector-to-centre normalize ;
-: fraction-from-wall ( oint segment -- 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 ;
+: from ( seg loc -- radius d-f-c )
+ dupd location>> distance-from-centre [ radius>> ] dip ;
-USING: jamshred.log prettyprint io.streams.string ;
+: 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 - ;
: distant 10 ; inline
-:: (collision-coefficient) ( -2b sqrt(b^2-2ac) 2a -- c )
- sqrt(b^2-2ac) complex? [
+:: (collision-coefficient) ( -b sqrt(b^2-4ac) 2a -- c )
+ sqrt(b^2-4ac) complex? [
distant
] [
- -2b sqrt(b^2-2ac) + 2a /
- -2b sqrt(b^2-2ac) - 2a / max ! the -ve answer is behind us
+ -b sqrt(b^2-4ac) + 2a /
+ -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 ] ;
-: distance-to-collision ( oint segment -- distance )
- [ sideways-heading ] [ [ location>> ] bi@ v- collision-coefficient ]
- [ drop forward>> n*v norm ] 2tri ;
+: sideways-heading ( oint segment -- v )
+ [ forward>> ] bi@ proj-perp ;
+
+: sideways-relative-location ( oint segment -- loc )
+ [ [ location>> ] bi@ v- ] keep forward>> proj-perp ;
-:: (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 ] ;
+: collision-vector ( oint segment -- v )
+ dupd [ sideways-heading ] [ sideways-relative-location ] 2bi
+ collision-coefficient swap forward>> n*v ;
-: wall-normal ( segment oint -- n )
- location>> (wall-normal) ;
+USING: prettyprint jamshred.log io.streams.string ;
+: distance-to-collision ( oint segment -- distance )
+ collision-vector norm [ dup . ] with-string-writer jamshred-log ;
: bounce-forward ( segment oint -- )
[ wall-normal ] [ forward>> swap reflect ] [ (>>forward) ] tri ;