QPainterPath *getPath(const CLRenderCurve* pCurve, const CLBoundingBox *pBB)
{
QPainterPath *path = new QPainterPath();
const std::vector<CLRenderPoint*>& elements = *pCurve->getListOfCurveElements();
std::vector<CLRenderPoint*>::const_iterator it = elements.begin();
bool first = true;
for (; it != elements.end(); ++it)
{
const CLRenderPoint* current = *it;
const CLRenderCubicBezier* cubic = dynamic_cast<const CLRenderCubicBezier*>(current);
if (first)
{
moveToPoint(*path, current, pBB);
first = false;
continue;
}
if (cubic != NULL)
{
addToPath(*path, cubic, pBB);
}
else
{
addToPath(*path, current, pBB);
}
}
return path;
}
BOOL findHamilton () {
addToPath(0);
int i, v;
do {
int last = path[pi-1];
BOOL addedany = NO;
for (v = 0; v < V; ++v) {
if (g[last][v]) {
addedany = addedany || addToPath(v);
}
}
if (!addedany)
return NO;
} while (pi < V);
if (pi == V)
path[pi++] = path[0];
return YES;
}
void readFileByPath(FILE* file, char* name, char* originPath, char* path, int offset, RootDir* rd, BOOL* found){
int RD_Size = sizeof(RootDir);
long point = ftell(file);
fseek(file, offset, SEEK_SET);
//printf("INpath:\n");
//puts(path);
//printf("\n");
if(belongTo(path,originPath)){
//printf("belongTo\n");
do {
memset(rd, 0, RD_Size);
fread(rd, RD_Size, 1, file);
if (isDir(rd)){
int fst_Clus = rd->DIR_FstClus[0] + rd->DIR_FstClus[1] * 0x10;
char* newPath = (char*)malloc(MAX);
memset(newPath, 0, strlen(newPath));
memcpy(newPath, path, strlen(path));
RootDir oldRd;
memcpy(&oldRd, rd, RD_Size);
addToPath(newPath, rd->DIR_Name);
readFileByPath(file,name,originPath, newPath, (fst_Clus * 0x200 + DATA_START), rd,found);
memcpy(rd, &oldRd, RD_Size);
free(newPath);
}else{
if (pathEquals(path,originPath)&&fileEquals(rd->DIR_Name,name)&&isValid(rd)){
// printf("%s%s\n",path, rd->DIR_Name);
int fatVal=rd->DIR_FstClus[0] + rd->DIR_FstClus[1] * 0x10;
while(fatVal<0xFF8){
readContent(file,fatVal);
*found=TRUE;
fatVal=getFATValue(file,fatVal);
//printf("FATVAL:%3X\n",fatVal);
if(fatVal==0xFF7){
my_print("坏",1);
my_print("簇",1);
break;
}
}
my_print("\n",1);
return;
}
}
}while(!isEmpty(rd));
}
fseek(file, point, SEEK_SET);
}
void readDirByPath(FILE* file, char* originPath, char* path, int offset, RootDir* rd){
int RD_Size = sizeof(RootDir);
long point = ftell(file);
fseek(file, offset, SEEK_SET);
if(contains(path,originPath)||belongTo(path,originPath)){
int cnt=0;
do {
memset(rd, 0, RD_Size);
fread(rd, RD_Size, 1, file);
if (isDir(rd)){
int fst_Clus = rd->DIR_FstClus[0] + rd->DIR_FstClus[1] * 0x10;
char* newPath = (char*)malloc(MAX);
memset(newPath, 0, strlen(newPath));
memcpy(newPath, path, strlen(path));
RootDir oldRd;
memcpy(&oldRd, rd, RD_Size);
addToPath(newPath, rd->DIR_Name);
readDirByPath(file, originPath, newPath, (fst_Clus * 0x200 + DATA_START), rd);
memcpy(rd, &oldRd, RD_Size);
free(newPath);
cnt++;
}else{
if (contains(path,originPath)&&isValid(rd)){
// printf("%s%s\n",path, rd->DIR_Name);
printFile(path, rd);
cnt++;
}
}
}while(!isEmpty(rd));
if(!cnt)
printPath(path);
}
fseek(file, point, SEEK_SET);
}
//Encode actions into a readable code
void finishAction(int num) {
flag7 = true;
flag8 = true;
flag9 = true;
flag13 = true;
flag14 = true;
clearTimer(T1);
total_distance += total_distance/13;
motor[motorD] = 0;
motor[motorE] = 0;
string chare; //Placeholder, used to store which action was taken (ie. "a")
string chare2 = ""; //Another placeholder, records the action in 3 digits (ie. "000")
float add; //Used for rounding/data type conversion
int add2; //Used for rounding/data type conversion
if(num == 1) { //LEFT
chare = "c";
add = round(total_distance);
add = abs(add);
add2 = add;
chare2 = add2;
if(strlen(chare2) < 3) {
if(strlen(chare2) == 2) {
string addon = "0"; //Used to turn "9" into "009"
strcat(addon, chare2);
chare2 = addon;
} else if(strlen(chare2) == 1) {
string addon = "00";
strcat(addon, chare2);
chare2 = addon;
} else {
chare2 = "000";
}
}
strcat(chare, chare2);
addToPath(chare); //Adds the 4 digit action to the path (ie. "c070", which is turn left 70 degrees)
} else if(num == 3) { //RIGHT
chare = "d";
add = round(total_distance);
add = abs(add);
add2 = add;
chare2 = add2;
if(strlen(chare2) < 3) {
if(strlen(chare2) == 2) {
string addon = "0";
strcat(addon, chare2);
chare2 = addon;
} else if(strlen(chare2) == 1) {
string addon = "00";
strcat(addon, chare2);
chare2 = addon;
} else {
chare2 = "000";
}
}
strcat(chare, chare2);
addToPath(chare);
} else if(num == 2) { //BACK
chare = "b";
add = nMotorEncoder[motorE]/56.33
add = round(add);
add = abs(add);
add2 = add;
chare2 = add2;
if(strlen(chare2) < 3) {
if(strlen(chare2) == 2) {
string addon = "0";
strcat(addon, chare2);
chare2 = addon;
} else if(strlen(chare2) == 1) {
string addon = "00";
strcat(addon, chare2);
chare2 = addon;
} else {
chare2 = "000";
}
}
strcat(chare, chare2);
addToPath(chare);
} else if(num == 4) { //FORWARDS
chare = "a";
add = nMotorEncoder[motorE]/56.33
add = round(add);
add = abs(add);
add2 = add;
chare2 = add2;
if(strlen(chare2) < 3) {
if(strlen(chare2) == 2) {
string addon = "0";
strcat(addon, chare2);
chare2 = addon;
} else if(strlen(chare2) == 1) {
string addon = "00";
strcat(addon, chare2);
chare2 = addon;
} else {
chare2 = "000";
}
}
strcat(chare, chare2);
addToPath(chare);
} else if(num == 7) { //LSCISSOR
string s = "e002";
addToPath(s);
} else if(num == 8) { //RSCISSOR
string s = "e001";
addToPath(s);
} else if(num == 9) { //SWEEp
string s = "e005";
addToPath(s);
}else if(num == 13) { //RGRABBER
string s = "e003";
addToPath(s);
} else if(num == 14) { //LGRABBER
string s = "e004";
addToPath(s);
}
total_distance = 0;
prev_rot = 0;
resetEncoders();
}
void getNextUnvisited()
{
checkIfAllVisited();
signed char x1 = currentPosition[0];
signed char y1 = currentPosition[1];
signed char cnt =0;
signed char colX=0;
signed char compare;
signed char rowY=0;
signed char foundOne=0;
//ecrobot_sound_tone(900,90,100);
if((allVisited)&&(getTokensFound()!=3))
{
addToPath(none);
return;
}
if(getTokensFound()==3)
{
getPathComplicated(x1,y1,6,6);
return;
}
signed char l,t,r,d;
for(colX=0; colX<=12;colX++){
for(rowY=0; rowY<=12;rowY++){
mazeVisited[colX][rowY]=visited(colX,rowY);
mazeDistance[colX][rowY]=0;
}
}
//now start calculatin'
mazeDistance[x1][y1]=50;
for(cnt=0; cnt<=35 ; cnt++)//nah, don't calculate how much: just do it ;)
{
for(colX=0;colX<=12;colX++)
{
for(rowY=12;rowY>=0;rowY--)
{
//if((mazeDistance[x1][y1]!=0)){foundOne !=0 ;break;};
l=r=t=d=0;
if((hasDirection(left,colX,rowY)==1) && (mazeDistance[colX-1][rowY] != 0))
{
if(colX>0)
l=mazeDistance[colX-1][rowY]-1;
}
if((hasDirection(right,colX,rowY)==1) && (mazeDistance[colX+1][rowY] != 0))
{
if(colX<12)
r=mazeDistance[colX+1][rowY]-1;
}
if((hasDirection(top,colX,rowY)==1) && (mazeDistance[colX][rowY+1] != 0))
{
if(rowY<12)
t=mazeDistance[colX][rowY+1]-1;
}
if((hasDirection(down,colX,rowY)==1) && (mazeDistance[colX][rowY-1] != 0))
{
if(rowY>0)
d=mazeDistance[colX][rowY-1]-1;
}
mazeDistance[colX][rowY]=getLargestValue(l,t,r,d,mazeDistance[colX][rowY]);
}
}
}
for(colX=0; colX<=12;colX++){
for(rowY=0; rowY<=12;rowY++){
mazeDistance[colX][rowY]=50-mazeDistance[colX][rowY];
}
}
for(compare=0; compare<=35;compare++)
{
if(foundOne){return;};
for(rowY=12;rowY>=0;rowY--)
{
if(foundOne){return;};
for(colX=0;colX<=12;colX++)
{
if((mazeVisited[colX][rowY]==0)&&(mazeDistance[colX][rowY]==compare))
{
getPathComplicated(x1,y1,colX,rowY);
foundOne =1;
return;
}
}
}
}
//alle besucht
ecrobot_sound_tone(400,250,100);
systick_wait_ms(250);
ecrobot_sound_tone(900,250,100);
systick_wait_ms(250);
if(!allVisited)
getPathComplicated(x1,y1,6,6);
setAllVisited(1);
}
void getPathComplicated(signed char x1, signed char y1, signed char xDestination, signed char yDestination)
{
if((x1==xDestination)&&(y1==yDestination))
{
stopBoth();
ecrobot_sound_tone(100,500,100);
systick_wait_ms(2000);
endAll();
addToPath(none);
return;
}
//display_currentNode();
//systick_wait_ms(1000);
/*
display_clear(1);
display_goto_xy(0,0);
display_string("gPC called");
display_goto_xy(0,1);
display_int((int)x1,1);
display_goto_xy(3,1);
display_int((int)y1,1);
display_goto_xy(0,2);
display_int((int)xDestination,1);
display_goto_xy(3,2);
display_int((int)yDestination,1);
display_update();
*/
//systick_wait_ms(1000);
signed char xCurrent;
signed char yCurrent;
signed char startReached = 0;
signed char endReached = 0;
signed char xDistance= xDestination - x1;
signed char yDistance= yDestination - y1;
signed char yD2=0;
signed char xD2=0;
if(yDistance<0){yD2=-yDistance;}else{yD2=yDistance;}
if(xDistance<0){xD2=-xDistance;}else{xD2=xDistance;}
char maxIterations = (xD2+yD2)/2+40;//40 is safety padding
char cntLoops;
if(maxIterations<0)
maxIterations=-maxIterations;
//************************************** MAIN BODY ******
int x,y;
for(x=0;x<=12;x++)
{
for(y=0;y<=12;y++)
{
mazeStart[x][y]=0;
mazeEnd[x][y]=0;
}
}
mazeStart[x1][y1]=50;
int colX=0;
int rowY=0;
mazeEnd[xDestination][yDestination]=50;
signed char l,r,t,d;
for(cntLoops=0; cntLoops<=maxIterations*2; cntLoops++)
{
if((startReached==1) && (endReached==1))
{
cntLoops = (maxIterations*2)+1;
}
//***************************set mazeStart's values checks now if values overflow
for(colX=0;colX<=12;colX++)
{
for(rowY=12;rowY>=0;rowY--)
{
if(mazeStart[xDestination][yDestination]!=0){endReached = 1;break;};
//check if the field has the path to the adjecant field
l=r=t=d=0;
if((hasDirection(left,colX,rowY)==1) && (mazeStart[colX-1][rowY] != 0))
{
if(colX>0)
l=mazeStart[colX-1][rowY]-1;
}
if((hasDirection(right,colX,rowY)==1) && (mazeStart[colX+1][rowY] != 0))
{
if(colX<12)
r=mazeStart[colX+1][rowY]-1;
}
if((hasDirection(top,colX,rowY)==1) && (mazeStart[colX][rowY+1] != 0))
{
if(rowY<12)
t=mazeStart[colX][rowY+1]-1;
}
if((hasDirection(down,colX,rowY)==1) && (mazeStart[colX][rowY-1] != 0))
{
if(rowY>0)
d=mazeStart[colX][rowY-1]-1;
}
mazeStart[colX][rowY]=getLargestValue(l,t,r,d,mazeStart[colX][rowY]);
}
}
//*************************** set mazeEnd's values
for(colX=0;colX<=12;colX++)
{
for(rowY=12;rowY>=0;rowY--)
{
if((mazeEnd[x1][y1]!=0)){startReached = 1;break;};
l=r=t=d=0;
if((hasDirection(left,colX,rowY)==1) && (mazeEnd[colX-1][rowY] != 0))
{
if(colX>0)
l=mazeEnd[colX-1][rowY]-1;
}
if((hasDirection(right,colX,rowY)==1) && (mazeEnd[colX+1][rowY] != 0))
{
if(colX<12)
r=mazeEnd[colX+1][rowY]-1;
}
if((hasDirection(top,colX,rowY)==1) && (mazeEnd[colX][rowY+1] != 0))
{
if(rowY<12)
t=mazeEnd[colX][rowY+1]-1;
}
if((hasDirection(down,colX,rowY)==1) && (mazeEnd[colX][rowY-1] != 0))
{
if(rowY>0)
d=mazeEnd[colX][rowY-1]-1;
}
mazeEnd[colX][rowY]=getLargestValue(l,t,r,d,mazeEnd[colX][rowY]);
}
}
}
//prnt values both matrixes now added into mazeStart
int ty, tx;
tx=ty=0;
for(ty=12; ty>=0; ty--)
{
for(tx=0; tx<=12; tx++)
{
mazeStart[tx][ty]=mazeStart[tx][ty]+mazeEnd[tx][ty];
mazeEnd[tx][ty]=0;
}
}
//now add way of highest numbers to path so it can be printed or driven
xCurrent = x1;
yCurrent = y1;
signed char xDisLeft = xDistance;
signed char yDisLeft = yDistance;
signed char pathValue = mazeStart[x1][y1];
mazeEnd[xCurrent][yCurrent]=1; //repurposing mazeE to store already visited Nodes
if(endReached==1)
{
while((xCurrent!=xDestination) || (yCurrent != yDestination))
{
if((xDisLeft>0)||(((mazeEnd[xCurrent-1][yCurrent]==1)||(mazeStart[xCurrent-1][yCurrent]!=pathValue||(hasDirection(left,xCurrent,yCurrent)==0)))&&((mazeEnd[xCurrent][yCurrent+1]==1)||(mazeStart[xCurrent][yCurrent+1]!=pathValue)||(hasDirection(top,xCurrent,yCurrent)==0))&&((mazeEnd[xCurrent][yCurrent-1]==1)||(mazeStart[xCurrent][yCurrent-1]!=pathValue)||(hasDirection(down,xCurrent,yCurrent)==0))))
{
if(hasDirection(right,xCurrent,yCurrent) && (mazeStart[xCurrent+1][yCurrent]==pathValue) && (mazeEnd[xCurrent+1][yCurrent]==0))
{
addToPath(right);
mazeEnd[xCurrent][yCurrent]=1;
xCurrent++;
xDisLeft--;
}
}
if((xCurrent==xDestination) && (yCurrent == yDestination))
break;
if(xDisLeft<0||(((mazeEnd[xCurrent+1][yCurrent]==1)||(mazeStart[xCurrent+1][yCurrent]!=pathValue)||(hasDirection(right,xCurrent,yCurrent)==0))&&((mazeEnd[xCurrent][yCurrent+1]==1)||(mazeStart[xCurrent][yCurrent+1]!=pathValue)||(hasDirection(top,xCurrent,yCurrent)==0))&&((mazeEnd[xCurrent][yCurrent-1]==1)||(mazeStart[xCurrent][yCurrent-1]!=pathValue)||(hasDirection(down,xCurrent,yCurrent)==0))))
{
if(hasDirection(left,xCurrent,yCurrent) && (mazeStart[xCurrent-1][yCurrent]==pathValue) && (mazeEnd[xCurrent-1][yCurrent]==0))
{
addToPath(left);
mazeEnd[xCurrent][yCurrent]=1;
xCurrent--;
xDisLeft++;
}
}
if((xCurrent==xDestination) && (yCurrent == yDestination))
break;
if(yDisLeft>0||(((mazeEnd[xCurrent+1][yCurrent]==1)||(mazeStart[xCurrent+1][yCurrent]!=pathValue)||(hasDirection(right,xCurrent,yCurrent)==0))&&((mazeEnd[xCurrent][yCurrent-1]==1)||(mazeStart[xCurrent][yCurrent-1]!=pathValue)||(hasDirection(down,xCurrent,yCurrent)==0))&&((mazeEnd[xCurrent-1][yCurrent]==1)||(mazeStart[xCurrent-1][yCurrent]!=pathValue)||(hasDirection(left,xCurrent,yCurrent)==0))))
{
if(hasDirection(top,xCurrent,yCurrent) && (mazeStart[xCurrent][yCurrent+1]==pathValue) && (mazeEnd[xCurrent][yCurrent+1]==0))
{
addToPath(top);
mazeEnd[xCurrent][yCurrent]=1;
yCurrent++;
yDisLeft--;
}
}
if((xCurrent==xDestination) && (yCurrent == yDestination))
break;
if(yDisLeft<0||(((mazeEnd[xCurrent+1][yCurrent]==1)||(mazeStart[xCurrent+1][yCurrent]!=pathValue)||(hasDirection(right,xCurrent,yCurrent)==0))&&((mazeEnd[xCurrent][yCurrent+1]==1)||(mazeStart[xCurrent][yCurrent+1]!=pathValue)||(hasDirection(top,xCurrent,yCurrent)==0))&&((mazeEnd[xCurrent-1][yCurrent]==1)||(mazeStart[xCurrent-1][yCurrent]!=pathValue)||(hasDirection(left,xCurrent,yCurrent)==0))))
{
if(hasDirection(down,xCurrent,yCurrent) && (mazeStart[xCurrent][yCurrent-1]==pathValue) && (mazeEnd[xCurrent][yCurrent-1]==0))
{
addToPath(down);
mazeEnd[xCurrent][yCurrent]=1;
yCurrent--;
yDisLeft++;
}
}
if((xCurrent==xDestination) && (yCurrent == yDestination))
break;
}
}
/*
display_goto_xy(0,3);
display_string("ret:");
display_goto_xy(4,3);
display_int(pathStorage[1],1);
display_update();*/
//systick_wait_ms(2000);
}
