Model: renaming of components and ports

parent 357f0746
......@@ -14,8 +14,8 @@ component CollisionDetection<N1 n = 2,N1 x = 1>{
connect hulls[:] -> singleSetCompare.setIn[:];
connect singleSetCompare.listOut1[:] -> rectIntersection[:].rect1;
connect singleSetCompare.listOut2[:] -> rectIntersection[:].rect2;
connect singleSetCompare.outA[:] -> rectIntersection[:].rect1;
connect singleSetCompare.outB[:] -> rectIntersection[:].rect2;
connect rectIntersection[:].collision -> multiOr.boolIn[:];
connect multiOr.boolOut -> collision;
}
\ No newline at end of file
package ba.intersection;
component CollisionToConflict{
ports in B collisionIn,
in Z indexIn1,
in Z indexIn2,
in Q^{2,1} colPosIn,
in Q^{3,5} trajectoryIn1,
in Q^{3,5} trajectoryIn2,
in Q timeCutoff,
out B conflict;
implementation Math{
if collisionIn
//TODO: with norm()?
Q dx1 = trajectoryIn1(1,indexIn1) - trajectoryIn1(1,indexIn1 + 1);
Q dy1 = trajectoryIn1(2,indexIn1) - trajectoryIn1(2,indexIn1 + 1);
Q dist1 = sqrt(dx1*dx1 + dy1*dy1);
Q dx2 = trajectoryIn2(1,indexIn2) - trajectoryIn2(1,indexIn2 + 1);
Q dy2 = trajectoryIn2(2,indexIn2) - trajectoryIn2(2,indexIn2 + 1);
Q dist2 = sqrt(dx2*dx2 + dy2*dy2);
Q dxp1 = trajectoryIn1(1,indexIn1) - colPosIn(1,1);
Q dyp1 = trajectoryIn1(2,indexIn1) - colPosIn(2,1);
Q distp1 = sqrt(dxp1*dxp1 + dyp1*dyp1);
Q dxp2 = trajectoryIn2(1,indexIn2) - colPosIn(1,1);
Q dyp2 = trajectoryIn2(2,indexIn2) - colPosIn(2,1);
Q distp2 = sqrt(dxp2*dxp2 + dyp2*dyp2);
Q time1 = distp1/dist1 * (trajectoryIn1(3,indexIn1 + 1) - trajectoryIn1(3,indexIn1));
Q time2 = distp2/dist2 * (trajectoryIn2(3,indexIn2 + 1) - trajectoryIn2(3,indexIn2));
conflict = abs(time1 - time2) < timeCutoff;
else
conflict = False;
end
}
}
\ No newline at end of file
package ba.intersection;
//TODO: x = (n-1)n/2
component ConflictToStopConverter<N1 n = 2,N1 x = 1,N1 m = 5>{
port in B conflictIn[x];
component CollisionToStop<N1 n = 2,N1 x = 1,N1 m = 5>{
port in B collisionIn[x];
port in B active;
port out B stopOut[n];
......@@ -38,7 +38,7 @@ component ConflictToStopConverter<N1 n = 2,N1 x = 1,N1 m = 5>{
if active
for i = 1:x
if conflictIn(i)
if collisionIn(i)
Z curIndex = indexLookup(i,1);
stopOut(curIndex) = True;
end
......
package ba.intersection;
component LineInterToIndex{
//Finds the earliest line intersection from a set of LineIntersection components
component FirstLineIntersection{
ports in B collisionIn[10],
in Q^{2,1} pointsIn[10],
out B collisionOut,
out Z index1,
out Z index2,
out Z linexA,
out Z linexB,
out Q^{2,1} pos;
implementation Math{
Z k = 1;
B found = False;
for i = 1:4
for j = i:4
if found == False
if collisionIn(k)
index1 = i;
index2 = j;
linexA = i;
linexB = j;
pos = pointsIn(k);
found = True;
end
......
......@@ -10,16 +10,16 @@ component IntersectionController<N1 n = 2,N1 x = 1,N1 m = 5,N1 m1 = 4,N1 m2 = 10
out B stop[n];
instance SingleSetCompareMatrix<3,m,n,x> singleSetCompare;
instance TrajectoryCompare<m,m1,m2> trajectoryCompare[x];
instance ConflictToStopConverter<n,x,m> conflictToStopConverter;
instance TrajectoryCollision<m,m1,m2> trajectoryCollision[x];
instance CollisionToStop<n,x,m> collisionToStop;
connect trajectoryIn[:] -> singleSetCompare.setIn[:];
connect singleSetCompare.listOut1[:] -> trajectoryCompare[:].trajectory1;
connect singleSetCompare.listOut2[:] -> trajectoryCompare[:].trajectory2;
connect singleSetCompare.outA[:] -> trajectoryCollision[:].trajectoryA;
connect singleSetCompare.outB[:] -> trajectoryCollision[:].trajectoryB;
connect cutoffTime -> trajectoryCompare[:].cutoffTime;
connect trajectoryCompare[:].conflict -> conflictToStopConverter.conflictIn[:];
connect cutoffTime -> trajectoryCollision[:].cutoffTime;
connect trajectoryCollision[:].collision -> collisionToStop.collisionIn[:];
connect isActive -> conflictToStopConverter.active;
connect conflictToStopConverter.stopOut[:] -> stop[:];
connect isActive -> collisionToStop.active;
connect collisionToStop.stopOut[:] -> stop[:];
}
\ No newline at end of file
package ba.intersection;
component TimeCutoffFilter{
ports in B collisionIn,
in Z IndexInA,
in Z IndexInB,
in Q^{2,1} colPosIn,
in Q^{3,5} trajectoryInA,
in Q^{3,5} trajectoryInB,
in Q timeCutoff,
out B collisionOut;
implementation Math{
if collisionIn
//TODO: with norm()?
Q dx1 = trajectoryInA(1,IndexInA) - trajectoryInA(1,IndexInA + 1);
Q dy1 = trajectoryInA(2,IndexInA) - trajectoryInA(2,IndexInA + 1);
Q dist1 = sqrt(dx1*dx1 + dy1*dy1);
Q dx2 = trajectoryInB(1,IndexInB) - trajectoryInB(1,IndexInB + 1);
Q dy2 = trajectoryInB(2,IndexInB) - trajectoryInB(2,IndexInB + 1);
Q dist2 = sqrt(dx2*dx2 + dy2*dy2);
Q dxp1 = trajectoryInA(1,IndexInA) - colPosIn(1,1);
Q dyp1 = trajectoryInA(2,IndexInA) - colPosIn(2,1);
Q distp1 = sqrt(dxp1*dxp1 + dyp1*dyp1);
Q dxp2 = trajectoryInB(1,IndexInB) - colPosIn(1,1);
Q dyp2 = trajectoryInB(2,IndexInB) - colPosIn(2,1);
Q distp2 = sqrt(dxp2*dxp2 + dyp2*dyp2);
Q time1 = distp1/dist1 * (trajectoryInA(3,IndexInA + 1) - trajectoryInA(3,IndexInA));
Q time2 = distp2/dist2 * (trajectoryInB(3,IndexInB + 1) - trajectoryInB(3,IndexInB));
collisionOut = abs(time1 - time2) < timeCutoff;
else
collisionOut = False;
end
}
}
\ No newline at end of file
package ba.intersection;
import ba.util.TrajectoryToLines;
import ba.util.LineIntersection;
import ba.util.DualSetCompareMatrix;
//TODO: m1 = m -1, m2 = m1(m1+1)/2
component TrajectoryCollision<N1 m = 5, N1 m1 = 4, N1 m2 = 10>{
ports in Q^{3,m} trajectoryA,
in Q^{3,m} trajectoryB,
in Q cutoffTime,
out B collision;
instance TrajectoryToLines trajectoryToLinesA;
instance TrajectoryToLines trajectoryToLinesB;
instance DualSetCompareMatrix<4,1,m1,m2> dualSetCompare;
instance LineIntersection lineIntersection[m2];
instance FirstLineIntersection firstLineIntersection;
instance TimeCutoffFilter timeCutoffFilter;
connect trajectoryA -> trajectoryToLinesA.trajectoryIn;
connect trajectoryB -> trajectoryToLinesB.trajectoryIn;
connect lineIntersection[:].intersects -> firstLineIntersection.collisionIn[:];
connect lineIntersection[:].point -> firstLineIntersection.pointsIn[:];
connect firstLineIntersection.collisionOut -> timeCutoffFilter.collisionIn;
connect firstLineIntersection.linexA -> timeCutoffFilter.IndexInA;
connect firstLineIntersection.linexB -> timeCutoffFilter.IndexInB;
connect firstLineIntersection.pos -> timeCutoffFilter.colPosIn;
connect trajectoryA -> timeCutoffFilter.trajectoryInA;
connect trajectoryB -> timeCutoffFilter.trajectoryInB;
connect cutoffTime -> timeCutoffFilter.timeCutoff;
connect timeCutoffFilter.collisionOut -> collision;
connect trajectoryToLinesA.lineOut[:] -> dualSetCompare.setInA[:];
connect trajectoryToLinesB.lineOut[:] -> dualSetCompare.setInB[:];
connect dualSetCompare.outA[:] -> lineIntersection[:].lineA;
connect dualSetCompare.outB[:] -> lineIntersection[:].lineB;
}
\ No newline at end of file
package ba.intersection;
import ba.util.TrajectoryToLine;
import ba.util.LineIntersection;
import ba.util.DualSetCompareMatrix;
//TODO: m1 = m -1, m2 = m1(m1+1)/2
component TrajectoryCompare<N1 m = 5, N1 m1 = 4, N1 m2 = 10>{
ports in Q^{3,m} trajectory1,
in Q^{3,m} trajectory2,
in Q cutoffTime,
out B conflict;
instance DualSetCompareMatrix<4,1,m1,m2> dualSetCompare;
instance TrajectoryToLine trajectoryToLine1;
instance TrajectoryToLine trajectoryToLine2;
instance LineIntersection lineIntersection[m2];
instance LineInterToIndex lineInterToIndex;
instance CollisionToConflict collisionToConflict;
connect trajectory1 -> trajectoryToLine1.trajectoryIn;
connect trajectory2 -> trajectoryToLine2.trajectoryIn;
connect lineIntersection[:].intersects -> lineInterToIndex.collisionIn[:];
connect lineIntersection[:].point -> lineInterToIndex.pointsIn[:];
connect lineInterToIndex.collisionOut -> collisionToConflict.collisionIn;
connect lineInterToIndex.index1 -> collisionToConflict.indexIn1;
connect lineInterToIndex.index2 -> collisionToConflict.indexIn2;
connect lineInterToIndex.pos -> collisionToConflict.colPosIn;
connect trajectory1 -> collisionToConflict.trajectoryIn1;
connect trajectory2 -> collisionToConflict.trajectoryIn2;
connect cutoffTime -> collisionToConflict.timeCutoff;
connect collisionToConflict.conflict -> conflict;
connect trajectoryToLine1.lineOut[:] -> dualSetCompare.setIn1[:];
connect trajectoryToLine2.lineOut[:] -> dualSetCompare.setIn2[:];
connect dualSetCompare.listOut1[:] -> lineIntersection[:].line1;
connect dualSetCompare.listOut2[:] -> lineIntersection[:].line2;
}
\ No newline at end of file
......@@ -3,17 +3,17 @@ package ba.util;
//TODO: EMAM2Cpp does not work with matrix generics
//TODO: EMAM2Cpp does not work when T is first generic, n and n2 are not present in generated code
component DualSetCompare<N1 n = 5, N2 n2 = 15,T>{
ports in T setIn1[n],
in T setIn2[n],
out T listOut1[n2],
out T listOut2[n2];
ports in T setInA[n],
in T setInB[n],
out T outA[n2],
out T outB[n2];
implementation Math{
Z counter = 1;
for i = 1:n
for j = i:n
listOut1(counter) = setIn1(i);
listOut2(counter) = setIn2(j);
outA(counter) = setInA(i);
outB(counter) = setInB(j);
counter = counter + 1;
end
end
......
......@@ -2,17 +2,17 @@ package ba.util;
//TODO: n2 = n(n+1)/2
//TODO: remove once EMAM2CPP supports matrix generics
component DualSetCompareMatrix<N1 rows = 4,N1 cols = 1,N1 n = 4, N2 n2 = 10>{
ports in Q^{rows,cols} setIn1[n],
in Q^{rows,cols} setIn2[n],
out Q^{rows,cols} listOut1[n2],
out Q^{rows,cols} listOut2[n2];
ports in Q^{rows,cols} setInA[n],
in Q^{rows,cols} setInB[n],
out Q^{rows,cols} outA[n2],
out Q^{rows,cols} outB[n2];
implementation Math{
Z counter = 1;
for i = 1:n
for j = i:n
listOut1(counter) = setIn1(i);
listOut2(counter) = setIn2(j);
outA(counter) = setInA(i);
outB(counter) = setInB(j);
counter = counter + 1;
end
end
......
package ba.util;
component LineIntersection{
ports in Q^{4,1} line1,
in Q^{4,1} line2,
ports in Q^{4,1} lineA,
in Q^{4,1} lineB,
out Q^{2,1} point,
out B intersects;
implementation Math{
Q n1 = (line1(1,1)-line1(3,1))*(line2(2,1)-line2(4,1))-(line1(2,1)-line1(4,1))*(line2(1,1)-line2(3,1));
Q n2 = (line1(1,1)-line1(3,1))*(line2(2,1)-line2(4,1))-(line1(2,1)-line1(4,1))*(line2(1,1)-line2(3,1));
Q n1 = (lineA(1,1)-lineA(3,1))*(lineB(2,1)-lineB(4,1))-(lineA(2,1)-lineA(4,1))*(lineB(1,1)-lineB(3,1));
Q n2 = (lineA(1,1)-lineA(3,1))*(lineB(2,1)-lineB(4,1))-(lineA(2,1)-lineA(4,1))*(lineB(1,1)-lineB(3,1));
Q cutoff = 0.00000001;
if (abs(n1) > cutoff) && (abs(n2) > cutoff)
Q z1 = (line1(1,1)*line1(4,1)-line1(2,1)*line1(3,1))*(line2(1,1)-line2(3,1))-(line1(1,1)-line1(3,1))*(line2(1,1)*line2(4,1)-line2(2,1)*line2(3,1));
Q z2 = (line1(1,1)*line1(4,1)-line1(2,1)*line1(3,1))*(line2(2,1)-line2(4,1))-(line1(2,1)-line1(4,1))*(line2(1,1)*line2(4,1)-line2(2,1)*line2(3,1));
Q z1 = (lineA(1,1)*lineA(4,1)-lineA(2,1)*lineA(3,1))*(lineB(1,1)-lineB(3,1))-(lineA(1,1)-lineA(3,1))*(lineB(1,1)*lineB(4,1)-lineB(2,1)*lineB(3,1));
Q z2 = (lineA(1,1)*lineA(4,1)-lineA(2,1)*lineA(3,1))*(lineB(2,1)-lineB(4,1))-(lineA(2,1)-lineA(4,1))*(lineB(1,1)*lineB(4,1)-lineB(2,1)*lineB(3,1));
point(1,1) = z1/n1;
point(2,1) = z2/n2;
......@@ -36,43 +36,43 @@ component LineIntersection{
Q ymax2;
//bounding box 1
if line1(1,1) >= line1(3,1)
xmax1 = line1(1,1);
xmin1 = line1(3,1);
if lineA(1,1) >= lineA(3,1)
xmax1 = lineA(1,1);
xmin1 = lineA(3,1);
else
xmax1 = line1(3,1);
xmin1 = line1(1,1);
xmax1 = lineA(3,1);
xmin1 = lineA(1,1);
end
if line1(2,1) >= line1(4,1)
ymax1 = line1(2,1);
ymin1 = line1(4,1);
if lineA(2,1) >= lineA(4,1)
ymax1 = lineA(2,1);
ymin1 = lineA(4,1);
else
ymax1 = line1(4,1);
ymin1 = line1(2,1);
ymax1 = lineA(4,1);
ymin1 = lineA(2,1);
end
//bounding box 2
if line2(1,1) >= line2(3,1)
xmax2 = line2(1,1);
xmin2 = line2(3,1);
if lineB(1,1) >= lineB(3,1)
xmax2 = lineB(1,1);
xmin2 = lineB(3,1);
else
xmax2 = line2(3,1);
xmin2 = line2(1,1);
xmax2 = lineB(3,1);
xmin2 = lineB(1,1);
end
if line2(2,1) >= line2(4,1)
ymax2 = line2(2,1);
ymin2 = line2(4,1);
if lineB(2,1) >= lineB(4,1)
ymax2 = lineB(2,1);
ymin2 = lineB(4,1);
else
ymax2 = line2(4,1);
ymin2 = line2(2,1);
ymax2 = lineB(4,1);
ymin2 = lineB(2,1);
end
B onLine1 = (xmin1 <= point(1,1)) && (point(1,1) <= xmax1) && (ymin1 <= point(2,1)) && (point(2,1) <= ymax1);
B onLine2 = (xmin2 <= point(1,1)) && (point(1,1) <= xmax2) && (ymin2 <= point(2,1)) && (point(2,1) <= ymax2);
B onLineA = (xmin1 <= point(1,1)) && (point(1,1) <= xmax1) && (ymin1 <= point(2,1)) && (point(2,1) <= ymax1);
B onLineB = (xmin2 <= point(1,1)) && (point(1,1) <= xmax2) && (ymin2 <= point(2,1)) && (point(2,1) <= ymax2);
intersects = onLine1 && onLine2;
intersects = onLineA && onLineB;
else
point(1,1) = 0;
point(2,1) = 0;
......
......@@ -18,35 +18,35 @@ component RectIntersection{
connect rect1 -> rectToLines1.rect;
connect rect2 -> rectToLines2.rect;
connect rectToLines1.line1 -> lineIntersection[1].line1;
connect rectToLines2.line1 -> lineIntersection[1].line2;
connect rectToLines1.lineA -> lineIntersection[1].lineA;
connect rectToLines2.lineA -> lineIntersection[1].lineB;
connect rectToLines1.line1 -> lineIntersection[2].line1;
connect rectToLines2.line2 -> lineIntersection[2].line2;
connect rectToLines1.lineA -> lineIntersection[2].lineA;
connect rectToLines2.lineB -> lineIntersection[2].lineB;
connect rectToLines1.line1 -> lineIntersection[3].line1;
connect rectToLines2.line3 -> lineIntersection[3].line2;
connect rectToLines1.lineA -> lineIntersection[3].lineA;
connect rectToLines2.lineC -> lineIntersection[3].lineB;
connect rectToLines1.line1 -> lineIntersection[4].line1;
connect rectToLines2.line4 -> lineIntersection[4].line2;
connect rectToLines1.lineA -> lineIntersection[4].lineA;
connect rectToLines2.lineD -> lineIntersection[4].lineB;
connect rectToLines1.line2 -> lineIntersection[5].line1;
connect rectToLines2.line2 -> lineIntersection[5].line2;
connect rectToLines1.lineB -> lineIntersection[5].lineA;
connect rectToLines2.lineB -> lineIntersection[5].lineB;
connect rectToLines1.line2 -> lineIntersection[6].line1;
connect rectToLines2.line3 -> lineIntersection[6].line2;
connect rectToLines1.lineB -> lineIntersection[6].lineA;
connect rectToLines2.lineC -> lineIntersection[6].lineB;
connect rectToLines1.line2 -> lineIntersection[7].line1;
connect rectToLines2.line4 -> lineIntersection[7].line2;
connect rectToLines1.lineB -> lineIntersection[7].lineA;
connect rectToLines2.lineD -> lineIntersection[7].lineB;
connect rectToLines1.line3 -> lineIntersection[8].line1;
connect rectToLines2.line3 -> lineIntersection[8].line2;
connect rectToLines1.lineC -> lineIntersection[8].lineA;
connect rectToLines2.lineC -> lineIntersection[8].lineB;
connect rectToLines1.line3 -> lineIntersection[9].line1;
connect rectToLines2.line4 -> lineIntersection[9].line2;
connect rectToLines1.lineC -> lineIntersection[9].lineA;
connect rectToLines2.lineD -> lineIntersection[9].lineB;
connect rectToLines1.line4 -> lineIntersection[10].line1;
connect rectToLines2.line4 -> lineIntersection[10].line2;
connect rectToLines1.lineD -> lineIntersection[10].lineA;
connect rectToLines2.lineD -> lineIntersection[10].lineB;
connect lineIntersection[:].intersects -> multiOr.boolIn[:];
connect multiOr.boolOut -> collision;
......
......@@ -3,10 +3,10 @@ package ba.util;
component RectToLines{
ports in Q^{2,4} rect,
//TODO: as array once issue is resolved
out Q^{4,1} line1,
out Q^{4,1} line2,
out Q^{4,1} line3,
out Q^{4,1} line4;
out Q^{4,1} lineA,
out Q^{4,1} lineB,
out Q^{4,1} lineC,
out Q^{4,1} lineD;
implementation Math{
Q^{4,1} tmpLine;
......@@ -25,13 +25,13 @@ component RectToLines{
//TODO: as array once issue is resolved
if i == 1
line1 = tmpLine;
lineA = tmpLine;
elseif i == 2
line2 = tmpLine;
lineB = tmpLine;
elseif i == 3
line3 = tmpLine;
lineC = tmpLine;
elseif i == 4
line4 = tmpLine;
lineD = tmpLine;
end
end
}
......
......@@ -3,15 +3,15 @@ package ba.util;
//TODO: EMAM2Cpp does not work with matrix generics
component SingleSetCompare<N1 n = 2,N1 x = 1,T>{
ports in T setIn[n],
out T listOut1[x],
out T listOut2[x];
out T outA[x],
out T outB[x];
implementation Math{
Z counter = 1;
for i = 1:n
for j = (i+1):n
listOut1(counter) = setIn(i);
listOut2(counter) = setIn(j);
outA(counter) = setIn(i);
outB(counter) = setIn(j);
counter = counter + 1;
end
end
......
......@@ -3,15 +3,15 @@ package ba.util;
//TODO: remove once EMAM2Cpp works with matrix generics
component SingleSetCompareMatrix<N1 rows = 3,N1 cols = 5, N1 n = 2,N1 x = 1>{
ports in Q^{rows,cols} setIn[n],
out Q^{rows,cols} listOut1[x],
out Q^{rows,cols} listOut2[x];
out Q^{rows,cols} outA[x],
out Q^{rows,cols} outB[x];
implementation Math{
Z counter = 1;
for i = 1:n
for j = (i+1):n
listOut1(counter) = setIn(i);
listOut2(counter) = setIn(j);
outA(counter) = setIn(i);
outB(counter) = setIn(j);
counter = counter + 1;
end
end
......
package ba.util;
component TrajectoryToLine{
component TrajectoryToLines{
ports in Q^{3,5} trajectoryIn,
//m-1 lineOuts
out Q^{4,1} lineOut[4];
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment