losAlgorithm[mesh_] :=
Module[{mesh0 = mesh, pts, nvertices, edges, tri, va, edgelengths,
distMatrix, tchainlists, \[CapitalDelta]\[Theta]precΔθprec, vtrilist,
ntri, tl3dstore, tnum, eAB, vA, vB, vD, vS, e1, e2, el,
vertmeshlabels, vertex2Dcoords, trilist2D, trichain3Dlabels,
trilabel, trilabel0, coordsA, coordsB, coordsD,
ang, \[Theta]θ, \[Theta]minθmin, \[Theta]maxθmax, vnum2D, tchainstack,
doubletri, boundaryEdgeQ, tpop, trichainnum, vectAB, angAB,
angBAD, e3, el3, \[Delta]SDδSD, outMatrix, \[Theta]MatrixθMatrix, tMatrix,
distMatrixsym},
(**************************)
(*Numerical Parameters*)
(**************************)
dthetaprec = 0.05; (*Precision in Theta max - Theta min ,
perhaps can be removed at somestage but at the moment useful for testing*)
(**************************)
(*Mesh Calculations*)
(**************************)
(*List of coordinates of all vertices on the mesh*)
pts = MeshCoordinates[mesh];
(*Number of vertices on the mesh*)
nvertices = Length[pts];
(*List of all edges on the mesh,
in terms of pairs of vertex indices*)
edges = MeshCells[mesh, 1] /. Line[p_] :> p ;
(*List of the "ordered" vertex index triples for all triangles on the mesh*)
tri = MeshCells[mesh, 2] /. Polygon[p_] :> p;
(*List of edgelengths numbered by edge *)
edgelengths = PropertyValue[{mesh, 1}, MeshCellMeasure];
(*List of the angles within each of the corners of all triangles on the mesh*)
va = VectorAngle[#1 - #2, #3 - #2] & @@@
Partition[#, 3, 1, {2, -2}] & /@
MeshPrimitives[mesh, {2}][[All, 1]];
(**************************)
(*Preparation of data structures for storage*)
(**************************)
(*Matrix to store all distances between all pairs of vertices*)
distMatrix =
Table[If[i == j, 0], {i, 1, nvertices}, {j, 1, nvertices}];
(*Matrix to store all angles between pairs of vertices*)
ThetaMatrix =
Table[If[i == j, 0], {i, 1, nvertices}, {j, 1, nvertices}];
(*Matrix to store all triangle chains (in 3D labelling) between pairs of vertices*)
tMatrix =
Table[If[i == j, 0], {i, 1, nvertices}, {j, 1, nvertices}];
(**************************)
(*Start of Algorithm*)
(**************************)
(********)
(*Step 1 -
Choose a vertex vS on the mesh (Here we map over all Vertices) *)
(********)
Do[
(********)
(*Step 2 -
Choose a triangle containing vS as a vertex*)
(********)
(*Create a list of triangles around the vertex vS*)
vtrilist = Position[tri, vS];
(*Number of triangles around vertex S*)
ntri = Length[vtrilist];
(*Choose the first triangle in the list*)
tnum = 1;
(*While Loop to be performed over all triangles around vertex S*)
(**********************)
(******
while loop here******)
(**********************)
While[tnum < ntri,
Label[step2];
If[tnum == ntri + 1, Break[]];
(*Calculate ordered list of vertices A and B assuming we have outward oriented facets*)
eAB = tri[[
vtrilist[[tnum]][[1]]]] /. {vS, a_, b_} | {b_, vS, a_} | {a_, b_, vS} :> {a, b};
vA = eAB[[1]]; (*Set vertex A*)
vB = eAB[[2]]; (*Set vertex B*)
(*Calculate labels of the vertices of first two edges*)
e1 = {vA, vS};
e2 = {vB, vS};
(*Calculate edge lengths for the first triangle in the chain(s)*)
el = edgelengths[[Join[Flatten[Position[edges, e1 | Reverse[e1]]],Flatten[Position[edges, e2 | Reverse[e2]]]]]];
(*Store the edge length data in the distance matrix*)
distMatrix[[vS, vA]] = el[[1]];
distMatrix[[vS, vB]] = el[[2]];
(*Create a set of (temporary) lists which will hold information about vertexlabels,
coordinates etc*)
(*Lists will be stored (perhaps in tMatrix etc),
whenever a new triangle chain is created*)
(*Probably this information is not needed in the final algorithm,
but will be needed for debugging*)
(*List of the vertex labels from the original mesh in the triangle chain*)
vertmeshlabels = {};
(*List of the 2D transformed coordinates of the vertices*)
vertex2Dcoords = {};
(*List of the vertex triples that make up each flattened triangle chain in 2D*)
trilist2D = {};
(*List of 3D face triangle labels from the mesh in the triangle chain*)
trichain3Dlabels = {};
(*label of current triangle in the triangle chain *)
trilabel = vtrilist[[tnum]][[1]];
(*Set previous triangle label to the current one (used for later calculations) *)
trilabel0 = trilabel;
(*Add the first triangle label to the list of 3D triangle(face) labels in the chain*)
trichain3Dlabels = {trichain3Dlabels, trilabel};
(*Map first triangle in the chain to the plane*)
(*Origin Vertex vS*)
(*Add vS mesh label to list*)
vertmeshlabels = {vertmeshlabels, vS};
(*Add vS 2D coordinate pair to 2D coordinate list*)
vertex2Dcoords = {vertex2Dcoords, {0, 0}};
(*Vertex vA *)
(*Add vA mesh label to list*)
vertmeshlabels = {vertmeshlabels, vA};
coordsA = {el[[1]], 0}; (*Calculate 2D flattened coordinates of vertex vA*)
(*Add vA 2D coordinate pair to 2D coordinate list*)
vertex2Dcoords = {vertex2Dcoords, coordsA};
(*Vertex vB *)
(*Add vB mesh label to list*)
vertmeshlabels = {vertmeshlabels, vB};
ang = va[[vtrilist[[tnum]][[1]], vtrilist[[tnum]][[2]]]];
coordsB = {el[[2]]*Cos[ang], el[[2]]*Sin[ang]}; (*Calculate 2D flattened coordinates of vertex vB*)
(*Add vB 2D coordinate pair to 2D coordinate list*)
vertex2Dcoords = {vertex2Dcoords, coordsB};
(*Add the ordered triple of vertices to the 2D triangle list*)
trilist2D = {trilist2D, {1, 2, 3}};
thetamin = 0; (*Set min angle to be 0*)
thetamax = ang; (*Set max angle to be the angle <BSA*)
(********)
(*Step 3*)
(********)
(*Counter of number of 2D vertices in triangle chain, will be reset everytime we do a new chain? starts at vertex 4*)
vnum2D = 4;
(*Initialise chain stack to enable chain branching, this first starts off as being empty*)
(*What we also want to do is to rebuild a new chain stack for each branch *)
tchainstack = {};
(********)
(*Step 4*)
(********)
doubletri = 0; (*Label = 0 if triangles not repeated in chain, = 1 if repeated*)
nextensions = 1; (*Max number of extensions of triangle chain within one chain*)
While[nextensions < 10000,
Label[step4];
(*BoundaryQ - Returns False if edge NOT a boundary,
True only 1 tri is attached to edge vA -> vB*)
boundaryEdgeQ = Intersection[Position[tri, vA][[All, 1]], Position[tri, vB][[All, 1]]] != 2;
(*Calculate whether we are within machine precision or not \
thetamax-thetamin< dthetaprec*)
If[(thetamax - thetamin < dthetaprec) ||
boundaryEdgeQ || (doubletri == 1),
If[(tchainstack == {}),
tnum++;
Goto[step2],(*picknewtriangle*)
{tpop, tchainstack} = {#, {##2}} & @@ tchainstack;
(*newstructure of tchainstack*)
tchainstack = tchainstack[[1]];
(*to reset values so that we go down other chains*)
(*Tlabel, Alabel, Blabel, coordsA2D,coordsB2D,thetamin,thetamax*)
trilabel0 = tpop[[1]];
vA = tpop[[2]];
vB = tpop[[3]];
coordsA = tpop[[4]];
coordsB = tpop[[5]];
thetamin = tpop[[6]];
thetamax = tpop[[7]];
vnum2D = tpop[[8]];
(*here we store the previous tchainlist for plotting*)
vertmeshlabels = Flatten[vertmeshlabels]; (*Flatten the linked list created previously*)
trichain3Dlabels = Flatten[trichain3Dlabels];(*Flatten the linked list created previously*)
vertex2Dcoords = Partition[Flatten[vertex2Dcoords],2];(*Flatten the linked list created previously*)
trilist2D =
Partition[Flatten[trilist2D], 3];(*Flatten the linked list created previously*)
(*now we need to go back in the list and "restart" so to say the counters*)
(*need to reset the storage, and also the vnum2D otherwise we wont have diff chains, mainly important for plotting, but probably stops labelling effects*)
trichainnum = Position[trichain3Dlabels, trilabel0][[1, 1]];
trichain3Dlabels = Take[trichain3Dlabels, trichainnum];
vertmeshlabels = Take[vertmeshlabels, vnum2D - 1];
trilist2D = Take[trilist2D, trichainnum];
vertex2Dcoords = Take[vertex2Dcoords, vnum2D - 1];
(*Reset doubled triangle label to zero *)
doubletri = 0;
];
];
(*Find triangle label on the other side of the edge AB on the previous triangle in the chain*)
trilabel = Select[Intersection[Position[tri, vA][[All, 1]],Position[tri, vB][[All, 1]]], # != trilabel0 &][[1]];
(*Check to see if triangle has been visited before in the chain if yes, go to new chainstack,*)
If[MemberQ[trichain3Dlabels, trilabel], doubletri = 1;
Goto[step4];];
trilabel0 = trilabel;
(********)
(*Step 5*)
(********)
(*Add the 3D triangle label to the triangle chain list*)
trichain3Dlabels = {trichain3Dlabels, trilabel};
(*Calculate the label of the next vertex*)
vD = Select[tri[[trilabel]], False == MemberQ[{vA, vB}, #] &][[1]];
vertmeshlabels = {vertmeshlabels, vD};
(*Calculate angle, in 2D, of edge A-B, wrt x-axis*)
vectAB = coordsB - coordsA;
angAB = ArcTan[vectAB[[1]], vectAB[[2]]];
(*Calculate angle <BAD*)
angBAD = va[[trilabel, Position[tri[[trilabel]], vA][[1, 1]]]];
e3 = {vA, vD};
el3 = edgelengths[[Flatten[Position[edges, e3 | Reverse[e3]]]]][[1]];
(*Calculation of 2D flattened coordinates of vertex D*)
coordsD = (coordsA + {el3*Cos[angAB - angBAD], el3*Sin[angAB - angBAD]});
vertex2Dcoords = {vertex2Dcoords, coordsD};
(*Add ordered triple of vertices to triangle list*)
trilist2D = {trilist2D, Flatten[Map[Position[vertmeshlabels, #] &,tri[[trilabel]]]]};
(*Increment vnum2D*)
vnum2D = vnum2D + 1;
(*Calculate the angle that SD makes with the x-axis*)
theta = ArcTan[coordsD[[1]], coordsD[[2]]];
(********)
(*Step 6 - If theta<thetamin set A = D and return to step 4*)
(********)
If[theta < thetamin,
vA = vD;
coordsA = coordsD;
Goto[step4];
];
(********)
(*Step 7 - If theta>thetamax set B = D and return to step 4*)
(********)
If[theta > thetamax,
vB = vD;
coordsB = coordsD;
Goto[step4];
];
(********)
(*Step 8 -
If theta is an element (thetamin,thetamax)*)
(********)
(*Compute Euclidean distance between planar representations of S and D*)
deltaSD = Sqrt[Total[coordsD^2]];
(*Update distance matrix and angle matrix if the calculated distance is smaller than what was previously stored*)
If[distMatrix[[vS, vD]] == Null,
distMatrix[[vS, vD]] = deltaSD;
thetaMatrix[[vS, vD]] = theta;
tMatrix[[vS, vD]] = Flatten[trichain3Dlabels],
If[distMatrix[[vS, vD]] > deltaSD,
distMatrix[[vS, vD]] = deltaSD;
thetaMatrix[[vS, vD]] = theta;
tMatrix[[vS, vD]] = Flatten[trichain3Dlabels]
]];
(*Store information needed to extend triangle over AD onto stack: Tlabel, Alabel, Blabel, coordsA2D,coordsB2D,thetamin,thetamax*)
tchainstack = {{trilabel, vA, vD, coordsA, coordsD, thetamin, thetamax, vnum2D}, tchainstack};
(*Extend triangle chain over edge BD, set A = D and set thetamin = theta*)
vA = vD;
coordsA = coordsD;
thetamin = theta;
nextensions++;
];
], {vS, nvertices}
];
(*Now make sure distance matrix is symmetric and replace Null by large number, Infinity in this case *)
distMatrixsym = Table[Min[{distMatrix[[i, j]] /. Null -> Infinity,
distMatrix[[j, i]] /. Null -> Infinity}], {i, 1, nvertices}, {j, 1, nvertices}];
distMatrix = distMatrix /. Null -> Infinity;
(*Implement shortest distance *)
outMatrix = GraphDistanceMatrix[WeightedAdjacencyGraph[distMatrixsym],Method -> "FloydWarshall"];
{outMatrix, tri, thetaMatrix, tMatrix, distMatrixsym, distMatrix}
];
pathCalc[vS_, vF_, trilist3D_, \[Theta]SD_θSD_, mesh_] :=
Module[{pts, tri, va, edges, edgelengths, ntri, i, path3D, vA, vB,
eAB, e1, e2, el, coordsA, ang, coordsB, mAB, cAB, xI,
yI, \[Alpha]AIαAI, coordsI, vD, vectAB, angAB, angBAD, e3, el3,
coordsD, \[Theta]θ, vtrilist, posvtrilist},
ntri = Length[trilist3D];
pts = MeshCoordinates[mesh];
tri = MeshCells[mesh, 2] /. Polygon[p_] :> p;
edges = MeshCells[mesh, 1] /. Line[p_] :> p ;
edgelengths = PropertyValue[{mesh, 1}, MeshCellMeasure];
va = VectorAngle[#1 - #2, #3 - #2] & @@@
Partition[#, 3, 1, {2, -2}] & /@
MeshPrimitives[mesh, {2}][[All, 1]];
i = 1;
(**)
path3D = {};
(*Add starting vertex coordinates to path list*)
AppendTo[path3D, pts[[vS]]];
(*Now calculate first intersection with edge (Maybe to check if \
vertices are adjacent ?) *)
eAB = tri[[
trilist3D[[
i]]]] /. {vS, a_, b_} | {b_, vS, a_} | {a_, b_, vS} :> {a, b};
vA = eAB[[1]]; (*Set vertex A*)
vB = eAB[[2]]; (*Set vertex B*)
e1 = {vA, vS};
e2 = {vB, vS};
(*Calculate edge lengths for the first triangle in the chain(s)*)
el = edgelengths[[
Join[Flatten[Position[edges, e1 | Reverse[e1]]],
Flatten[Position[edges, e2 | Reverse[e2]]]]]];
coordsA = {el[[1]],
0}; (*Calculate 2D flattened coordinates of vertex vA*)
vtrilist = Position[tri, vS];
posvtrilist = Position[vtrilist, trilist3D[[i]]][[1, 1]];
ang = va[[trilist3D[[i]], vtrilist[[posvtrilist, 2]]]];
coordsB = {el[[2]]*Cos[ang],
el[[2]]*Sin[
ang]}; (*Calculate 2D flattened coordinates of vertex vB*)
mAB = (coordsB[[2]] - coordsA[[2]])/(coordsB[[1]] -
coordsA[[1]]); (*problem if perfectly vertical!*)
cAB = coordsA[[2]] - mAB*coordsA[[1]];
xI = cAB/(Tan[\[Theta]SD]Tan[θSD] - mAB);
yI = Tan[\[Theta]SD]*xI;Tan[θSD]*xI;
\[Alpha]AIαAI = Norm[{xI, yI} - coordsA]/Norm[coordsB - coordsA];
coordsI = pts[[vA]] + \[Alpha]AI*αAI*(pts[[vB]] - pts[[vA]]);
AppendTo[path3D, coordsI];
i = 2;
While[i < ntri + 1,
vD = Select[tri[[trilist3D[[i]]]],
False == MemberQ[{vA, vB}, #] &][[1]];
vectAB = coordsB - coordsA;
angAB = ArcTan[vectAB[[1]], vectAB[[2]]];
(*Calculate angle <BAD*)
vtrilist = Position[tri, vA];
posvtrilist = Position[vtrilist, trilist3D[[i]]][[1, 1]];
angBAD = va[[trilist3D[[i]], vtrilist[[posvtrilist, 2]]]];
e3 = {vA, vD};
el3 =
edgelengths[[Flatten[Position[edges, e3 | Reverse[e3]]]]][[1]];
(*Calculation of 2D flattened coordinates of vertex D*)
coordsD = (coordsA + {el3*Cos[angAB - angBAD],
el3*Sin[angAB - angBAD]});
(*Calculate the angle that SD makes with the x-
axis*)
\[Theta]θ = ArcTan[coordsD[[1]], coordsD[[2]]];
If[\[Theta]If[θ < \[Theta]SDθSD,
vA = vD;
coordsA = coordsD;
];
If[\[Theta]If[θ > \[Theta]SDθSD,
vB = vD;
coordsB = coordsD;
];
mAB = (coordsB[[2]] - coordsA[[2]])/(coordsB[[1]] -
coordsA[[1]]); (*problem if perfectly vertical!*)
cAB = coordsA[[2]] - mAB*coordsA[[1]];
xI = cAB/(Tan[\[Theta]SD]Tan[θSD] - mAB);
yI = Tan[\[Theta]SD]*xI;Tan[θSD]*xI;
\[Alpha]AIαAI = Norm[{xI, yI} - coordsA]/Norm[coordsB - coordsA];
coordsI = pts[[vA]] + \[Alpha]AI*αAI*(pts[[vB]] - pts[[vA]]);
AppendTo[path3D, coordsI];
i++
];
AppendTo[path3D, pts[[vF]]];
{path3D}
];
