void Linearization::run()
{
std::cout << "Run with k=" << k
<< ", RF=" << rf
<< ", DT=" << dt
<< std::endl;
FileGraphReader reader(fileName);
globalTimer.start();
std::cout << "Reading file..." << std::flush;
directedGraph = new DirectedGraph();
directedGraph->loadGraph(reader);
reader.reset();
undirectedGraph = new UndirectedGraph();
undirectedGraph->loadGraph(reader);
for (int i = 0; i != undirectedGraph->getMaxNodeId() + 1; i++) {
if (undirectedGraph->hasNode(i) && undirectedGraph->degreeOfNode(i) != 0) {
this->randomCandidate.push_back(i);
}
}
globalTimer.printSecondsAndReset();
std::cout << "DirectedEdge/UndirectedEdge: " << directedGraph->getTotalEdgeCount();
std::cout << "/" << undirectedGraph->getTotalEdgeCount() << std::endl;
linearization();
globalTimer.printSecondsAndReset();
printStatistics();
//writeBinary();
}
void process_sampl_V()
{
int i;
// make tbl (input : position / output : sensor value)
// L0
for(i = 0 ; i < 256 ; i++) ssv_L0[i] = 0; // clear
linearization(ssv_L0, sV0, 0 , sV0 ,sFL0[yV0] );
linearization(ssv_L0 , sV0, sFL0[yV0], sV1 ,sFL0[yV1] );
linearization(ssv_L0 , sV1, sFL0[yV1], sV2 ,sFL0[yV2] );
linearization(ssv_L0 , sV2, sFL0[yV2], sV3 ,sFL0[yV3] );
linearization(ssv_L0 , sV3, sFL0[yV3], sV4 ,sFL0[yV4] );
linearization(ssv_L0 , sV4, sFL0[yV4], sV5 ,sFL0[yV5] );
linearization(ssv_L0 , sV5, sFL0[yV5], sV6 ,sFL0[yV6] );
linearization(ssv_L0 , sV6, sFL0[yV6], sV7 ,sFL0[yV7] );
linearization(ssv_L0 , sV7, sFL0[yV7], sV8 ,sFL0[yV8] );
linearization(ssv_L0 , sV8, sFL0[yV8], sV9 ,sFL0[yV9] );
linearization(ssv_L0 , sV9, sFL0[yV9], sV10 ,sFL0[yV10] );
linearization(ssv_L0 , sV10,sFL0[yV10],sV10 ,255 );
// R0
for(i = 0 ; i < 256 ; i++) ssv_R0[i] = 0; // clear
linearization(ssv_R0, sV0 , 0 , sV0 , sFR0[yV0] );
linearization(ssv_R0, sV0 , sFR0[yV0] , sV1 , sFR0[yV1] );
linearization(ssv_R0, sV1 , sFR0[yV1] , sV2 , sFR0[yV2] );
linearization(ssv_R0, sV2 , sFR0[yV2] , sV3 , sFR0[yV3] );
linearization(ssv_R0, sV3 , sFR0[yV3] , sV4 , sFR0[yV4] );
linearization(ssv_R0, sV4 , sFR0[yV4] , sV5 , sFR0[yV5] );
linearization(ssv_R0, sV5 , sFR0[yV5] , sV6 , sFR0[yV6] );
linearization(ssv_R0, sV6 , sFR0[yV6] , sV7 , sFR0[yV7] );
linearization(ssv_R0, sV7 , sFR0[yV7] , sV8 , sFR0[yV8] );
linearization(ssv_R0, sV8 , sFR0[yV8] , sV9 , sFR0[yV9] );
linearization(ssv_R0, sV9 , sFR0[yV9] , sV10 , sFR0[yV10]);
linearization(ssv_R0, sV10 , sFR0[yV10], sV10 , 255 );
// make tbl (input : sensor value / output : position)
// L0
for(i = 0 ; i < 256 ; i++) pos_L0[i] = 0; // clear
linearization(pos_L0, 0 , sV0 ,sFL0[yV0] , sV0 );
linearization(pos_L0, sFL0[yV0] , sV0 ,sFL0[yV1] , sV1 );
linearization(pos_L0, sFL0[yV1] , sV1 ,sFL0[yV2] , sV2 );
linearization(pos_L0, sFL0[yV2] , sV2 ,sFL0[yV3] , sV3 );
linearization(pos_L0, sFL0[yV3] , sV3 ,sFL0[yV4] , sV4 );
linearization(pos_L0, sFL0[yV4] , sV4 ,sFL0[yV5] , sV5 );
linearization(pos_L0, sFL0[yV5] , sV5 ,sFL0[yV6] , sV6 );
linearization(pos_L0, sFL0[yV6] , sV6 ,sFL0[yV7] , sV7 );
linearization(pos_L0, sFL0[yV7] , sV7 ,sFL0[yV8] , sV8 );
linearization(pos_L0, sFL0[yV8] , sV8 ,sFL0[yV9] , sV9 );
linearization(pos_L0, sFL0[yV9] , sV9 ,sFL0[yV10] , sV10 );
linearization(pos_L0, sFL0[yV10] , sV10 ,255 , sV10 );
// R0
for(i = 0 ; i < 256 ; i++) pos_R0[i] = 0; // clear
linearization(pos_R0, 0 , sV0 ,sFR0[yV0] , sV0 );
linearization(pos_R0, sFR0[yV0] , sV0 ,sFR0[yV1] , sV1 );
linearization(pos_R0, sFR0[yV1] , sV1 ,sFR0[yV2] , sV2 );
linearization(pos_R0, sFR0[yV2] , sV2 ,sFR0[yV3] , sV3 );
linearization(pos_R0, sFR0[yV3] , sV3 ,sFR0[yV4] , sV4 );
linearization(pos_R0, sFR0[yV4] , sV4 ,sFR0[yV5] , sV5 );
linearization(pos_R0, sFR0[yV5] , sV5 ,sFR0[yV6] , sV6 );
linearization(pos_R0, sFR0[yV6] , sV6 ,sFR0[yV7] , sV7 );
linearization(pos_R0, sFR0[yV7] , sV7 ,sFR0[yV8] , sV8 );
linearization(pos_R0, sFR0[yV8] , sV8 ,sFR0[yV9] , sV9 );
linearization(pos_R0, sFR0[yV9] , sV9 ,sFR0[yV10] , sV10 );
linearization(pos_R0, sFR0[yV10] , sV10 ,255 , sV10 );
// set threshold ///////////////
threshold_L0 = (unsigned char)((int)sFL0[540]); // 540
threshold_R0 = (unsigned char)((int)sFR0[540]); // 540
threshold_L0_A = (int)threshold_L0 * 1 / 3;
threshold_R0_A = (int)threshold_R0 * 1 / 3;
}
void process_sampl_H()
{
int i;
// make tbl (input : position / output : sensor value)
// L45
for(i = 0 ; i < 256 ; i++) ssv_L45[i] = 0; // clear
linearization(ssv_L45, 0,((sL1[SS_L45]>sC1[SS_L45])?255:0), xL3, sL3[SS_L45]); // LeftWall~L3
linearization(ssv_L45, xL3, sL3[SS_L45], xL2, sL2[SS_L45]); // L3~L2
linearization(ssv_L45, xL2, sL2[SS_L45], xL1, sL1[SS_L45]); // L2~L1
linearization(ssv_L45, xL1, sL1[SS_L45], xC1, sC1[SS_L45]); // L1~C1
linearization(ssv_L45, xC1, sC1[SS_L45], xR1, sR1[SS_L45]); // C1~R1
linearization(ssv_L45, xR1, sR1[SS_L45], xR2, sR2[SS_L45]); // R1~R2
linearization(ssv_L45, xR2, sR2[SS_L45], xR3, sR3[SS_L45]); // R2~R3
linearization(ssv_L45, xR3, sR3[SS_L45], 255,((sC1[SS_L45]<sR1[SS_L45])?255:0) ); // R3~RightWall
// R45
for(i = 0 ; i < 256 ; i++) ssv_R45[i] = 0; // clear
linearization(ssv_R45, 0,((sL1[SS_R45]>sC1[SS_R45])?255:0), xL3, sL3[SS_R45]); // LeftWall~L3
linearization(ssv_R45, xL3, sL3[SS_R45], xL2, sL2[SS_R45]); // L3~L2
linearization(ssv_R45, xL2, sL2[SS_R45], xL1, sL1[SS_R45]); // L2~L1
linearization(ssv_R45, xL1, sL1[SS_R45], xC1, sC1[SS_R45]); // L1~C1
linearization(ssv_R45, xC1, sC1[SS_R45], xR1, sR1[SS_R45]); // C1~R1
linearization(ssv_R45, xR1, sR1[SS_R45], xR2, sR2[SS_R45]); // R1~R2
linearization(ssv_R45, xR2, sR2[SS_R45], xR3, sR3[SS_R45]); // R2~R3
linearization(ssv_R45, xR3, sR3[SS_R45], 255,((sC1[SS_R45]<sR1[SS_R45])?255:0) ); // R3~RightWall
// L90
for(i = 0 ; i < 256 ; i++) ssv_L90[i] = 0; // clear
linearization(ssv_L90, 0,((sL1[SS_L90]>sC1[SS_L90])?255:0), xL3, sL3[SS_L90]); // LeftWall~L3
linearization(ssv_L90, xL3, sL3[SS_L90], xL2, sL2[SS_L90]); // L3~L2
linearization(ssv_L90, xL2, sL2[SS_L90], xL1, sL1[SS_L90]); // L2~L1
linearization(ssv_L90, xL1, sL1[SS_L90], xC1, sC1[SS_L90]); // L1~C1
linearization(ssv_L90, xC1, sC1[SS_L90], xR1, sR1[SS_L90]); // C1~R1
linearization(ssv_L90, xR1, sR1[SS_L90], xR2, sR2[SS_L90]); // R1~R2
linearization(ssv_L90, xR2, sR2[SS_L90], xR3, sR3[SS_L90]); // R2~R3
linearization(ssv_L90, xR3, sR3[SS_L90], 255,((sC1[SS_L90]<sR1[SS_L90])?255:0) ); // R3~RightWall
// R90
for(i = 0 ; i < 256 ; i++) ssv_R90[i] = 0; // clear
linearization(ssv_R90, 0,((sL1[SS_R90]>sC1[SS_R90])?255:0), xL3, sL3[SS_R90]); // LeftWall~L3
linearization(ssv_R90, xL3, sL3[SS_R90], xL2, sL2[SS_R90]); // L3~L2
linearization(ssv_R90, xL2, sL2[SS_R90], xL1, sL1[SS_R90]); // L2~L1
linearization(ssv_R90, xL1, sL1[SS_R90], xC1, sC1[SS_R90]); // L1~C1
linearization(ssv_R90, xC1, sC1[SS_R90], xR1, sR1[SS_R90]); // C1~R1
linearization(ssv_R90, xR1, sR1[SS_R90], xR2, sR2[SS_R90]); // R1~R2
linearization(ssv_R90, xR2, sR2[SS_R90], xR3, sR3[SS_R90]); // R2~R3
linearization(ssv_R90, xR3, sR3[SS_R90], 255,((sC1[SS_R90]<sR1[SS_R90])?255:0) ); // R3~RightWall
// make tbl (input : sensor value / output : position)
// L45
for(i = 0 ; i < 256 ; i++) pos_L45[i] = 0; // clear
linearization(pos_L45,sL3[SS_L45],xL3, ((sL1[SS_L45]>sC1[SS_L45])?255:0),0); // LeftWall~L3
linearization(pos_L45, sL2[SS_L45], xL2, sL3[SS_L45], xL3); // L3~L2
linearization(pos_L45, sL1[SS_L45], xL1, sL2[SS_L45], xL2); // L2~L1
linearization(pos_L45, sC1[SS_L45], xC1, sL1[SS_L45], xL1); // L1~C1
linearization(pos_L45, sR1[SS_L45], xR1, sC1[SS_L45], xC1); // C1~R1
linearization(pos_L45, sR2[SS_L45], xR2, sR1[SS_L45], xR1); // R1~R2
linearization(pos_L45, sR3[SS_L45], xR3, sR2[SS_L45], xR2); // R2~R3
linearization(pos_L45, ((sC1[SS_L45]<sR1[SS_L45])?255:0),255, sR3[SS_L45], xR3); // R3~RightWall
// R45
for(i = 0 ; i < 256 ; i++) pos_R45[i] = 0; // clear
linearization(pos_R45, ((sL1[SS_R45]>sC1[SS_R45])?255:0),0,sL3[SS_R45],xL3); // LeftWall~L3
linearization(pos_R45, sL3[SS_R45], xL3, sL2[SS_R45], xL2); // L3~L2
linearization(pos_R45, sL2[SS_R45], xL2, sL1[SS_R45], xL1); // L2~L1
linearization(pos_R45, sL1[SS_R45], xL1, sC1[SS_R45], xC1); // L1~C1
linearization(pos_R45, sC1[SS_R45], xC1, sR1[SS_R45], xR1); // C1~R1
linearization(pos_R45, sR1[SS_R45], xR1, sR2[SS_R45], xR2); // R1~R2
linearization(pos_R45, sR2[SS_R45], xR2, sR3[SS_R45], xR3); // R2~R3
linearization(pos_R45, sR3[SS_R45], xR3, ((sC1[SS_R45]<sR1[SS_R45])?255:0),255); // R3~RightWall
// L90
for(i = 0 ; i < 256 ; i++) pos_L90[i] = 0; // clear
linearization(pos_L90,sL3[SS_L90],xL3, ((sL1[SS_L90]>sC1[SS_L90])?255:0),0); // LeftWall~L3
linearization(pos_L90, sL2[SS_L90], xL2, sL3[SS_L90], xL3); // L3~L2
linearization(pos_L90, sL1[SS_L90], xL1, sL2[SS_L90], xL2); // L2~L1
linearization(pos_L90, sC1[SS_L90], xC1, sL1[SS_L90], xL1); // L1~C1
linearization(pos_L90, sR1[SS_L90], xR1, sC1[SS_L90], xC1); // C1~R1
linearization(pos_L90, sR2[SS_L90], xR2, sR1[SS_L90], xR1); // R1~R2
linearization(pos_L90, sR3[SS_L90], xR3, sR2[SS_L90], xR2); // R2~R3
linearization(pos_L90, ((sC1[SS_L90]<sR1[SS_L90])?255:0),255, sR3[SS_L90], xR3); // R3~RightWall
// R90
for(i = 0 ; i < 256 ; i++) pos_R90[i] = 0; // clear
linearization(pos_R90, ((sL1[SS_R90]>sC1[SS_R90])?255:0),0,sL3[SS_R90],xL3); // LeftWall~L3
linearization(pos_R90, sL3[SS_R90], xL3, sL2[SS_R90], xL2); // L3~L2
linearization(pos_R90, sL2[SS_R90], xL2, sL1[SS_R90], xL1); // L2~L1
linearization(pos_R90, sL1[SS_R90], xL1, sC1[SS_R90], xC1); // L1~C1
linearization(pos_R90, sC1[SS_R90], xC1, sR1[SS_R90], xR1); // C1~R1
linearization(pos_R90, sR1[SS_R90], xR1, sR2[SS_R90], xR2); // R1~R2
linearization(pos_R90, sR2[SS_R90], xR2, sR3[SS_R90], xR3); // R2~R3
linearization(pos_R90, sR3[SS_R90], xR3, ((sC1[SS_R90]<sR1[SS_R90])?255:0),255); // R3~RightWall
// set threshold ///////////////
threshold_L45 = (unsigned char)((int)sR3[SS_L45] * 2 / 3); // 2/3
threshold_R45 = (unsigned char)((int)sL3[SS_R45] * 2 / 3); // 2/3
threshold_L45_LOW=(unsigned char)((int)sR3[SS_L45] * 1 / 2); // 1/2
threshold_R45_LOW=(unsigned char)((int)sL3[SS_R45] * 1 / 2); // 1/2
/*
threshold_L45_D = (unsigned char)((int)sR1[SS_L45] * 2 / 3); // 2/3
threshold_R45_D = (unsigned char)((int)sL1[SS_R45] * 2 / 3); // 2/3
threshold_L45_DL= (unsigned char)((int)sR1[SS_L45] * 1 / 2); // 1/2
threshold_R45_DL= (unsigned char)((int)sL1[SS_R45] * 1 / 2); // 1/2
*/
threshold_L45_D = (unsigned char)((int)sC1[SS_L45] * 2 / 3); // 2/3
threshold_R45_D = (unsigned char)((int)sC1[SS_R45] * 2 / 3); // 2/3
threshold_L45_DL= (unsigned char)((int)sC1[SS_L45] * 1 / 2); // 1/2
threshold_R45_DL= (unsigned char)((int)sC1[SS_R45] * 1 / 2); // 1/2
threshold_L45_BR= (unsigned char)((int)sR3[SS_L45] * 2 / 3); // 1/2
threshold_R45_BR= (unsigned char)((int)sL3[SS_R45] * 2 / 3); // 1/2
threshold_L45_BRL=(unsigned char)((int)sR3[SS_L45] * 1 / 2); // 1/3
threshold_R45_BRL=(unsigned char)((int)sL3[SS_R45] * 1 / 2); // 1/3
threshold_L90 = (unsigned char)((int)sR3[SS_L90] * 2 / 3); // 2/3
threshold_R90 = (unsigned char)((int)sL3[SS_R90] * 2 / 3); // 2/3
threshold_L90_LOW=(unsigned char)((int)sR3[SS_L90] * 1 / 3); // 1/2
threshold_R90_LOW=(unsigned char)((int)sL3[SS_R90] * 1 / 3); // 1/2
}
int LinearRelaxFixed::inlinearization(const IntervalVector& box, LinearSolver& lp_solver){
return linearization(box,lp_solver);
}
