Model: fixed out of bounds for intersectionController

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