Commit 6643b4e3 authored by Alexander David Hellwig's avatar Alexander David Hellwig
Browse files

Model: fixed out of bounds for intersectionController

parent 2ab2f0a9
...@@ -23,8 +23,8 @@ component System{ ...@@ -23,8 +23,8 @@ component System{
in B slowDown[2], in B slowDown[2],
out Q(0m/s : 60m/s) curVel[2]; out Q(0m/s : 60m/s) curVel[2];
instance IntersectionController intersectionController; instance IntersectionController<2,1,5,4,10> intersectionController;
instance CollisionDetection collisionDetection; instance CollisionDetection<2,1> collisionDetection;
instance CommQuality<B> stopCommQuality[2]; instance CommQuality<B> stopCommQuality[2];
instance VelocityController(3.0) velocityController[2]; instance VelocityController(3.0) velocityController[2];
......
...@@ -9,12 +9,11 @@ component RectIntersection{ ...@@ -9,12 +9,11 @@ component RectIntersection{
in Rectangle rect2, in Rectangle rect2,
out B collision; out B collision;
//check intersection of 4*4 lines w/o permutations
//4*(4+1)/2 = 10 possibilities
instance RectToLines rectToLines1; instance RectToLines rectToLines1;
instance RectToLines rectToLines2; instance RectToLines rectToLines2;
//check intersection of 4*4 lines w/o permutations
//4*(4+1)/2 = 10 possibilities
instance DualSetCompareMatrix<2,4,4,10> dualSetCompare; instance DualSetCompareMatrix<2,4,4,10> dualSetCompare;
instance LineIntersection lineIntersection[10]; instance LineIntersection lineIntersection[10];
......
package ba.intersection; package ba.intersection;
//Finds the earliest line intersection from a set of LineIntersection components //Finds the earliest line intersection from a set of LineIntersection components
component FirstLineIntersection{ component FirstLineIntersection<N1 m1 = 4, N1 m2 = 10>{
ports in B collisionIn[10], ports in B collisionIn[m2],
in Q^{2,1} pointsIn[10], in Q^{2,1} pointsIn[m2],
out B collisionOut, out B collisionOut,
out Z linexA, out Z linexA,
...@@ -14,8 +14,8 @@ component FirstLineIntersection{ ...@@ -14,8 +14,8 @@ component FirstLineIntersection{
Z k = 1; Z k = 1;
B found = False; B found = False;
for i = 1:4 for i = 1:m1
for j = i:4 for j = i:m1
if found == False if found == False
if collisionIn(k) if collisionIn(k)
linexA = i; linexA = i;
......
package ba.intersection; package ba.intersection;
component TimeCutoffFilter(adaptable Q timeCutoff){ component TimeCutoffFilter<N1 m = 5>(adaptable Q timeCutoff){
ports in B collisionIn, ports in B collisionIn,
in Z IndexInA, in Z indexInA,
in Z IndexInB, in Z indexInB,
in Q^{2,1} colPosIn, in Q^{2,1} colPosIn,
in Q^{3,5} trajectoryInA, in Q^{3,m} trajectoryInA,
in Q^{3,5} trajectoryInB, in Q^{3,m} trajectoryInB,
//in Q timeCutoff, //in Q timeCutoff,
out B collisionOut; out B collisionOut;
implementation Math{ implementation Math{
if collisionIn if ((collisionIn) && (indexInA >= 1) && (indexInB >= 1) && (indexInA <= m - 1) && (indexInB <= m - 1))
//TODO: with norm()? //TODO: with norm()?
Q dx1 = trajectoryInA(1,IndexInA) - trajectoryInA(1,IndexInA + 1); Q dx1 = trajectoryInA(1,indexInA) - trajectoryInA(1,indexInA + 1);
Q dy1 = trajectoryInA(2,IndexInA) - trajectoryInA(2,IndexInA + 1); Q dy1 = trajectoryInA(2,indexInA) - trajectoryInA(2,indexInA + 1);
Q dist1 = sqrt(dx1*dx1 + dy1*dy1); Q dist1 = sqrt(dx1*dx1 + dy1*dy1);
Q dx2 = trajectoryInB(1,IndexInB) - trajectoryInB(1,IndexInB + 1); Q dx2 = trajectoryInB(1,indexInB) - trajectoryInB(1,indexInB + 1);
Q dy2 = trajectoryInB(2,IndexInB) - trajectoryInB(2,IndexInB + 1); Q dy2 = trajectoryInB(2,indexInB) - trajectoryInB(2,indexInB + 1);
Q dist2 = sqrt(dx2*dx2 + dy2*dy2); Q dist2 = sqrt(dx2*dx2 + dy2*dy2);
Q dxp1 = trajectoryInA(1,IndexInA) - colPosIn(1,1); Q dxp1 = trajectoryInA(1,indexInA) - colPosIn(1,1);
Q dyp1 = trajectoryInA(2,IndexInA) - colPosIn(2,1); Q dyp1 = trajectoryInA(2,indexInA) - colPosIn(2,1);
Q distp1 = sqrt(dxp1*dxp1 + dyp1*dyp1); Q distp1 = sqrt(dxp1*dxp1 + dyp1*dyp1);
Q dxp2 = trajectoryInB(1,IndexInB) - colPosIn(1,1); Q dxp2 = trajectoryInB(1,indexInB) - colPosIn(1,1);
Q dyp2 = trajectoryInB(2,IndexInB) - colPosIn(2,1); Q dyp2 = trajectoryInB(2,indexInB) - colPosIn(2,1);
Q distp2 = sqrt(dxp2*dxp2 + dyp2*dyp2); Q distp2 = sqrt(dxp2*dxp2 + dyp2*dyp2);
Q time1 = distp1/dist1 * (trajectoryInA(3,IndexInA + 1) - trajectoryInA(3,IndexInA)); Q time1 = distp1/dist1 * (trajectoryInA(3,indexInA + 1) - trajectoryInA(3,indexInA));
Q time2 = distp2/dist2 * (trajectoryInB(3,IndexInB + 1) - trajectoryInB(3,IndexInB)); Q time2 = distp2/dist2 * (trajectoryInB(3,indexInB + 1) - trajectoryInB(3,indexInB));
collisionOut = abs(time1 - time2) < timeCutoff; collisionOut = abs(time1 - time2) < timeCutoff;
else else
......
...@@ -14,8 +14,8 @@ component TrajectoryCollision<N1 m = 5, N1 m1 = 4, N1 m2 = 10>{ ...@@ -14,8 +14,8 @@ component TrajectoryCollision<N1 m = 5, N1 m1 = 4, N1 m2 = 10>{
instance TrajectoryToLines trajectoryToLinesB; instance TrajectoryToLines trajectoryToLinesB;
instance DualSetCompareMatrix<4,1,4,10> dualSetCompare; instance DualSetCompareMatrix<4,1,4,10> dualSetCompare;
instance LineIntersection lineIntersection[m2]; instance LineIntersection lineIntersection[m2];
instance FirstLineIntersection firstLineIntersection; instance FirstLineIntersection<4,10> firstLineIntersection;
instance TimeCutoffFilter(2.0) timeCutoffFilter; instance TimeCutoffFilter<5>(2.0) timeCutoffFilter;
connect trajectoryA -> trajectoryToLinesA.trajectoryIn; connect trajectoryA -> trajectoryToLinesA.trajectoryIn;
connect trajectoryB -> trajectoryToLinesB.trajectoryIn; connect trajectoryB -> trajectoryToLinesB.trajectoryIn;
...@@ -24,8 +24,8 @@ component TrajectoryCollision<N1 m = 5, N1 m1 = 4, N1 m2 = 10>{ ...@@ -24,8 +24,8 @@ component TrajectoryCollision<N1 m = 5, N1 m1 = 4, N1 m2 = 10>{
connect lineIntersection[:].point -> firstLineIntersection.pointsIn[:]; connect lineIntersection[:].point -> firstLineIntersection.pointsIn[:];
connect firstLineIntersection.collisionOut -> timeCutoffFilter.collisionIn; connect firstLineIntersection.collisionOut -> timeCutoffFilter.collisionIn;
connect firstLineIntersection.linexA -> timeCutoffFilter.IndexInA; connect firstLineIntersection.linexA -> timeCutoffFilter.indexInA;
connect firstLineIntersection.linexB -> timeCutoffFilter.IndexInB; connect firstLineIntersection.linexB -> timeCutoffFilter.indexInB;
connect firstLineIntersection.pos -> timeCutoffFilter.colPosIn; connect firstLineIntersection.pos -> timeCutoffFilter.colPosIn;
connect trajectoryA -> timeCutoffFilter.trajectoryInA; connect trajectoryA -> timeCutoffFilter.trajectoryInA;
connect trajectoryB -> timeCutoffFilter.trajectoryInB; connect trajectoryB -> timeCutoffFilter.trajectoryInB;
......
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