TimeCutoffFilter.emam 1.56 KB
Newer Older
1
2
package ba.intersection;

3
component TimeCutoffFilter<N1 m = 5>(adaptable Q timeCutoff){
4
    ports in B collisionIn,
5
6
          in Z indexInA,
          in Z indexInB,
7
          in Q^{2,1} colPosIn,
8
9
          in Q^{3,m} trajectoryInA,
          in Q^{3,m} trajectoryInB,
10
          //in Q timeCutoff,
11
12
13
          out B collisionOut;

    implementation Math{
14
        if ((collisionIn) && (indexInA >= 1) && (indexInB >= 1) && (indexInA <= m - 1) && (indexInB <= m - 1))
15
            //TODO: with norm()?
16
17
            Q dx1 = trajectoryInA(1,indexInA) - trajectoryInA(1,indexInA + 1);
            Q dy1 = trajectoryInA(2,indexInA) - trajectoryInA(2,indexInA + 1);
18
19
            Q dist1 = sqrt(dx1*dx1 + dy1*dy1);

20
21
            Q dx2 = trajectoryInB(1,indexInB) - trajectoryInB(1,indexInB + 1);
            Q dy2 = trajectoryInB(2,indexInB) - trajectoryInB(2,indexInB + 1);
22
23
            Q dist2 = sqrt(dx2*dx2 + dy2*dy2);

24
25
            Q dxp1 = trajectoryInA(1,indexInA) - colPosIn(1,1);
            Q dyp1 = trajectoryInA(2,indexInA) - colPosIn(2,1);
26
27
            Q distp1 = sqrt(dxp1*dxp1 + dyp1*dyp1);

28
29
            Q dxp2 = trajectoryInB(1,indexInB) - colPosIn(1,1);
            Q dyp2 = trajectoryInB(2,indexInB) - colPosIn(2,1);
30
31
            Q distp2 = sqrt(dxp2*dxp2 + dyp2*dyp2);

32
33
            Q time1 = distp1/dist1 * (trajectoryInA(3,indexInA + 1) - trajectoryInA(3,indexInA));
            Q time2 = distp2/dist2 * (trajectoryInB(3,indexInB + 1) - trajectoryInB(3,indexInB));
34
35
36
37
38
39
40

            collisionOut = abs(time1 - time2) < timeCutoff;
        else
            collisionOut = False;
        end
    }
}