(* * Navigation2.m * * by K. Pittenger, D. Schlingmann, and D. Whitecotton * * This package contains the function * navigate2[stationlist] which * sets up the graphical simulation of an * object (a blue dot) moving in a preset environment * (two rooms) from its starting point * (the first element of stationlist) * over intermediate destinations * (the intermediate elements of stationlist) * to its final destination (the last * element of stationlist). The moving object * maneuvers around preset obstacles (furniture). *) (* * Prepend the context Navigation2` to $ContextPath. *) BeginPackage["Navigation2`"] EndPackage[] navigate2::usage = "navigate2[stationlist] \ sets up the graphical simulation of an \ object (a blue dot) moving in a preset environment \ (two rooms) from its starting point \ (the first element of stationlist) over \ intermediate destinations (the intermediate \ elements of stationlist) to its final destination \ (the last element of stationlist). \ The moving object maneuvers around preset \ obstacles (furniture). \ stationlist lists a finite number of points. \ The first point on the list represents the \ starting point. The second point represents \ the first destination of the travelling object, \ and the last point on the list represents the \ final destination."; (* * Global objects. *) navigate2; Begin["Navigation2`Private`"]; (* * Local objects. *) `navigate; `obstacles; `inroom; (* * Define navigate, obstacles, and inroom. *) (* navigate[stationlist, obstaclelist] sets up the graphical simulation of an object (a blue dot) moving from its starting point (the first element of stationlist) over intermediate destinations (the intermediate elements of stationlist) to its final destination (the last element of stationlist). The moving object avoids the obstacles listed in obstaclelist. The program assumes that two obstacles don't intersect with each other and that there is enough space between two obstacles for the moving object to move through. stationlist lists a finite number of points. The first point on the list represents the starting point. The second point represents the first destination of the travelling object, and the last point on the list represents the final destination. obstaclelist lists a finite number of lists. Each such list contains two points which represent a cuboid. The two points portray {xmin, ymin, zmin} and {xmax, ymax, zmax} of the cuboid. faultycub checks if the inputs for the obstacles are correct. c represents the size of the corpus of the moving object. boxinbox checks if two obstacles intersect each other. statinbox checks if a station is in an obstacle. oldstationlist is used for the graphics only. inroom1 & inroom2 check whether a station is in room1 or room2. g codes the stations with respect to their rooms. newlist adds to stationlist at the right places the coordinates for the door. newstationlist makes sure that a station in newlist is not listed twice in sequence. norm computes the length of the vector v. straightline[t, v, w] represents the equation of the straight line through v and w at time t. The absolute value of t also portrays the distance from v. step represents the length of a single step of the moving object. detourlist adds at the right places further intermediate destinations to stationlist which are necessary to make the object go around the given obstacles. This new created list is called detour. myposition[t, stationlist] computes the position of an object at time t. The object travels at a constant speed of one distance-unit per one time-unit, e.g., 1 foot/sec. stationsgr and stationsgraph set up the three dimensional graphical objects of the destination points. cornersgr and cornersgraph supply the same frame for each picture. dim computes an appropriate dimension of the frame depending on the elements of detour and obstaclelist. totaltime represents the total time it takes to move the object from the starting point to the final destination. *) navigate[stationlist_, obstaclelist_] := Module[ {`faultycub, `c, `boxinbox, `statinbox, `oldstationlist, `inroom1, `inroom2, `g, `newlist, `newstationlist, `norm, `straightline, `step, `detourlist, `detour, `myposition, `stationsgr, `stationsgraph, `obstaclesgr, `obstaclesgraph, `cornersgr, `cornersgraph, `dim, `totaltime}, faultycub[cub_] := N[(cub[[1,1]] >= cub[[2,1]]) || (cub[[1,2]] >= cub[[2,2]]) || (cub[[1,3]] >= cub[[2,3]])]; c = 0.2; boxinbox[b1_, b2_] := N[((((b1[[1,1]]-c) >= (b2[[1,1]]-c)) && ((b1[[1,1]]-c) <= (b2[[2,1]]+c))) || (((b1[[2,1]]+c) >= (b2[[1,1]]-c)) && ((b1[[2,1]]+c) <= (b2[[2,1]]+c))) || (((b1[[1,1]]-c) <= (b2[[1,1]]-c)) && ((b1[[2,1]]+c) >= (b2[[2,1]]+c)))) && ((((b1[[1,2]]-c) >= (b2[[1,2]]-c)) && ((b1[[1,2]]-c) <= (b2[[2,2]]+c))) || (((b1[[2,2]]+c) >= (b2[[1,2]]-c)) && ((b1[[2,2]]+c) <= (b2[[2,2]]+c))) || (((b1[[1,2]]-c) <= (b2[[1,2]]-c)) && ((b1[[2,2]]+c) >= (b2[[2,2]]+c)))) && ((((b1[[1,3]]-c) >= (b2[[1,3]]-c)) && ((b1[[1,3]]-c) <= (b2[[2,3]]+c))) || (((b1[[2,3]]+c) >= (b2[[1,3]]-c)) && ((b1[[2,3]]+c) <= (b2[[2,3]]+c))) || (((b1[[1,3]]-c) <= (b2[[1,3]]-c)) && ((b1[[2,3]]+c) >= (b2[[2,3]]+c))))]; statinbox[v_,b_] := N[((b[[1,1]]-c) <= v[[1]]) && (v[[1]] <= (b[[2,1]]+c)) && ((b[[1,2]]-c) <= v[[2]]) && (v[[2]] <= (b[[2,2]]+c)) && ((b[[1,3]]-c) <= v[[3]]) && (v[[3]] <= (b[[2,3]]+c))]; If[ (Length[obstaclelist] >= 1) && (Intersection[ {True}, Union[ Flatten[ Table[ faultycub[obstaclelist[[i]]], {i, Length[obstaclelist]}]]]] == {True}), Print["Error: Some obstacles have wrong arguments."], If[ (Length[obstaclelist] >= 2) && (Intersection[ {True}, Union[ Flatten[ Table[ Table[boxinbox[obstaclelist[[i]], obstaclelist[[j]]], {j, i+1, Length[obstaclelist]}], {i, Length[obstaclelist]-1}]]]] == {True}), Print["Error: The obstacles are not separate."], If[ (Length[obstaclelist] >= 1) && (Intersection[ {True}, Union[ Flatten[ Table[ Table[statinbox[stationlist[[i]], obstaclelist[[j]]], {j, Length[obstaclelist]}], {i, Length[stationlist]}]]]] == {True}), Print["Error: A station is in an obstacle."], oldstationlist = stationlist; inroom1[v_] := N[(0 < v[[1]]) && (v[[1]] < 10) && (0 < v[[2]]) && (v[[2]] < 20)]; inroom2[v_] := N[(10 < v[[1]]) && (v[[1]] < 20) && (0 < v[[2]]) && (v[[2]] < 20)]; g[v_] := If[inroom1[v],1,2]; newlist = {oldstationlist[[1]]}; Do[ If[(g[oldstationlist[[i]]] == g[oldstationlist[[i+1]]]), newlist = Append[newlist,oldstationlist[[i+1]]], If[g[oldstationlist[[i]]] == 1, newlist = Join[newlist,{{N[39/4],10.,.75}, {N[41/4],10.,.75},oldstationlist[[i+1]]}], newlist = Join[newlist,{{N[41/4],10.,.75}, {N[39/4],10.,.75},oldstationlist[[i+1]]}]]], {i,1,Length[oldstationlist]-1}]; newstationlist = {newlist[[1]]}; Do[ If[N[Last[newstationlist] != newlist[[i+1]]], newstationlist = Append[newstationlist,newlist[[i+1]]]], {i,1,Length[newlist]-1}]; norm[u_] := Sqrt[u[[1]]^2+u[[2]]^2+u[[3]]^2]; straightline[s_,x_,y_] := x+s*(y-x)/norm[y-x]; step = 0.4; detourlist[mystationlist_?((Length[#] == 2)&), myobstaclelist_] := Module[ {`statlist, `obslist, (* `norm, `straightline, `c, *) `v, `w, `detour, `positiveobstaclelist, `poblist, `posoblist, `negoblist, `detourcornerpts, `detourcopts, `pob, `myline, `alterpoblist, `upgrade}, statlist = mystationlist; obslist = myobstaclelist; (* norm[u_] := Sqrt[u[[1]]^2+u[[2]]^2+u[[3]]^2]; straightline[s_,x_,y_] := x+s*(y-x)/norm[y-x]; c = 0.2; *) v = statlist[[1]]; w = statlist[[2]]; detour ={v}; positiveobstaclelist[ localstationlist_?((Length[#] == 2)&), localobstaclelist_] := Module[ {`statlist, `obslist, (* `norm, `straightline *) `myline, `solutions, `sol, `f, `mylist, `poblist, `i}, statlist = localstationlist; obslist = localobstaclelist; (* norm[v_] := Sqrt[v[[1]]^2+v[[2]]^2+v[[3]]^2]; straightline[s_,x_,y_] := x+s*(y-x)/norm[y-x]; *) myline[t_] := straightline[t, statlist[[1]], statlist[[2]]]; solutions = {}; If[obslist != {}, Do[ Do[ Do[ sol = Solve[myline[t][[j]] == obslist[[i,k,j]],t]; f[1,1] = 2; f[1,2] = 3; f[2,1] = 1; f[2,2] = 3; f[3,1] = 1; f[3,2] = 2; If[ (sol != {}) && (sol != {{}}) && (sol[[1,1,2]] != ComplexInfinity) && N[(sol[[1,1,2]] >= 0) && (sol[[1,1,2]] <= norm[statlist[[2]]-statlist[[1]]]) && (myline[sol[[1,1,2]]][[f[j,1]]] >= obslist[[i,1,f[j,1]]] && myline[sol[[1,1,2]]][[f[j,1]]] <= obslist[[i,2,f[j,1]]]) && (myline[sol[[1,1,2]]][[f[j,2]]] >= obslist[[i,1,f[j,2]]] && myline[sol[[1,1,2]]][[f[j,2]]] <= obslist[[i,2,f[j,2]]])], AppendTo[solutions, {{sol[[1,1,2]]}, obslist[[i]], {myline[sol[[1,1,2]]]}}] ], {j, 3} ], {k,2} ], {i, Length[obslist]} ]; mylist = Sort[Union[solutions], (N[#1[[1,1]] <= #2[[1,1]]])&]; poblist = {}; i = 1; If[Length[mylist] >= 1, If[Length[mylist] == 1, poblist = mylist, While[(i < Length[mylist]), If[mylist[[i,2]] != mylist[[i+1,2]], If[(i+1) == Length[mylist], poblist = Join[poblist, {mylist[[i]], mylist[[i+1]]}]; i = i+1, poblist = Append[poblist, mylist[[i]]]]; i = i+1, poblist = Append[poblist, {Join[mylist[[i,1]], mylist[[i+1,1]]], mylist[[i,2]], Join[mylist[[i,3]], mylist[[i+1,3]]]}]; i = i+2]]]]; poblist, poblist = {}] ]; poblist = positiveobstaclelist[statlist, obslist]; posoblist = Table[poblist[[i,2]], {i, Length[poblist]}]; negoblist = Complement[obslist, posoblist]; detourcornerpts[start_, end_, poblistelement_] := Module[ {`pob1, `v1, `w1, (* `c, `norm, `straightline, *) `myline, `mytimes, `hits, `xl, `xh, `yl, `yh, `zl, `zh, `sides, `n}, pob1 = poblistelement; v1 = start; w1 = end; (* c = 1/5; norm[u_] := Sqrt[u[[1]]^2+u[[2]]^2+u[[3]]^2]; straightline[s_,x_,y_] := x+s*(y-x)/norm[y-x]; *) myline[t_] := straightline[t,v1,w1]; mytimes = pob1[[1]]; hits = pob1[[3]]; xl = pob1[[2,1,1]]; xh = pob1[[2,2,1]]; yl = pob1[[2,1,2]]; yh = pob1[[2,2,2]]; zl = pob1[[2,1,3]]; zh = pob1[[2,2,3]]; sides = {}; n = Length[pob1[[3]]]; Do[ Which[ ((Abs[N[pob1[[2,1,1]]-pob1[[3,i,1]]]] <= Abs[N[pob1[[2,1,2]]-pob1[[3,i,2]]]]) && (Abs[N[pob1[[2,1,1]]-pob1[[3,i,1]]]] <= Abs[N[pob1[[2,1,3]]-pob1[[3,i,3]]]]) && (Abs[N[pob1[[2,1,1]]-pob1[[3,i,1]]]] <= Abs[N[pob1[[2,2,1]]-pob1[[3,i,1]]]]) && (Abs[N[pob1[[2,1,1]]-pob1[[3,i,1]]]] <= Abs[N[pob1[[2,2,2]]-pob1[[3,i,2]]]]) && (Abs[N[pob1[[2,1,1]]-pob1[[3,i,1]]]] <= Abs[N[pob1[[2,2,3]]-pob1[[3,i,3]]]])), sides = Append[sides, {1,1}], ((Abs[N[pob1[[2,1,2]]-pob1[[3,i,2]]]] <= Abs[N[pob1[[2,1,1]]-pob1[[3,i,1]]]]) && (Abs[N[pob1[[2,1,2]]-pob1[[3,i,2]]]] <= Abs[N[pob1[[2,1,3]]-pob1[[3,i,3]]]]) && (Abs[N[pob1[[2,1,2]]-pob1[[3,i,2]]]] <= Abs[N[pob1[[2,2,1]]-pob1[[3,i,1]]]]) && (Abs[N[pob1[[2,1,2]]-pob1[[3,i,2]]]] <= Abs[N[pob1[[2,2,2]]-pob1[[3,i,2]]]]) && (Abs[N[pob1[[2,1,2]]-pob1[[3,i,2]]]] <= Abs[N[pob1[[2,2,3]]-pob1[[3,i,3]]]])), sides = Append[sides, {1,2}], ((Abs[N[pob1[[2,1,3]]-pob1[[3,i,3]]]] <= Abs[N[pob1[[2,1,1]]-pob1[[3,i,1]]]]) && (Abs[N[pob1[[2,1,3]]-pob1[[3,i,3]]]] <= Abs[N[pob1[[2,1,2]]-pob1[[3,i,2]]]]) && (Abs[N[pob1[[2,1,3]]-pob1[[3,i,3]]]] <= Abs[N[pob1[[2,2,1]]-pob1[[3,i,1]]]]) && (Abs[N[pob1[[2,1,3]]-pob1[[3,i,3]]]] <= Abs[N[pob1[[2,2,2]]-pob1[[3,i,2]]]]) && (Abs[N[pob1[[2,1,3]]-pob1[[3,i,3]]]] <= Abs[N[pob1[[2,2,3]]-pob1[[3,i,3]]]])), sides = Append[sides, {1,3}], ((Abs[N[pob1[[2,2,1]]-pob1[[3,i,1]]]] <= Abs[N[pob1[[2,1,1]]-pob1[[3,i,1]]]]) && (Abs[N[pob1[[2,2,1]]-pob1[[3,i,1]]]] <= Abs[N[pob1[[2,1,2]]-pob1[[3,i,2]]]]) && (Abs[N[pob1[[2,2,1]]-pob1[[3,i,1]]]] <= Abs[N[pob1[[2,1,3]]-pob1[[3,i,3]]]]) && (Abs[N[pob1[[2,2,1]]-pob1[[3,i,1]]]] <= Abs[N[pob1[[2,2,2]]-pob1[[3,i,2]]]]) && (Abs[N[pob1[[2,2,1]]-pob1[[3,i,1]]]] <= Abs[N[pob1[[2,2,3]]-pob1[[3,i,3]]]])), sides = Append[sides, {2,1}], ((Abs[N[pob1[[2,2,2]]-pob1[[3,i,2]]]] <= Abs[N[pob1[[2,1,1]]-pob1[[3,i,1]]]]) && (Abs[N[pob1[[2,2,2]]-pob1[[3,i,2]]]] <= Abs[N[pob1[[2,1,2]]-pob1[[3,i,2]]]]) && (Abs[N[pob1[[2,2,2]]-pob1[[3,i,2]]]] <= Abs[N[pob1[[2,1,3]]-pob1[[3,i,3]]]]) && (Abs[N[pob1[[2,2,2]]-pob1[[3,i,2]]]] <= Abs[N[pob1[[2,2,1]]-pob1[[3,i,1]]]]) && (Abs[N[pob1[[2,2,2]]-pob1[[3,i,2]]]] <= Abs[N[pob1[[2,2,3]]-pob1[[3,i,3]]]])), sides = Append[sides, {2,2}], ((Abs[N[pob1[[2,2,3]]-pob1[[3,i,3]]]] <= Abs[N[pob1[[2,1,1]]-pob1[[3,i,1]]]]) && (Abs[N[pob1[[2,2,3]]-pob1[[3,i,3]]]] <= Abs[N[pob1[[2,1,2]]-pob1[[3,i,2]]]]) && (Abs[N[pob1[[2,2,3]]-pob1[[3,i,3]]]] <= Abs[N[pob1[[2,1,3]]-pob1[[3,i,3]]]]) && (Abs[N[pob1[[2,2,3]]-pob1[[3,i,3]]]] <= Abs[N[pob1[[2,2,1]]-pob1[[3,i,1]]]]) && (Abs[N[pob1[[2,2,3]]-pob1[[3,i,3]]]] <= Abs[N[pob1[[2,2,2]]-pob1[[3,i,2]]]])), sides = Append[sides, {2,3}] ], {i, n} ]; If[Length[sides] == 2, Which[ (sides[[1,2]] == 1) && (sides[[2]] == {1,2}), {{myline[mytimes[[1]]-c][[1]], yl-c, hits[[2,3]]}}, (sides[[1,2]] == 1) && (sides[[2]] == {2,2}), {{myline[mytimes[[1]]-c][[1]], yh+c, hits[[2,3]]}}, (sides[[1,2]] == 1) && (sides[[2]] == {1,3}), {{myline[mytimes[[1]]-c][[1]], hits[[2,2]], zl-c}}, (sides[[1,2]] == 1) && (sides[[2]] == {2,3}), {{myline[mytimes[[1]]-c][[1]], hits[[2,2]], zh+c}}, (sides[[1,2]] == 2) && (sides[[2]] == {1,1}), {{xl-c, myline[mytimes[[1]]-c][[2]], hits[[2,3]]}}, (sides[[1,2]] == 2) && (sides[[2]] == {2,1}), {{xh+c, myline[mytimes[[1]]-c][[2]], hits[[2,3]]}}, (sides[[1,2]] == 2) && (sides[[2]] == {1,3}), {{hits[[2,1]], myline[mytimes[[1]]-c][[2]], zl-c}}, (sides[[1,2]] == 2) && (sides[[2]] == {2,3}), {{hits[[2,1]], myline[mytimes[[1]]-c][[2]], zh+c}}, (sides[[1,2]] == 3) && (sides[[2]] == {1,1}), {{xl-c, hits[[2,2]], myline[mytimes[[1]]-c][[3]]}}, (sides[[1,2]] == 3) && (sides[[2]] == {2,1}), {{xh+c, hits[[2,2]], myline[mytimes[[1]]-c][[3]]}}, (sides[[1,2]] == 3) && (sides[[2]] == {1,2}), {{hits[[2,1]], yl-c, myline[mytimes[[1]]-c][[3]]}}, (sides[[1,2]] == 3) && (sides[[2]] == {2,2}), {{hits[[2,1]], yh+c, myline[mytimes[[1]]-c][[3]]}}, (sides[[1,2]] == 1) && (sides[[2,2]] == 1), Which[ N[(((norm[start-{myline[mytimes[[1]]-c][[1]],yl-c, hits[[1,3]]}] + norm[{myline[mytimes[[1]]-c][[1]],yl-c, hits[[1,3]]}- {myline[mytimes[[2]]+c][[1]],yl-c, hits[[2,3]]}] + norm[{myline[mytimes[[2]]+c][[1]],yl-c, hits[[2,3]]}-end]) <= (norm[start-{myline[mytimes[[1]]-c][[1]],yh+c, hits[[1,3]]}] + norm[{myline[mytimes[[1]]-c][[1]],yh+c, hits[[1,3]]}- {myline[mytimes[[2]]+c][[1]],yh+c, hits[[2,3]]}] + norm[{myline[mytimes[[2]]+c][[1]],yh+c, hits[[2,3]]}-end])) (* && ((norm[start-{myline[mytimes[[1]]-c][[1]],yl-c, hits[[1,3]]}] + norm[{myline[mytimes[[1]]-c][[1]],yl-c, hits[[1,3]]}- {myline[mytimes[[2]]+c][[1]],yl-c, hits[[2,3]]}] + norm[{myline[mytimes[[2]]+c][[1]],yl-c, hits[[2,3]]}-end]) <= (norm[start-{myline[mytimes[[1]]-c][[1]],hits[[1,2]],zl-c}] + norm[{myline[mytimes[[1]]-c][[1]], hits[[1,2]],zl-c}- {myline[mytimes[[2]]+c][[1]], hits[[2,2]],zl-c}] + norm[{myline[mytimes[[2]]+c][[1]], hits[[2,2]],zl-c}-end])) && ((norm[start-{myline[mytimes[[1]]-c][[1]],yl-c, hits[[1,3]]}] + norm[{myline[mytimes[[1]]-c][[1]],yl-c, hits[[1,3]]}- {myline[mytimes[[2]]+c][[1]],yl-c, hits[[2,3]]}] + norm[{myline[mytimes[[2]]+c][[1]],yl-c, hits[[2,3]]}-end]) <= (norm[start-{myline[mytimes[[1]]-c][[1]],hits[[1,2]],zh+c}] + norm[{myline[mytimes[[1]]-c][[1]], hits[[1,2]],zh+c}- {myline[mytimes[[2]]+c][[1]], hits[[2,2]],zh+c}] + norm[{myline[mytimes[[2]]+c][[1]], hits[[2,2]],zh+c}-end])) *) )], {{myline[mytimes[[1]]-c][[1]],yl-c, hits[[1,3]]}, {myline[mytimes[[2]]+c][[1]],yl-c, hits[[2,3]]}}, N[(((norm[start-{myline[mytimes[[1]]-c][[1]],yh+c, hits[[1,3]]}] + norm[{myline[mytimes[[1]]-c][[1]],yh+c, hits[[1,3]]}- {myline[mytimes[[2]]+c][[1]],yh+c, hits[[2,3]]}] + norm[{myline[mytimes[[2]]+c][[1]],yh+c, hits[[2,3]]}-end]) <= (norm[start-{myline[mytimes[[1]]-c][[1]],yl-c, hits[[1,3]]}] + norm[{myline[mytimes[[1]]-c][[1]],yl-c, hits[[1,3]]}- {myline[mytimes[[2]]+c][[1]],yl-c, hits[[2,3]]}] + norm[{myline[mytimes[[2]]+c][[1]],yl-c, hits[[2,3]]}-end])) (* && ((norm[start-{myline[mytimes[[1]]-c][[1]],yh+c, hits[[1,3]]}] + norm[{myline[mytimes[[1]]-c][[1]],yh+c, hits[[1,3]]}- {myline[mytimes[[2]]+c][[1]],yh+c, hits[[2,3]]}] + norm[{myline[mytimes[[2]]+c][[1]],yh+c, hits[[2,3]]}-end]) <= (norm[start-{myline[mytimes[[1]]-c][[1]],hits[[1,2]],zl-c}] + norm[{myline[mytimes[[1]]-c][[1]], hits[[1,2]],zl-c}- {myline[mytimes[[2]]+c][[1]], hits[[2,2]],zl-c}] + norm[{myline[mytimes[[2]]+c][[1]], hits[[2,2]],zl-c}-end])) && ((norm[start-{myline[mytimes[[1]]-c][[1]],yh+c, hits[[1,3]]}] + norm[{myline[mytimes[[1]]-c][[1]],yh+c, hits[[1,3]]}- {myline[mytimes[[2]]+c][[1]],yh+c, hits[[2,3]]}] + norm[{myline[mytimes[[2]]+c][[1]],yh+c, hits[[2,3]]}-end]) <= (norm[start-{myline[mytimes[[1]]-c][[1]],hits[[1,2]],zh+c}] + norm[{myline[mytimes[[1]]-c][[1]], hits[[1,2]],zh+c}- {myline[mytimes[[2]]+c][[1]], hits[[2,2]],zh+c}] + norm[{myline[mytimes[[2]]+c][[1]], hits[[2,2]],zh+c}-end])) *) )], {{myline[mytimes[[1]]-c][[1]],yh+c, hits[[1,3]]}, {myline[mytimes[[2]]+c][[1]],yh+c, hits[[2,3]]}} (* , N[(((norm[start-{myline[mytimes[[1]]-c][[1]],hits[[1,2]],zl-c}] + norm[{myline[mytimes[[1]]-c][[1]], hits[[1,2]],zl-c}- {myline[mytimes[[2]]+c][[1]], hits[[2,2]],zl-c}] + norm[{myline[mytimes[[2]]+c][[1]], hits[[2,2]],zl-c}-end]) <= (norm[start-{myline[mytimes[[1]]-c][[1]],hits[[1,2]],zh+c}] + norm[{myline[mytimes[[1]]-c][[1]], hits[[1,2]],zh+c}- {myline[mytimes[[2]]+c][[1]], hits[[2,2]],zh+c}] + norm[{myline[mytimes[[2]]+c][[1]], hits[[2,2]],zh+c}-end])) && ((norm[start-{myline[mytimes[[1]]-c][[1]],hits[[1,2]],zl-c}] + norm[{myline[mytimes[[1]]-c][[1]], hits[[1,2]],zl-c}- {myline[mytimes[[2]]+c][[1]], hits[[2,2]],zl-c}] + norm[{myline[mytimes[[2]]+c][[1]], hits[[2,2]],zl-c}-end]) <= (norm[start-{myline[mytimes[[1]]-c][[1]],yl-c, hits[[1,3]]}] + norm[{myline[mytimes[[1]]-c][[1]],yl-c, hits[[1,3]]}- {myline[mytimes[[2]]+c][[1]],yl-c, hits[[2,3]]}] + norm[{myline[mytimes[[2]]+c][[1]],yl-c, hits[[2,3]]}-end])) && ((norm[start-{myline[mytimes[[1]]-c][[1]],hits[[1,2]],zl-c}] + norm[{myline[mytimes[[1]]-c][[1]], hits[[1,2]],zl-c}- {myline[mytimes[[2]]+c][[1]], hits[[2,2]],zl-c}] + norm[{myline[mytimes[[2]]+c][[1]], hits[[2,2]],zl-c}-end]) <= (norm[start-{myline[mytimes[[1]]-c][[1]],yh+c, hits[[1,3]]}] + norm[{myline[mytimes[[1]]-c][[1]],yh+c, hits[[1,3]]}- {myline[mytimes[[2]]+c][[1]],yh+c, hits[[2,3]]}] + norm[{myline[mytimes[[2]]+c][[1]],yh+c, hits[[2,3]]}-end])))], {{myline[mytimes[[1]]-c][[1]], hits[[1,2]],zl-c}, {myline[mytimes[[2]]+c][[1]], hits[[2,2]],zl-c}}, N[(((norm[start-{myline[mytimes[[1]]-c][[1]],hits[[1,2]],zh+c}] + norm[{myline[mytimes[[1]]-c][[1]], hits[[1,2]],zh+c}- {myline[mytimes[[2]]+c][[1]], hits[[2,2]],zh+c}] + norm[{myline[mytimes[[2]]+c][[1]], hits[[2,2]],zh+c}-end]) <= (norm[start-{myline[mytimes[[1]]-c][[1]],hits[[1,2]],zl-c}] + norm[{myline[mytimes[[1]]-c][[1]], hits[[1,2]],zl-c}- {myline[mytimes[[2]]+c][[1]], hits[[2,2]],zl-c}] + norm[{myline[mytimes[[2]]+c][[1]], hits[[2,2]],zl-c}-end])) && ((norm[start-{myline[mytimes[[1]]-c][[1]],hits[[1,2]],zh+c}] + norm[{myline[mytimes[[1]]-c][[1]], hits[[1,2]],zh+c}- {myline[mytimes[[2]]+c][[1]], hits[[2,2]],zh+c}] + norm[{myline[mytimes[[2]]+c][[1]], hits[[2,2]],zh+c}-end]) <= (norm[start-{myline[mytimes[[1]]-c][[1]],yl-c, hits[[1,3]]}] + norm[{myline[mytimes[[1]]-c][[1]],yl-c, hits[[1,3]]}- {myline[mytimes[[2]]+c][[1]],yl-c, hits[[2,3]]}] + norm[{myline[mytimes[[2]]+c][[1]],yl-c, hits[[2,3]]}-end])) && ((norm[start-{myline[mytimes[[1]]-c][[1]],hits[[1,2]],zh+c}] + norm[{myline[mytimes[[1]]-c][[1]], hits[[1,2]],zh+c}- {myline[mytimes[[2]]+c][[1]], hits[[2,2]],zh+c}] + norm[{myline[mytimes[[2]]+c][[1]], hits[[2,2]],zh+c}-end]) <= (norm[start-{myline[mytimes[[1]]-c][[1]],yh+c, hits[[1,3]]}] + norm[{myline[mytimes[[1]]-c][[1]],yh+c, hits[[1,3]]}- {myline[mytimes[[2]]+c][[1]],yh+c, hits[[2,3]]}] + norm[{myline[mytimes[[2]]+c][[1]],yh+c, hits[[2,3]]}-end])))], {{myline[mytimes[[1]]-c][[1]], hits[[1,2]],zh+c}, {myline[mytimes[[2]]+c][[1]], hits[[2,2]],zh+c}} *) ], (sides[[1,2]] == 2) && (sides[[2,2]] == 2), Which[ N[(((norm[start-{xl-c,myline[mytimes[[1]]-c][[2]], hits[[1,3]]}] + norm[{xl-c,myline[mytimes[[1]]-c][[2]], hits[[1,3]]}- {xl-c,myline[mytimes[[2]]+c][[2]], hits[[2,3]]}] + norm[{xl-c,myline[mytimes[[2]]+c][[2]], hits[[2,3]]}-end]) <= (norm[start-{xh+c,myline[mytimes[[1]]-c][[2]], hits[[1,3]]}] + norm[{xh+c,myline[mytimes[[1]]-c][[2]], hits[[1,3]]}- {xh+c,myline[mytimes[[2]]+c][[2]], hits[[2,3]]}] + norm[{xh+c,myline[mytimes[[2]]+c][[2]], hits[[2,3]]}-end])) (* && ((norm[start-{xl-c,myline[mytimes[[1]]-c][[2]], hits[[1,3]]}] + norm[{xl-c,myline[mytimes[[1]]-c][[2]], hits[[1,3]]}- {xl-c,myline[mytimes[[2]]+c][[2]], hits[[2,3]]}] + norm[{xl-c,myline[mytimes[[2]]+c][[2]], hits[[2,3]]}-end]) <= (norm[start-{hits[[1,1]],myline[mytimes[[1]]-c][[2]], zl-c}] + norm[{hits[[1,1]],myline[mytimes[[1]]-c][[2]], zl-c}- {hits[[2,1]],myline[mytimes[[2]]+c][[2]], zl-c}] + norm[{hits[[2,1]],myline[mytimes[[2]]+c][[2]], zl-c}-end])) && ((norm[start-{xl-c,myline[mytimes[[1]]-c][[2]], hits[[1,3]]}] + norm[{xl-c,myline[mytimes[[1]]-c][[2]], hits[[1,3]]}- {xl-c,myline[mytimes[[2]]+c][[2]], hits[[2,3]]}] + norm[{xl-c,myline[mytimes[[2]]+c][[2]], hits[[2,3]]}-end]) <= (norm[start-{hits[[1,1]],myline[mytimes[[1]]-c][[2]], zh+c}] + norm[{hits[[1,1]],myline[mytimes[[1]]-c][[2]], zh+c}- {hits[[2,1]],myline[mytimes[[2]]+c][[2]], zh+c}] + norm[{hits[[2,1]],myline[mytimes[[2]]+c][[2]], zh+c}-end])) *) )], {{xl-c,myline[mytimes[[1]]-c][[2]], hits[[1,3]]}, {xl-c,myline[mytimes[[2]]+c][[2]], hits[[2,3]]}}, N[(((norm[start-{xh+c,myline[mytimes[[1]]-c][[2]], hits[[1,3]]}] + norm[{xh+c,myline[mytimes[[1]]-c][[2]], hits[[1,3]]}- {xh+c,myline[mytimes[[2]]+c][[2]], hits[[2,3]]}] + norm[{xh+c,myline[mytimes[[2]]+c][[2]], hits[[2,3]]}-end]) <= (norm[start-{xl-c,myline[mytimes[[1]]-c][[2]], hits[[1,3]]}] + norm[{xl-c,myline[mytimes[[1]]-c][[2]], hits[[1,3]]}- {xl-c,myline[mytimes[[2]]+c][[2]], hits[[2,3]]}] + norm[{xl-c,myline[mytimes[[2]]+c][[2]], hits[[2,3]]}-end])) (* && ((norm[start-{xh+c,myline[mytimes[[1]]-c][[2]], hits[[1,3]]}] + norm[{xh+c,myline[mytimes[[1]]-c][[2]], hits[[1,3]]}- {xh+c,myline[mytimes[[2]]+c][[2]], hits[[2,3]]}] + norm[{xh+c,myline[mytimes[[2]]+c][[2]], hits[[2,3]]}-end]) <= (norm[start-{hits[[1,1]],myline[mytimes[[1]]-c][[2]], zl-c}] + norm[{hits[[1,1]],myline[mytimes[[1]]-c][[2]], zl-c}- {hits[[2,1]],myline[mytimes[[2]]+c][[2]], zl-c}] + norm[{hits[[2,1]],myline[mytimes[[2]]+c][[2]], zl-c}-end])) && ((norm[start-{xh+c,myline[mytimes[[1]]-c][[2]], hits[[1,3]]}] + norm[{xh+c,myline[mytimes[[1]]-c][[2]], hits[[1,3]]}- {xh+c,myline[mytimes[[2]]+c][[2]], hits[[2,3]]}] + norm[{xh+c,myline[mytimes[[2]]+c][[2]], hits[[2,3]]}-end]) <= (norm[start-{hits[[1,1]],myline[mytimes[[1]]-c][[2]], zh+c}] + norm[{hits[[1,1]],myline[mytimes[[1]]-c][[2]], zh+c}- {hits[[2,1]],myline[mytimes[[2]]+c][[2]], zh+c}] + norm[{hits[[2,1]],myline[mytimes[[2]]+c][[2]], zh+c}-end])) *) )], {{xh+c,myline[mytimes[[1]]-c][[2]], hits[[1,3]]}, {xh+c,myline[mytimes[[2]]+c][[2]], hits[[2,3]]}} (* , N[(((norm[start-{hits[[1,1]],myline[mytimes[[1]]-c][[2]], zl-c}] + norm[{hits[[1,1]],myline[mytimes[[1]]-c][[2]], zl-c}- {hits[[2,1]],myline[mytimes[[2]]+c][[2]], zl-c}] + norm[{hits[[2,1]],myline[mytimes[[2]]+c][[2]], zl-c}-end]) <= (norm[start-{hits[[1,1]],myline[mytimes[[1]]-c][[2]], zh+c}] + norm[{hits[[1,1]],myline[mytimes[[1]]-c][[2]], zh+c}- {hits[[2,1]],myline[mytimes[[2]]+c][[2]], zh+c}] + norm[{hits[[2,1]],myline[mytimes[[2]]+c][[2]], zh+c}-end])) && ((norm[start-{hits[[1,1]],myline[mytimes[[1]]-c][[2]], zl-c}] + norm[{hits[[1,1]],myline[mytimes[[1]]-c][[2]], zl-c}- {hits[[2,1]],myline[mytimes[[2]]+c][[2]], zl-c}] + norm[{hits[[2,1]],myline[mytimes[[2]]+c][[2]], zl-c}-end]) <= (norm[start-{xl-c,myline[mytimes[[1]]-c][[2]], hits[[1,3]]}] + norm[{xl-c,myline[mytimes[[1]]-c][[2]], hits[[1,3]]}- {xl-c,myline[mytimes[[2]]+c][[2]], hits[[2,3]]}] + norm[{xl-c,myline[mytimes[[2]]+c][[2]], hits[[2,3]]}-end])) && ((norm[start-{hits[[1,1]],myline[mytimes[[1]]-c][[2]], zl-c}] + norm[{hits[[1,1]],myline[mytimes[[1]]-c][[2]], zl-c}- {hits[[2,1]],myline[mytimes[[2]]+c][[2]], zl-c}] + norm[{hits[[2,1]],myline[mytimes[[2]]+c][[2]], zl-c}-end]) <= (norm[start-{xh+c,myline[mytimes[[1]]-c][[2]], hits[[1,3]]}] + norm[{xh+c,myline[mytimes[[1]]-c][[2]], hits[[1,3]]}- {xh+c,myline[mytimes[[2]]+c][[2]], hits[[2,3]]}] + norm[{xh+c,myline[mytimes[[2]]+c][[2]], hits[[2,3]]}-end])))], {{hits[[1,1]],myline[mytimes[[1]]-c][[2]], zl-c}, {hits[[2,1]],myline[mytimes[[2]]+c][[2]], zl-c}}, N[(((norm[start-{hits[[1,1]],myline[mytimes[[1]]-c][[2]], zh+c}] + norm[{hits[[1,1]],myline[mytimes[[1]]-c][[2]], zh+c}- {hits[[2,1]],myline[mytimes[[2]]+c][[2]], zh+c}] + norm[{hits[[2,1]],myline[mytimes[[2]]+c][[2]], zh+c}-end]) <= (norm[start-{hits[[1,1]],myline[mytimes[[1]]-c][[2]], zl-c}] + norm[{hits[[1,1]],myline[mytimes[[1]]-c][[2]], zl-c}- {hits[[2,1]],myline[mytimes[[2]]+c][[2]], zl-c}] + norm[{hits[[2,1]],myline[mytimes[[2]]+c][[2]], zl-c}-end])) && ((norm[start-{hits[[1,1]],myline[mytimes[[1]]-c][[2]], zh+c}] + norm[{hits[[1,1]],myline[mytimes[[1]]-c][[2]], zh+c}- {hits[[2,1]],myline[mytimes[[2]]+c][[2]], zh+c}] + norm[{hits[[2,1]],myline[mytimes[[2]]+c][[2]], zh+c}-end]) <= (norm[start-{xl-c,myline[mytimes[[1]]-c][[2]], hits[[1,3]]}] + norm[{xl-c,myline[mytimes[[1]]-c][[2]], hits[[1,3]]}- {xl-c,myline[mytimes[[2]]+c][[2]], hits[[2,3]]}] + norm[{xl-c,myline[mytimes[[2]]+c][[2]], hits[[2,3]]}-end])) && ((norm[start-{hits[[1,1]],myline[mytimes[[1]]-c][[2]], zh+c}] + norm[{hits[[1,1]],myline[mytimes[[1]]-c][[2]], zh+c}- {hits[[2,1]],myline[mytimes[[2]]+c][[2]], zh+c}] + norm[{hits[[2,1]],myline[mytimes[[2]]+c][[2]], zh+c}-end]) <= (norm[start-{xh+c,myline[mytimes[[1]]-c][[2]], hits[[1,3]]}] + norm[{xh+c,myline[mytimes[[1]]-c][[2]], hits[[1,3]]}- {xh+c,myline[mytimes[[2]]+c][[2]], hits[[2,3]]}] + norm[{xh+c,myline[mytimes[[2]]+c][[2]], hits[[2,3]]}-end])))], {{hits[[1,1]],myline[mytimes[[1]]-c][[2]], zh+c}, {hits[[2,1]],myline[mytimes[[2]]+c][[2]], zh+c}} *) ] (* , (sides[[1,2]] == 3) && (sides[[2,2]] == 3), Which[ N[(((norm[start-{xl-c, hits[[1,2]],myline[mytimes[[1]]-c][[3]]}] + norm[{xl-c, hits[[1,2]],myline[mytimes[[1]]-c][[3]]}- {xl-c, hits[[2,2]],myline[mytimes[[2]]+c][[3]]}] + norm[{xl-c, hits[[2,2]],myline[mytimes[[2]]+c][[3]]}-end]) <= (norm[start-{xh+c, hits[[1,2]],myline[mytimes[[1]]-c][[3]]}] + norm[{xh+c, hits[[1,2]],myline[mytimes[[1]]-c][[3]]}- {xh+c, hits[[2,2]],myline[mytimes[[2]]+c][[3]]}] + norm[{xh+c, hits[[2,2]],myline[mytimes[[2]]+c][[3]]}-end])) && ((norm[start-{xl-c, hits[[1,2]],myline[mytimes[[1]]-c][[3]]}] + norm[{xl-c, hits[[1,2]],myline[mytimes[[1]]-c][[3]]}- {xl-c, hits[[2,2]],myline[mytimes[[2]]+c][[3]]}] + norm[{xl-c, hits[[2,2]],myline[mytimes[[2]]+c][[3]]}-end]) <= (norm[start-{hits[[1,1]],yl-c,myline[mytimes[[1]]-c][[3]]}] + norm[{hits[[1,1]],yl-c,myline[mytimes[[1]]-c][[3]]}- {hits[[2,1]],yl-c,myline[mytimes[[2]]+c][[3]]}] + norm[{hits[[2,1]],yl-c,myline[mytimes[[2]]+c][[3]]}-end])) && ((norm[start-{xl-c, hits[[1,2]],myline[mytimes[[1]]-c][[3]]}] + norm[{xl-c, hits[[1,2]],myline[mytimes[[1]]-c][[3]]}- {xl-c, hits[[2,2]],myline[mytimes[[2]]+c][[3]]}] + norm[{xl-c, hits[[2,2]],myline[mytimes[[2]]+c][[3]]}-end]) <= (norm[start-{hits[[1,1]],yh+c,myline[mytimes[[1]]-c][[3]]}] + norm[{hits[[1,1]],yh+c,myline[mytimes[[1]]-c][[3]]}- {hits[[2,1]],yh+c,myline[mytimes[[2]]+c][[3]]}] + norm[{hits[[2,1]],yh+c,myline[mytimes[[2]]+c][[3]]}-end])))], {{xl-c, hits[[1,2]],myline[mytimes[[1]]-c][[3]]}, {xl-c, hits[[2,2]],myline[mytimes[[2]]+c][[3]]}}, N[(((norm[start-{xh+c, hits[[1,2]],myline[mytimes[[1]]-c][[3]]}] + norm[{xh+c, hits[[1,2]],myline[mytimes[[1]]-c][[3]]}- {xh+c, hits[[2,2]],myline[mytimes[[2]]+c][[3]]}] + norm[{xh+c, hits[[2,2]],myline[mytimes[[2]]+c][[3]]}-end]) <= (norm[start-{xl-c, hits[[1,2]],myline[mytimes[[1]]-c][[3]]}] + norm[{xl-c, hits[[1,2]],myline[mytimes[[1]]-c][[3]]}- {xl-c, hits[[2,2]],myline[mytimes[[2]]+c][[3]]}] + norm[{xl-c, hits[[2,2]],myline[mytimes[[2]]+c][[3]]}-end])) && ((norm[start-{xh+c, hits[[1,2]],myline[mytimes[[1]]-c][[3]]}] + norm[{xh+c, hits[[1,2]],myline[mytimes[[1]]-c][[3]]}- {xh+c, hits[[2,2]],myline[mytimes[[2]]+c][[3]]}] + norm[{xh+c, hits[[2,2]],myline[mytimes[[2]]+c][[3]]}-end]) <= (norm[start-{hits[[1,1]],yl-c,myline[mytimes[[1]]-c][[3]]}] + norm[{hits[[1,1]],yl-c,myline[mytimes[[1]]-c][[3]]}- {hits[[2,1]],yl-c,myline[mytimes[[2]]+c][[3]]}] + norm[{hits[[2,1]],yl-c,myline[mytimes[[2]]+c][[3]]}-end])) && ((norm[start-{xh+c, hits[[1,2]],myline[mytimes[[1]]-c][[3]]}] + norm[{xh+c, hits[[1,2]],myline[mytimes[[1]]-c][[3]]}- {xh+c, hits[[2,2]],myline[mytimes[[2]]+c][[3]]}] + norm[{xh+c, hits[[2,2]],myline[mytimes[[2]]+c][[3]]}-end]) <= (norm[start-{hits[[1,1]],yh+c,myline[mytimes[[1]]-c][[3]]}] + norm[{hits[[1,1]],yh+c,myline[mytimes[[1]]-c][[3]]}- {hits[[2,1]],yh+c,myline[mytimes[[2]]+c][[3]]}] + norm[{hits[[2,1]],yh+c,myline[mytimes[[2]]+c][[3]]}-end])))], {{xh+c, hits[[1,2]],myline[mytimes[[1]]-c][[3]]}, {xh+c, hits[[2,2]],myline[mytimes[[2]]+c][[3]]}}, N[(((norm[start-{hits[[1,1]],yl-c,myline[mytimes[[1]]-c][[3]]}] + norm[{hits[[1,1]],yl-c,myline[mytimes[[1]]-c][[3]]}- {hits[[2,1]],yl-c,myline[mytimes[[2]]+c][[3]]}] + norm[{hits[[2,1]],yl-c,myline[mytimes[[2]]+c][[3]]}-end]) <= (norm[start-{hits[[1,1]],yh+c,myline[mytimes[[1]]-c][[3]]}] + norm[{hits[[1,1]],yh+c,myline[mytimes[[1]]-c][[3]]}- {hits[[2,1]],yh+c,myline[mytimes[[2]]+c][[3]]}] + norm[{hits[[2,1]],yh+c,myline[mytimes[[2]]+c][[3]]}-end])) && ((norm[start-{hits[[1,1]],yl-c,myline[mytimes[[1]]-c][[3]]}] + norm[{hits[[1,1]],yl-c,myline[mytimes[[1]]-c][[3]]}- {hits[[2,1]],yl-c,myline[mytimes[[2]]+c][[3]]}] + norm[{hits[[2,1]],yl-c,myline[mytimes[[2]]+c][[3]]}-end]) <= (norm[start-{xl-c, hits[[1,2]],myline[mytimes[[1]]-c][[3]]}] + norm[{xl-c, hits[[1,2]],myline[mytimes[[1]]-c][[3]]}- {xl-c, hits[[2,2]],myline[mytimes[[2]]+c][[3]]}] + norm[{xl-c, hits[[2,2]],myline[mytimes[[2]]+c][[3]]}-end])) && ((norm[start-{hits[[1,1]],yl-c,myline[mytimes[[1]]-c][[3]]}] + norm[{hits[[1,1]],yl-c,myline[mytimes[[1]]-c][[3]]}- {hits[[2,1]],yl-c,myline[mytimes[[2]]+c][[3]]}] + norm[{hits[[2,1]],yl-c,myline[mytimes[[2]]+c][[3]]}-end]) <= (norm[start-{xh+c, hits[[1,2]],myline[mytimes[[1]]-c][[3]]}] + norm[{xh+c, hits[[1,2]],myline[mytimes[[1]]-c][[3]]}- {xh+c, hits[[2,2]],myline[mytimes[[2]]+c][[3]]}] + norm[{xh+c, hits[[2,2]],myline[mytimes[[2]]+c][[3]]}-end])))], {{hits[[1,1]],yl-c,myline[mytimes[[1]]-c][[3]]}, {hits[[2,1]],yl-c,myline[mytimes[[2]]+c][[3]]}}, N[(((norm[start-{hits[[1,1]],yh+c,myline[mytimes[[1]]-c][[3]]}] + norm[{hits[[1,1]],yh+c,myline[mytimes[[1]]-c][[3]]}- {hits[[2,1]],yh+c,myline[mytimes[[2]]+c][[3]]}] + norm[{hits[[2,1]],yh+c,myline[mytimes[[2]]+c][[3]]}-end]) <= (norm[start-{hits[[1,1]],yl-c,myline[mytimes[[1]]-c][[3]]}] + norm[{hits[[1,1]],yl-c,myline[mytimes[[1]]-c][[3]]}- {hits[[2,1]],yl-c,myline[mytimes[[2]]+c][[3]]}] + norm[{hits[[2,1]],yl-c,myline[mytimes[[2]]+c][[3]]}-end])) && ((norm[start-{hits[[1,1]],yh+c,myline[mytimes[[1]]-c][[3]]}] + norm[{hits[[1,1]],yh+c,myline[mytimes[[1]]-c][[3]]}- {hits[[2,1]],yh+c,myline[mytimes[[2]]+c][[3]]}] + norm[{hits[[2,1]],yh+c,myline[mytimes[[2]]+c][[3]]}-end]) <= (norm[start-{xl-c, hits[[1,2]],myline[mytimes[[1]]-c][[3]]}] + norm[{xl-c, hits[[1,2]],myline[mytimes[[1]]-c][[3]]}- {xl-c, hits[[2,2]],myline[mytimes[[2]]+c][[3]]}] + norm[{xl-c, hits[[2,2]],myline[mytimes[[2]]+c][[3]]}-end])) && ((norm[start-{hits[[1,1]],yh+c,myline[mytimes[[1]]-c][[3]]}] + norm[{hits[[1,1]],yh+c,myline[mytimes[[1]]-c][[3]]}- {hits[[2,1]],yh+c,myline[mytimes[[2]]+c][[3]]}] + norm[{hits[[2,1]],yh+c,myline[mytimes[[2]]+c][[3]]}-end]) <= (norm[start-{xh+c, hits[[1,2]],myline[mytimes[[1]]-c][[3]]}] + norm[{xh+c, hits[[1,2]],myline[mytimes[[1]]-c][[3]]}- {xh+c, hits[[2,2]],myline[mytimes[[2]]+c][[3]]}] + norm[{xh+c, hits[[2,2]],myline[mytimes[[2]]+c][[3]]}-end])))], {{hits[[1,1]],yh+c,myline[mytimes[[1]]-c][[3]]}, {hits[[2,1]],yh+c,myline[mytimes[[2]]+c][[3]]}}] *) ], Which[ (sides[[1]] == {1,1}), {{xl-c, myline[mytimes[[1]]+c][[2]], myline[mytimes[[1]]+c][[3]]}}, (sides[[1]] == {2,1}), {{xh+c, myline[mytimes[[1]]+c][[2]], myline[mytimes[[1]]+c][[3]]}}, (sides[[1]] == {1,2}), {{myline[mytimes[[1]]+c][[1]], yl-c, myline[mytimes[[1]]+c][[3]]}}, (sides[[1]] == {2,2}), {{myline[mytimes[[1]]+c][[1]], yh+c, myline[mytimes[[1]]+c][[3]]}}, (sides[[1]] == {1,3}), {{myline[mytimes[[1]]+c][[1]], myline[mytimes[[1]]+c][[2]], zl-c}}, (sides[[1]] == {2,3}), {{myline[mytimes[[1]]+c][[1]], myline[mytimes[[1]]+c][[2]], zh+c}} ] ] ]; While[posoblist != {}, pob = poblist[[1]]; detourcopts = detourcornerpts[v, w, pob]; negoblist = Complement[obslist, posoblist]; myline[t_] := straightline[t,v,w]; If[ positiveobstaclelist[ N[{v,First[detourcopts]}],negoblist] == {}, detour = Join[detour,{First[detourcopts]}], detour = Join[detour, {myline[First[poblist[[1,1]]]-c], First[detourcopts]}]]; If[ positiveobstaclelist[ N[{First[detourcopts],w}], obslist] != {}, detour = Join[detour,{Last[detourcopts]}]]; alterpoblist = positiveobstaclelist[ {Last[detour],w},obslist]; If[ Length[alterpoblist] > (Length[poblist]-1), detour = Append[detour, myline[Last[poblist[[1,1]]]+c]]; poblist = Rest[poblist]; posoblist = Rest[posoblist]; v = Last[detour], obslist = Complement[obslist,{First[posoblist]}]; v = Last[detour]; poblist = alterpoblist; posoblist = Table[poblist[[i,2]], {i, Length[poblist]}]] ]; detour = Append[detour,w]; upgrade[ldetour_] := Module[ {detourtemp,newdetour,remdetour,i}, detourtemp = ldetour; newdetour = {ldetour[[1]]}; While[Last[newdetour] != Last[ldetour], remdetour = {}; i = 0; While[ positiveobstaclelist[ {Last[newdetour], detourtemp[[Length[detourtemp]-i]]}, N[obstaclelist]] != {}, remdetour = Prepend[remdetour,detourtemp[[Length[detourtemp]-i]]]; i = i+1]; newdetour = Append[newdetour,detourtemp[[Length[detourtemp]-i]]]; detourtemp = remdetour]; newdetour]; detour = upgrade[detour] ]; detourlist[mystationlist_, myobstaclelist_] := Module[{`mydetour, `i}, mydetour = detourlist[ {mystationlist[[1]],mystationlist[[2]]}, myobstaclelist]; i = 2; While[i < Length[mystationlist], mydetour = Join[mydetour, Rest[detourlist[ {mystationlist[[i]],mystationlist[[i+1]]}, myobstaclelist]]]; i = i+1]; mydetour]; detour = detourlist[newstationlist, obstaclelist]; myposition[t_,statlist_] := Module[{(* `norm, *) `v,`ti,`n,`marker}, (* norm[v_] := Sqrt[v[[1]]^2+v[[2]]^2+v[[3]]^2]; *) ti[1] = 0; ti[n_] := (norm[statlist[[n]]-statlist[[n-1]]])+ti[n-1]; marker = Last[Select[ Table[j,{j,1,(Length[statlist])}], ((N[ti[#]<=t])&)]]; Which[ marker < Length[statlist], statlist[[marker]]+ (t-ti[marker])* (statlist[[marker+1]]-statlist[[marker]])/ norm[(statlist[[marker+1]]-statlist[[marker]])], marker == (Length[statlist]), statlist[[marker]] ] ]; (* norm[v_] := Sqrt[v[[1]]^2+v[[2]]^2+v[[3]]^2]; *) stationsgr[statlistl_] := Table[ {{PointSize[0.02],RGBColor[1,0,0], Point[statlistl[[j]]]}, Line[{statlistl[[j]], {statlistl[[j]][[1]], statlistl[[j]][[2]], 0}}]}, {j,1,Length[statlistl]}]; stationsgraph = stationsgr[oldstationlist]; obstaclesgr[obstaclelistl_] := Table[Cuboid[obstaclelistl[[k,1]], obstaclelistl[[k,2]]], {k,1,Length[obstaclelistl]}]; obstaclesgraph = obstaclesgr[obstaclelist]; cornersgr[diml_(* dimlocal *)] := {{PointSize[0.0000001],Point[{-diml,-diml,-diml}]}, {PointSize[0.0000001],Point[{-diml,diml,-diml}]}, {PointSize[0.0000001],Point[{diml,diml,-diml}]}, {PointSize[0.0000001],Point[{diml,-diml,-diml}]}, {PointSize[0.0000001],Point[{-diml,-diml,diml}]}, {PointSize[0.0000001],Point[{-diml,diml,diml}]}, {PointSize[0.0000001],Point[{diml,diml,diml}]}, {PointSize[0.0000001],Point[{diml,-diml,diml}]}}; dim = N[Max[Abs[Join[obstaclelist,detour]]]] + 1; cornersgraph = cornersgr[dim]; totaltime = N[Sum[norm[detour[[h+1]]-detour[[h]]], {h,1,(Length[detour]-1)}]]; Do[ Show[ Graphics3D[{ {PointSize[0.015],RGBColor[0,0,1], Point[myposition[i,detour]]}, Line[{myposition[i,detour], {myposition[i,detour][[1]], myposition[i,detour][[2]], 0}}], stationsgraph, Polygon[{{0,0,0},{0,20,0},{20,20,0},{20,0,0}}], Polygon[{{0,0,0},{0,20,0},{0,20,4},{0,0,4}}], Polygon[{{0,20,0},{20,20,0},{20,20,4},{0,20,4}}], Polygon[{{20,0,0},{20,20,0},{20,20,4},{20,0,4}}], Cuboid[{39/4,0,0},{41/4,9,1}], Cuboid[{39/4,11,0},{41/4,20,1}], Cuboid[{2,7,1/2},{3,12,1}], Line[{{2,7,1},{3,7,1},{3,12,1},{2,12,1},{2,7,1}}], Line[{{2,7,0},{2,7,1/2}}], Line[{{3,7,0},{3,7,1/2}}], Line[{{3,12,0},{3,12,1/2}}], Line[{{2,12,0},{2,12,1/2}}], Cuboid[{12,4,1/2},{17,5,1}], Line[{{12,4,1},{17,4,1},{17,5,1},{12,5,1},{12,4,1}}], Line[{{12,4,0},{12,4,1/2}}], Line[{{17,4,0},{17,4,1/2}}], Line[{{17,5,0},{17,5,1/2}}], Line[{{12,5,0},{12,5,1/2}}], Cuboid[{12,15,1/2},{17,16,1}], Line[{{12,15,1},{17,15,1},{17,16,1},{12,16,1},{12,15,1}}], Line[{{12,15,0},{12,15,1/2}}], Line[{{17,15,0},{17,15,1/2}}], Line[{{17,16,0},{17,16,1/2}}], Line[{{12,16,0},{12,16,1/2}}], Line[{{9/2,8,1},{11/2,8,1},{11/2,11,1},{9/2,11,1},{9/2,8,1}}], Line[{{9/2,8,1/2},{11/2,8,1/2},{11/2,11,1/2},{9/2,11,1/2},{9/2,8,1/2}}], Line[{{9/2,8,0},{9/2,8,1}}], Line[{{11/2,8,0},{11/2,8,1}}], Line[{{11/2,11,0},{11/2,11,1}}], Line[{{9/2,11,0},{9/2,11,1}}], Line[{{13,9,1},{16,9,1},{16,10,1},{13,10,1},{13,9,1}}], Line[{{13,9,0},{13,9,1}}], Line[{{16,9,0},{16,9,1}}], Line[{{16,10,0},{16,10,1}}], Line[{{13,10,0},{13,10,1}}], {Thickness[0.0002],Table[Line[{{l,0,0},{l,20,0}}],{l,1,19}]}, {Thickness[0.00002],Table[Line[{{0,l,0},{20,l,0}}],{l,1,19}]} }, Axes->True, AxesLabel -> {"x","y"," "} ], ViewPoint->{0.670, -4.000, 1.460} ], {i, 0, totaltime, step} ]; Show[ Graphics3D[{ {PointSize[0.015], RGBColor[0,0,1], Point[myposition[totaltime,detour]]}, Line[{myposition[totaltime,detour], {myposition[totaltime,detour][[1]], myposition[totaltime,detour][[2]], 0}}], stationsgraph, Polygon[{{0,0,0},{0,20,0},{20,20,0},{20,0,0}}], Polygon[{{0,0,0},{0,20,0},{0,20,4},{0,0,4}}], Polygon[{{0,20,0},{20,20,0},{20,20,4},{0,20,4}}], Polygon[{{20,0,0},{20,20,0},{20,20,4},{20,0,4}}], Cuboid[{39/4,0,0},{41/4,9,1}], Cuboid[{39/4,11,0},{41/4,20,1}], Cuboid[{2,7,1/2},{3,12,1}], Line[{{2,7,1},{3,7,1},{3,12,1},{2,12,1},{2,7,1}}], Line[{{2,7,0},{2,7,1/2}}], Line[{{3,7,0},{3,7,1/2}}], Line[{{3,12,0},{3,12,1/2}}], Line[{{2,12,0},{2,12,1/2}}], Cuboid[{12,4,1/2},{17,5,1}], Line[{{12,4,1},{17,4,1},{17,5,1},{12,5,1},{12,4,1}}], Line[{{12,4,0},{12,4,1/2}}], Line[{{17,4,0},{17,4,1/2}}], Line[{{17,5,0},{17,5,1/2}}], Line[{{12,5,0},{12,5,1/2}}], Cuboid[{12,15,1/2},{17,16,1}], Line[{{12,15,1},{17,15,1},{17,16,1},{12,16,1},{12,15,1}}], Line[{{12,15,0},{12,15,1/2}}], Line[{{17,15,0},{17,15,1/2}}], Line[{{17,16,0},{17,16,1/2}}], Line[{{12,16,0},{12,16,1/2}}], Line[{{9/2,8,1},{11/2,8,1},{11/2,11,1},{9/2,11,1},{9/2,8,1}}], Line[{{9/2,8,1/2},{11/2,8,1/2},{11/2,11,1/2},{9/2,11,1/2},{9/2,8,1/2}}], Line[{{9/2,8,0},{9/2,8,1}}], Line[{{11/2,8,0},{11/2,8,1}}], Line[{{11/2,11,0},{11/2,11,1}}], Line[{{9/2,11,0},{9/2,11,1}}], Line[{{13,9,1},{16,9,1},{16,10,1},{13,10,1},{13,9,1}}], Line[{{13,9,0},{13,9,1}}], Line[{{16,9,0},{16,9,1}}], Line[{{16,10,0},{16,10,1}}], Line[{{13,10,0},{13,10,1}}], {Thickness[0.0002],Table[Line[{{i,0,0},{i,20,0}}],{i,1,19}]}, {Thickness[0.00002],Table[Line[{{0,i,0},{20,i,0}}],{i,1,19}]} }, Axes->True, AxesLabel -> {"x","y"," "} ], ViewPoint->{0.670, -4.000, 1.460} ] ]]] ] obstacles = {{{1.8,6.8,0.},{3.2,12.2,1.}},{{4.3,7.8,0.},{5.7,11.2,1.}}, {{9.55,0.,0.},{10.45,9.2,4.}},{{9.55,10.8,0.},{10.45,20.,4.}}, {{11.8,3.8,0.},{17.2,5.2,1.}},{{12.8,8.8,0.},{16.2,10.2,1.}}, {{11.8,14.8,0.},{17.2,16.2,1.}}} inroom[s_] := N[(0 < s[[1]]) && (s[[1]] < 20) && (0 < s[[2]]) && (s[[2]] < 20)] navigate2[stationlist_] := If[ (Intersection[{False}, Union[Table[inroom[stationlist[[i]]], {i, Length[stationlist]}]]] == {False}), Print["Error: Coordinates are outside the room."], navigate[N[Table[Append[stationlist[[i]],.75], {i,1,Length[stationlist]}]], obstacles]] Print["Welcome to the Navigation2 Package."] Print[" "] Print["The Navigation2 package contains the function "] Print["navigate2 which controls the movements of an"] Print["object (e.g., a robot) in a familiar"] Print["environment, say two rooms."] Print[" "] Print["For demonstration purposes, please assume the"] Print["design of the two rooms pictured below."] Print[" "] Show[Graphics3D[{ Polygon[{{0,0,0},{0,20,0},{20,20,0},{20,0,0}}], Polygon[{{0,0,0},{0,20,0},{0,20,4},{0,0,4}}], Polygon[{{0,20,0},{20,20,0},{20,20,4},{0,20,4}}], Polygon[{{20,0,0},{20,20,0},{20,20,4},{20,0,4}}], Cuboid[{39/4,0,0},{41/4,9,1}], Cuboid[{39/4,11,0},{41/4,20,1}], Cuboid[{2,7,1/2},{3,12,1}], Line[{{2,7,1},{3,7,1},{3,12,1},{2,12,1},{2,7,1}}], Line[{{2,7,0},{2,7,1/2}}], Line[{{3,7,0},{3,7,1/2}}], Line[{{3,12,0},{3,12,1/2}}], Line[{{2,12,0},{2,12,1/2}}], Cuboid[{12,4,1/2},{17,5,1}], Line[{{12,4,1},{17,4,1},{17,5,1},{12,5,1},{12,4,1}}], Line[{{12,4,0},{12,4,1/2}}], Line[{{17,4,0},{17,4,1/2}}], Line[{{17,5,0},{17,5,1/2}}], Line[{{12,5,0},{12,5,1/2}}], Cuboid[{12,15,1/2},{17,16,1}], Line[{{12,15,1},{17,15,1},{17,16,1},{12,16,1},{12,15,1}}], Line[{{12,15,0},{12,15,1/2}}], Line[{{17,15,0},{17,15,1/2}}], Line[{{17,16,0},{17,16,1/2}}], Line[{{12,16,0},{12,16,1/2}}], Line[{{9/2,8,1},{11/2,8,1},{11/2,11,1},{9/2,11,1},{9/2,8,1}}], Line[{{9/2,8,1/2},{11/2,8,1/2},{11/2,11,1/2},{9/2,11,1/2},{9/2,8,1/2}}], Line[{{9/2,8,0},{9/2,8,1}}], Line[{{11/2,8,0},{11/2,8,1}}], Line[{{11/2,11,0},{11/2,11,1}}], Line[{{9/2,11,0},{9/2,11,1}}], Line[{{13,9,1},{16,9,1},{16,10,1},{13,10,1},{13,9,1}}], Line[{{13,9,0},{13,9,1}}], Line[{{16,9,0},{16,9,1}}], Line[{{16,10,0},{16,10,1}}], Line[{{13,10,0},{13,10,1}}], {Thickness[0.0002],Table[Line[{{i,0,0},{i,20,0}}],{i,1,19}]}, {Thickness[0.00002],Table[Line[{{0,i,0},{20,i,0}}],{i,1,19}]} }, Axes->True, AxesLabel -> {"x","y"," "} ], ViewPoint->{0.670, -4.000, 1.460} ]; Print["For example, the code"] Print[" "] Print[" navigate2[{{6,7}, {15,2}, {19,16}}];"] Print[" "] Print["initiates the robot's movement,"] Print["where {6,7} is the starting point,"] Print["{15,2} is an intermediate destination,"] Print["and {19,16} is the final destination."] Print[" "] Print["Now you try to move the robot."] Print["Type in your own finite list of 2-dim coordinates."] Print["Please, use the gridlines for your orientation."] Print["(Be aware of a small protection zone around"] Print[" each obstacle.)"] (* * Protect the function and lock * the private functions. *) Protect[navigate2]; Append[Attributes[navigate], Locked] Append[Attributes[obstacles], Locked] Append[Attributes[inroom], Locked] End[];