$b->{X} - $a->{X});
$r= a_normalise($r,$a->{A});
return $r;
-}
+}
+
+sub v_rotateright ($) {
+ # v_rotateright(A)
+ # returns image of A rotated 90 deg clockwise
+ my ($a)= @_;
+ return { X => $a->{Y}, Y => -$a->{X} };
+}
+sub v_dotproduct ($$) {
+ # v_dotproduct(A,B)
+ my ($a,$b)= @_;
+ return $a->{X} * $b->{X} + $a->{Y} * $b->{Y};
+}
+sub v_scalarmult ($$) {
+ # v_scalarmult(S,V)
+ # multiplies V by scalar S and returns product
+ my ($s,$v)=@_;
+ return { X => $s * $v->{X}, Y => $s * $v->{Y} };
+}
+sub v_add ($;@) {
+ # v_add(A,B,...)
+ # vector sum of all inputs
+ my (@i) = @_;
+ my ($r,$i);
+ $r= { X => 0.0, Y => 0.0 };
+ foreach $i (@i) { $r->{X} += $i->{X}; $r->{Y} += $i->{Y}; }
+ return $r;
+}
+sub v_subtract ($$) {
+ # v_subtract(A,B)
+ # returns vector from A to B, ie B - A
+ my ($a,$b)= @_;
+ return { X => $b->{X} - $a->{X},
+ Y => $b->{Y} - $a->{Y} };
+}
+sub v_len ($) {
+ # v_len(V)
+ # scalar length of V
+ my ($v)=@_;
+ my ($x,$y) = ($v->{X}, $v->{Y});
+ return sqrt($x*$x + $y*$y);
+}
sub v_dist ($$) {
# v_dist(A,B)
# returns distance from A to B
- # A->{A} and B->{A} are ignored
- my ($a,$b)= @_;
- my ($xd,$yd);
- $xd= $b->{X} - $a->{X};
- $yd= $b->{Y} - $a->{Y};
- return sqrt($xd*$xd + $yd*$yd);
-}
+ return v_len(v_subtract($_[0],$_[1]));
+}
sub upd_min ($$) {
my ($limr,$now)=@_;
});
}
-sub cmd_join {
- my ($from,$to,$how,$minradius);
- $from= can(\&cva_idex);
- $to= can(\&cva_idex);
- $minradius= can(\&cva_len);
- my (@paths,@solkinds);
- o("% join ".loc2dbg($from)."..".loc2dbg($to)." $minradius\n");
- do {
- # two circular arcs of equal maximum possible radius
- # algorithm courtesy of Simon Tatham (`Railway problem',
- # pers.comm. to ijackson@chiark 23.1.2004)
- my ($sigma,$distfact, $theta,$phi, $a,$b,$c,$d, $m,$r, $radius);
- my ($cvec,$cfrom,$cto,$midpt, $delta1,$delta2, $path,$reverse);
- $sigma= ev_bearing($from,$to);
- $distfact= v_dist($from,$to);
- $theta= 0.5 * $pi - ($from->{A} - $sigma);
- $phi= 0.5 * $pi - ($to->{A} + $pi - $sigma);
- $a= 2 * (1 + cos($theta - $phi));
- $b= 2 * (cos($theta) - cos($phi));
- $c= -1;
- $d= sqrt($b*$b - 4*$a*$c);
- o("% twoarcs theta=".ang2deg($theta)." phi=".ang2deg($phi).
- " ${a}r^2 + ${b}r + ${c} = 0\n");
- foreach $m (qw(-1 1)) {
- if ($a < 1e-6) {
- o("% twoarcs $m insoluble\n");
- next;
- }
- $r= -0.5 * (-$b + $m*$d) / $a;
- $radius= -$r * $distfact;
- o("% twoarcs $m radius $radius ");
- if (abs($radius) < $minradius) { o("too-small\n"); next; }
- $cfrom= ev_compose({}, $from, { X=>0, Y=>-$radius, A=>-0.5*$pi });
- $cto= ev_compose({}, $to, { X=>0, Y=> $radius, A=> 0.5*$pi });
- $midpt= ev_lincomb({}, $cfrom, $cto, 0.5);
- $reverse= signum($r);
- if ($reverse<0) {
- $cfrom->{A} += $pi;
- $cto->{A} += $pi;
- }
- $delta1= ev_bearing($cfrom, $midpt) - $cfrom->{A};
- $delta2= ev_bearing($cto, $midpt) - $cto->{A};
- o("ok deltas ".ang2deg($delta1)." ".ang2deg($delta2)."\n");
- if ($reverse<0) {
- $delta1 -= 2*$pi;
- $delta2 -= 2*$pi;
- }
- my ($fs);
- $path= [{ T=>Arc, F=>$from, C=>$cfrom, R=> $radius, D=>$delta1 },
- { T=>Arc, F=>$to, C=>$cto, R=>-$radius, D=>$delta2 }];
- push @paths, $path;
- push @solkinds, [ 'twoarcs', 'cross' ];
+# joins_xxx all take $results, $from, $to, $minradius
+# where $results->[]{Path}{K} etc. and $results->[]{SolKinds}[]
+
+sub joins_twoarcs ($$$$) {
+ my ($results, $from,$to,$minradius) = @_;
+ # two circular arcs of equal maximum possible radius
+ # algorithm courtesy of Simon Tatham (`Railway problem',
+ # pers.comm. to ijackson@chiark 23.1.2004)
+ my ($sigma,$distfact, $theta,$phi, $a,$b,$c,$d, $m,$r, $radius);
+ my ($cvec,$cfrom,$cto,$midpt, $delta1,$delta2, $path,$reverse);
+ $sigma= ev_bearing($from,$to);
+ $distfact= v_dist($from,$to);
+ $theta= 0.5 * $pi - ($from->{A} - $sigma);
+ $phi= 0.5 * $pi - ($to->{A} + $pi - $sigma);
+ $a= 2 * (1 + cos($theta - $phi));
+ $b= 2 * (cos($theta) - cos($phi));
+ $c= -1;
+ $d= sqrt($b*$b - 4*$a*$c);
+ o("% twoarcs theta=".ang2deg($theta)." phi=".ang2deg($phi).
+ " ${a}r^2 + ${b}r + ${c} = 0\n");
+ foreach $m (qw(-1 1)) {
+ if ($a < 1e-6) {
+ o("% twoarcs $m insoluble\n");
+ next;
}
- } while 0;
- if ($minradius<=1e-6) {
- o("% arcsline no-radius\n");
- } else {
- # two circular arcs of specified radius
- # with an intervening straight
- my ($lr,$inv, $c,$d,$alpha,$t,$k,$l,$rpmsina,$rcosa,$linelen, $path);
- foreach $lr (qw(-1 +1)) {
- foreach $inv (qw(-1 +1)) {
- $c=ev_compose({},$from,{X=>0,Y=>-$lr*$minradius, A=>0 });
- $d=ev_compose({},$to,{X=>0, Y=>-$inv*$lr*$minradius, A=>$pi });
- $t= v_dist($c,$d);
- o("% arcsline $lr $inv t=$t ");
- if ($t < 1e-6) { o("concentric"); next; }
- $c->{A}= $d->{A}= ev_bearing($c,$d);
- o("bearing ".ang2deg($c->{A}));
- if ($inv>0) {
- o("\n");
- $k= ev_compose({}, $c, { X=>0, Y=>$lr*$minradius, A=>0 });
- $l= ev_compose({}, $d, { X=>0, Y=>$lr*$minradius, A=>0 });
- $linelen= $t;
- } else {
- my ($cosalpha) = 2.0 * $minradius / $t;
- if ($cosalpha > (1.0 - 1e-6)) { o(" too-close\n"); next; }
- $alpha= acos($cosalpha);
- $rpmsina= $lr * $minradius * sin($alpha);
- $rcosa= $minradius * $cosalpha;
- $k= ev_compose({}, $c, { X=>$rcosa, Y=>$rpmsina, A=>0 });
- $l= ev_compose({}, $d, { X=>-$rcosa, Y=>-$rpmsina, A=>0 });
- $k->{A}= $l->{A}= ev_bearing($k,$l);
- o(" alpha=".ang2deg($alpha)." kl^=".ang2deg($k->{A})."\n");
- $linelen= v_dist($k,$l);
- }
- $path= [{ T => Arc, F => $from, C => $c,
- R =>$lr*$minradius,
- D => -$lr * a_normalise
- ($lr * ($from->{A} - $k->{A}), 0) },
- { T => Line, A => $k, B => $l, L => $linelen },
- { T => Arc, F => $l, C => $d,
- R => $inv*$lr*$minradius,
- D => -$lr*$inv * a_normalise
- (-$lr*$inv * ($to->{A} - $l->{A}), 0) }];
- push @paths, $path;
- push @solkinds, [ 'arcsline', ($inv<0 ? 'cross' : 'loop') ];
+ $r= -0.5 * (-$b + $m*$d) / $a;
+ $radius= -$r * $distfact;
+ o("% twoarcs $m radius $radius ");
+ if (abs($radius) < $minradius) { o("too-small\n"); next; }
+ $cfrom= ev_compose({}, $from, { X=>0, Y=>-$radius, A=>-0.5*$pi });
+ $cto= ev_compose({}, $to, { X=>0, Y=> $radius, A=> 0.5*$pi });
+ $midpt= ev_lincomb({}, $cfrom, $cto, 0.5);
+ $reverse= signum($r);
+ if ($reverse<0) {
+ $cfrom->{A} += $pi;
+ $cto->{A} += $pi;
+ }
+ $delta1= ev_bearing($cfrom, $midpt) - $cfrom->{A};
+ $delta2= ev_bearing($cto, $midpt) - $cto->{A};
+ o("ok deltas ".ang2deg($delta1)." ".ang2deg($delta2)."\n");
+ if ($reverse<0) {
+ $delta1 -= 2*$pi;
+ $delta2 -= 2*$pi;
+ }
+ my ($fs);
+ $path= [{ T=>Arc, F=>$from, C=>$cfrom, R=> $radius, D=>$delta1 },
+ { T=>Arc, F=>$to, C=>$cto, R=>-$radius, D=>$delta2 }];
+ push @$results, { Path => $path,
+ SolKinds => [ 'twoarcs', 'cross' ] };
+ }
+}
+
+sub joins_arcsline ($$$$) {
+ my ($results, $from,$to,$minradius) = @_;
+ # two circular arcs of specified radius
+ # with an intervening straight
+ my ($lr,$inv, $c,$d,$alpha,$t,$k,$l,$rpmsina,$rcosa,$linelen, $path);
+ if ($minradius<=1e-6) { o("% arcsline no-radius\n"); return; }
+ foreach $lr (qw(-1 +1)) {
+ foreach $inv (qw(-1 +1)) {
+ $c=ev_compose({},$from,{X=>0,Y=>-$lr*$minradius, A=>0 });
+ $d=ev_compose({},$to,{X=>0, Y=>-$inv*$lr*$minradius, A=>$pi });
+ $t= v_dist($c,$d);
+ o("% arcsline $lr $inv t=$t ");
+ if ($t < 1e-6) { o("concentric"); next; }
+ $c->{A}= $d->{A}= ev_bearing($c,$d);
+ o("bearing ".ang2deg($c->{A}));
+ if ($inv>0) {
+ o("\n");
+ $k= ev_compose({}, $c, { X=>0, Y=>$lr*$minradius, A=>0 });
+ $l= ev_compose({}, $d, { X=>0, Y=>$lr*$minradius, A=>0 });
+ $linelen= $t;
+ } else {
+ my ($cosalpha) = 2.0 * $minradius / $t;
+ if ($cosalpha > (1.0 - 1e-6)) { o(" too-close\n"); next; }
+ $alpha= acos($cosalpha);
+ $rpmsina= $lr * $minradius * sin($alpha);
+ $rcosa= $minradius * $cosalpha;
+ $k= ev_compose({}, $c, { X=>$rcosa, Y=>$rpmsina, A=>0 });
+ $l= ev_compose({}, $d, { X=>-$rcosa, Y=>-$rpmsina, A=>0 });
+ $k->{A}= $l->{A}= ev_bearing($k,$l);
+ o(" alpha=".ang2deg($alpha)." kl^=".ang2deg($k->{A})."\n");
+ $linelen= v_dist($k,$l);
}
+ $path= [{ T => Arc, F => $from, C => $c,
+ R =>$lr*$minradius,
+ D => -$lr * a_normalise
+ ($lr * ($from->{A} - $k->{A}), 0) },
+ { T => Line, A => $k, B => $l, L => $linelen },
+ { T => Arc, F => $l, C => $d,
+ R => $inv*$lr*$minradius,
+ D => -$lr*$inv * a_normalise
+ (-$lr*$inv * ($to->{A} - $l->{A}), 0) }];
+ push @$results,
+ { Path => $path,
+ SolKinds => [ 'arcsline', ($inv<0 ? 'cross' : 'loop') ] };
}
}
- {
- # one circular arc and a straight line
- my ($inv,$a,$b, $gamma, $kl_len,$cosag,$aj_len);
- my ($jl_len, $j,$l,$cl_len,$c, $radius);
-# foreach $inv (qw(-1 +1)) {
- { $inv=-1;
- $a= $from;
- $b= { %$to }; $b->{A} += $pi;
- ($a,$b)=($b,$a) if $inv<0;
-print STDERR "a=".loc2dbg($a)." b=".loc2dbg($b)."\n";
-# $b= ev_compose({}, $b, {X=>0,Y=>0,A=>$pi});
- $gamma= 0.5 * ($a->{A} + $b->{A});
- $gamma += $pi if a_normalise($gamma - $a->{A}, 0) >= $pi;
- $kl_len= ($b->{X} - $a->{X}) * cos($gamma)
- + ($b->{Y} - $a->{Y}) * sin($gamma);
- o("% arcline $inv gamma=".ang2deg($gamma)." |kl|=$kl_len ");
-# if ($kl_len < 1e-6) { o("nope\n"); next; }
- $cosag= cos($a->{A} - $gamma);
- o("cos(a-g)=$cosag ");
-# if ($cosag < 1e-6) { o("nope\n"); next; }
- $aj_len= $kl_len/$cosag;
- o("|aj|=$aj_len\n");
- $j= ev_compose({}, $a, { X=>$aj_len, Y=>0, A=>0 });
- $l= ev_lincomb({}, $j,$b,0.5);
- $jl_len= v_dist($j,$l);
- $cl_len= $jl_len * tan($a->{A} - $gamma);
- $radius= $jl_len / $cosag;
- $c= ev_compose({}, $l, { X=>-$cl_len, Y=>0, A=>0 });
- push @paths, [{ T => Line, A => $a, B => $j, L => $aj_len },
- { T => Arc, F => $j, C => $c, R => $inv*$radius,
- D => -$inv * a_normalise
- (-$inv * ($b->{A} + $pi - $a->{A}), 0) }];
- push @solkinds, [ 'arcline' ];
+}
+
+sub joins_arcline ($$$$) {
+ my ($results, $from,$to,$minradius) = @_;
+ # one circular arc and a straight line
+ my ($swap,$echoice,$path, $ap,$bp,$av,$bv, $e,$f, $ae,$af,$afae);
+ my ($dak,$ak,$kj,$k,$j,$aja,$jl,$l,$jc,$lc,$c,$rj,$rb);
+ foreach $swap (qw(-1 +1)) {
+# {
+# $swap=+1;
+ foreach $echoice (qw(0 1)) {
+# {
+# $echoice=0;
+ $ap= $from; $bp= { %$to }; $bp->{A} += $pi;
+ ($ap,$bp)= ($bp,$ap) if $swap<0;
+ $av= ev_byang({}, $ap->{A});
+ $bv= ev_byang({}, $bp->{A});
+ $e= ev_byang({}, 0.5 * ($ap->{A} + $bp->{A} + $echoice * $pi));
+ $f= v_rotateright($e);
+ o("% arcline $swap $echoice e ".loc2dbg($e)."\n");
+# o("% arcline $swap $echoice f ".loc2dbg($f)."\n");
+# o("% arcline $swap $echoice av ".loc2dbg($av)."\n");
+ $ae= v_dotproduct($av,$e);
+ $af= v_dotproduct($av,$f);
+ o("% arcline $swap $echoice a.e=$ae a.f=$af ");
+ if (abs($ae) < 1e-6) { o(" singular\n"); next;
+ o("%");}
+ $afae= $af/$ae;
+ o("a.f/a.e=$afae\n");
+ $dak= v_dotproduct(v_subtract($ap,$bp), $e);
+ $ak= v_scalarmult($dak, $e);
+ $kj= v_scalarmult($dak * $afae, $f);
+ $k= v_add($ap, $ak);
+ $j= v_add($k, $kj);
+ $aja= v_dotproduct(v_subtract($ap,$j), $av);
+ o("% arcline $swap $echoice d_ak=$dak aj.a=$aja ");
+ if ($aja < 0) { o(" backwards aj\n"); next;
+ o("%");}
+ $jl= v_scalarmult(0.5, v_subtract($j, $bp));
+ $lc= v_scalarmult(-v_dotproduct($jl, $f) * $afae, $e);
+ $l= v_add($j, $jl);
+ $c= v_add($l, $lc);
+ $rj= v_dotproduct(v_subtract($j,$c), v_rotateright($av));
+ $rb= v_dotproduct(v_subtract($c,$bp), v_rotateright($bv));
+ o("r_j=$rj r_b=$rb ");
+ if ($rj * $rb < 0) { o(" backwards b\n"); next;
+ o("%");}
+ $j->{A}= $ap->{A};
+ $c->{A}= 0;
+ $path= [{ T => Line, A => $ap, B => $j, L => $aja },
+ { T => Arc, F => $j, C => $c, R => $rj,
+ D => -signum($rj) * a_normalise
+ (-signum($rj) * ($bp->{A} + $pi - $j->{A}), 0) }];
+ $path= [ reverse @$path ] if $swap<0;
+ push @$results, { Path => $path, SolKinds => [ 'arcline' ] };
}
}
+}
+
+sub cmd_join {
+ my ($from,$to,$minradius);
+ my (@results,$result);
my ($path,$segment,$bestpath,$len,$scores,$bestscores,@bends,$skl);
my ($crit,$cs,$i,$cmp);
- foreach $path (@paths) {
- $skl= shift @solkinds;
+ $from= can(\&cva_idex);
+ $to= can(\&cva_idex);
+ $minradius= can(\&cva_len);
+ o("% join ".loc2dbg($from)."..".loc2dbg($to)." $minradius\n");
+# joins_twoarcs(\@results, $from,$to,$minradius);
+# joins_arcsline(\@results, $from,$to,$minradius);
+ joins_arcline(\@results, $from,$to,$minradius);
+ foreach $result (@results) {
+ $path= $result->{Path};
+ $skl= $result->{SolKinds};
o("% possible path @$skl $path\n");
$len= 0;
@bends= ();
~ijackson / trains.git / commitdiff