void WildcardManager::constructPath(const char* path, char **res, uint8 *depth) { if (!path) throw gcException(ERR_BADPATH); (*depth)++; if (*depth > 25) throw gcException(ERR_WILDCARD, "Hit max recursion while constructing path (posible loop)."); size_t len = strlen(path); int32 start = -1; int32 stop = 0; std::vector<char*> list; AutoDeleteV<std::vector<char*>> lad(list); //split the string up into section based on %% and then add to vector for (size_t x=0; x<len; x++) { if (path[x] == '%') { if (x==0) { start = 0; } else if (start == -1) { start = (int32)x; char* temp = new char[start-stop+1]; for (int32 y=stop; y<=start; y++) temp[y-stop] = path[y]; temp[start-stop] = '\0'; list.push_back(temp); } else { stop = (int32)x; char *temp = new char[stop-start+1]; for (int32 y = start; y<=stop; y++) temp[y-start] = path[y]; temp[stop-start] = '\0'; list.push_back(temp); start = -1; stop++; } } else if (x>=len-1) { char* temp = new char[len-stop+1]; for (int32 y=stop; y<(int32)len; y++) temp[y-stop] = path[y]; temp[len-stop] = '\0'; list.push_back(temp); } } //all those starting with % are wildcards so resolve them for (size_t x=0; x<list.size(); x++) { if (list[x][0] == '%') { size_t len = strlen(list[x]); char *temp = new char[len]; for (size_t y=1; y<len; y++) temp[y-1] = list[x][y]; temp[len-1]='\0'; if (strlen(temp)==0) { delete [] temp; throw gcException(ERR_WILDCARD, gcString("Failed to find wildcard [{0}] Current node is null", path)); } AutoDelete<char> tad(temp); AutoDelete<WildcardInfo> wad; WildcardInfo* wcInfo = NULL; if (Safe::stricmp(temp, "TEMP") == 0) { WCSpecialInfo info; info.name = "temp"; onNeedInstallSpecialEvent(info); if (info.handled) { wcInfo = new WildcardInfo("temp", info.result.c_str(), "temp", true); AutoDelete<WildcardInfo> ad(wcInfo); wad = ad; } } else { wcInfo = findItem(temp); } if (!wcInfo) throw gcException(ERR_WILDCARD, gcString("Failed to find wildcard [{0}]", temp)); resolveWildCard(wcInfo); if (!wcInfo->m_bResolved) throw gcException(ERR_WILDCARD, gcString("Failed to resolve wildcard [{0}]", temp)); if (wcInfo->m_szPath == temp) { //dont do any thing, dont have enough info yet. size_t len = strlen(temp)+3; char* newPath = new char[len]; Safe::snprintf(newPath, len, "%%%s%%", temp); safe_delete(list[x]); list[x] = newPath; } else if (wcInfo->m_szPath != "") { char* newPath = NULL; constructPath(wcInfo->m_szPath.c_str(), &newPath, depth); safe_delete(list[x]); list[x] = newPath; if (Safe::stricmp(temp, "TEMP") != 0) { //this means we dont have to resolve this path next time wcInfo->m_szPath = newPath; } } else { throw gcException(ERR_WILDCARD, gcString("Failed to find wildcard [{0}]", temp)); } } } size_t totalLen = 0; size_t listSize = list.size(); for (size_t x=0; x<listSize; x++) totalLen += strlen(list[x]); safe_delete(*res); *res = new char[totalLen+1]; char* cur = *res; //put list back into one stting; for (size_t x=0; x<listSize; x++) { size_t itemLen = strlen(list[x]); strncpy(cur, list[x], itemLen); cur += itemLen; } (*res)[totalLen] = '\0'; (*depth)--; }
void ComputePath::getPath( Point from,Point to ) { insertInOpenSteps(ShortestPathStep::createFromPos(from)); do { auto currentStep = spOpenSteps.at(0); spClosedSteps.pushBack(currentStep); spOpenSteps.erase(0); if (currentStep->getPos() == to) { constructPath(currentStep); spOpenSteps.clear(); spClosedSteps.clear(); break; } auto points = _layer->getValidStep(currentStep->getPos()); for (int i = 0 ; i < points->count(); i++) { auto v = points->getControlPointAtIndex(i); auto step = ShortestPathStep::createFromPos(v); int closeindex = -1; for (int i = 0 ; i < spClosedSteps.size();i++ ) { if (spClosedSteps.at(i)->isEqual(step)) { closeindex = i; break; } } if (closeindex != -1) { continue; } auto moveCost = this->costToMoveFromStep(currentStep,step); int index = -1; for (int i = 0; i < spOpenSteps.size();i++) { auto item = spOpenSteps.at(i); if (step->isEqual(item)) { index = i; break; } } if (index == -1) { step->setParent(currentStep); step->setGScore(currentStep->getGScore() + moveCost); step->setHScore(this->computeHScoreFromCoord(step->getPos(),to)); this->insertInOpenSteps(step); } else { step = this->spOpenSteps.at(index); if (currentStep->getGScore() + moveCost < step->getGScore()) { step->setGScore(currentStep->getGScore() + moveCost); step->retain(); spOpenSteps.erase(index); this->insertInOpenSteps(step); step->release(); } } } } while (spOpenSteps.size() > 0); }
void WildcardManager::resolveWildCard(WildcardInfo *wcInfo) { if (wcInfo->m_bResolved) return; if (wcInfo->m_szType == "path" || wcInfo->m_szType == "exe") { //if (wildcardCheck(wcInfo->m_szPath.c_str())) // Warning("Wildcard check failed on [%s]\n", string); char* path = NULL; try { constructPath(wcInfo->m_szPath.c_str(), &path); wcInfo->m_szPath = path; wcInfo->m_bResolved = true; } catch (gcException) { } safe_delete(path); } else if (wcInfo->m_szType == "regkey") { wcInfo->m_szPath = UTIL::OS::getConfigValue(wcInfo->m_szPath); wcInfo->m_bResolved = true; } else if (wcInfo->m_szType == "regkey64") { wcInfo->m_szPath = UTIL::OS::getConfigValue(wcInfo->m_szPath, true); wcInfo->m_bResolved = true; } else if (wcInfo->m_szType == "msicheck" || wcInfo->m_szType == "dotnetcheck") { WCSpecialInfo info; info.name = wcInfo->m_szType; info.result = wcInfo->m_szPath; onNeedInstallSpecialEvent(info); if (info.handled) { wcInfo->m_szPath = info.result; wcInfo->m_bResolved = true; } } else if (wcInfo->m_szType == "special") { if (wcInfo->m_szName == "INSTALL_PATH" || wcInfo->m_szName == "PARENT_INSTALL_PATH") { wcInfo->m_bResolved = wcInfo->m_szPath.size() > 0; } else { WCSpecialInfo info; info.name = wcInfo->m_szName; needSpecial(&info); if (info.handled) { wcInfo->m_szPath = info.result; wcInfo->m_bResolved = true; } } } else { Warning(gcString("Unknown Wildcard type: {0}\n", wcInfo->m_szType)); } }
void extractEPK2file(const char *epk_file, struct config_opts_t *config_opts) { int file; if (!(file = open(epk_file, O_RDONLY))) { printf("\nCan't open file %s\n", epk_file); exit(1); } struct stat statbuf; if (fstat(file, &statbuf) < 0) { printf("\nfstat error\n"); exit(1); } int fileLength = statbuf.st_size; printf("File size: %d bytes\n", fileLength); void *buffer; if ( (buffer = mmap(0, fileLength, PROT_READ, MAP_SHARED, file, 0)) == MAP_FAILED ) { printf("\nCannot mmap input file. Aborting\n"); exit(1); } printf("\nVerifying digital signature of EPK2 firmware header...\n"); int verified = 0; DIR* dirFile = opendir(config_opts->config_dir); if (dirFile) { struct dirent* hFile; while ((hFile = readdir(dirFile)) != NULL) { if (!strcmp(hFile->d_name, ".") || !strcmp(hFile->d_name, "..") || hFile->d_name[0] == '.') continue; if (strstr(hFile->d_name, ".pem") || strstr(hFile->d_name, ".PEM")) { printf("Trying RSA key: %s... ", hFile->d_name); SWU_CryptoInit_PEM(config_opts->config_dir, hFile->d_name); int size = SIGNATURE_SIZE+0x634; while (size > SIGNATURE_SIZE) { verified = API_SWU_VerifyImage(buffer, size); if (verified) { printf("Success!\nDigital signature of the firmware is OK. Signed bytes: %d\n\n", size-SIGNATURE_SIZE); break; } size -= 1; } if (!verified) printf("Failed\n"); } if (verified) break; } closedir(dirFile); } if (!verified) { printf("Cannot verify firmware's digital signature (maybe you don't have proper PEM file). Aborting.\n\n"); #ifdef __CYGWIN__ puts("Press any key to continue..."); getch(); #endif if (munmap(buffer, fileLength) == -1) printf("Error un-mmapping the file"); close(file); exit(1); } int headerSize = 0x5B4 + SIGNATURE_SIZE; struct epk2header_t *fwInfo = malloc(headerSize); memcpy(fwInfo, buffer, headerSize); if (memcmp(fwInfo->EPK2magic, EPK2_MAGIC, 4)) { printf("EPK2 header is encrypted. Trying to decrypt...\n"); int uncrypted = 0; char key_file_name[1024] = ""; strcat(key_file_name, config_opts->config_dir); strcat(key_file_name, "/"); strcat(key_file_name, "AES.key"); FILE *fp = fopen(key_file_name, "r"); if (fp == NULL) { printf("\nError: Cannot open AES.key file.\n"); if (munmap(buffer, fileLength) == -1) printf("Error un-mmapping the file"); close(file); free(fwInfo); exit(1); } char* line = NULL; size_t len = 0; ssize_t read; size_t count = 0; while ((read = getline(&line, &len, fp)) != -1) { char* pos = line; for(count = 0; count < sizeof(aes_key)/sizeof(aes_key[0]); count++) { sscanf(pos, "%2hhx", &aes_key[count]); pos += 2 * sizeof(char); } SWU_CryptoInit_AES(aes_key); printf("Trying AES key (%s) ", strtok(line,"\n\r")); decryptImage(buffer+SIGNATURE_SIZE, headerSize-SIGNATURE_SIZE, (unsigned char*)fwInfo+SIGNATURE_SIZE); if (!memcmp(fwInfo->EPK2magic, EPK2_MAGIC, 4)) { printf("Success!\n"); //hexdump(decrypted, headerSize); uncrypted = 1; break; } else { printf("Failed\n"); } } fclose(fp); if (line) free(line); if (!uncrypted) { printf("\nFATAL: Cannot decrypt EPK2 header (proper AES key is missing). Aborting now. Sorry.\n\n"); if (munmap(buffer, fileLength) == -1) printf("Error un-mmapping the file"); close(file); free(fwInfo); #ifdef __CYGWIN__ puts("Press any key to continue..."); getch(); #endif exit(EXIT_FAILURE); } } printf("\nFirmware info\n"); printf("-------------\n"); printf("Firmware magic: %.*s\n", 4, fwInfo->EPK2magic); printf("Firmware otaID: %s\n", fwInfo->otaID); printf("Firmware version: %02x.%02x.%02x.%02x\n", fwInfo->fwVersion[3], fwInfo->fwVersion[2], fwInfo->fwVersion[1], fwInfo->fwVersion[0]); printf("PAK count: %d\n", fwInfo->pakCount); printf("PAKs total size: %d\n", fwInfo->fileSize); printf("Header length: %d\n\n", fwInfo->headerLength); struct epk2header_t *fwFile = (struct epk2header_t*) buffer; struct pak2_t **pakArray = malloc((fwInfo->pakCount) * sizeof(struct pak2_t*)); if (fileLength < fwInfo->fileSize) printf("\n!!!WARNING: Real file size is shorter than file size listed in the header. Number of extracted PAKs will be lowered to filesize...\n"); printf("\nScanning EPK2 firmware...\n"); // Scan PAK segments unsigned char *epk2headerOffset = fwFile->signature; unsigned char *pak2headerOffset = fwInfo->signature + sizeof(struct epk2header_t); struct pak2segmentHeader_t *pak2segmentHeader = (struct pak2segmentHeader_t*) ((fwFile->epakMagic) + (fwInfo->headerLength)); // Contains added lengths of signature data unsigned int signature_sum = sizeof(fwFile->signature) + sizeof(pak2segmentHeader->signature); unsigned int pak2segmentHeaderSignatureLength = sizeof(pak2segmentHeader->signature); int count = 0; int next_pak_length = fwInfo->fileSize; while (count < fwInfo->pakCount) { struct pak2header_t *pakHeader = (struct pak2header_t *) (pak2headerOffset); struct pak2_t *pak = malloc(sizeof(struct pak2_t)); pakArray[count] = pak; pak->header = pakHeader; pak->segment_count = 0; pak->segments = NULL; int verified = 0; struct pak2segmentHeader_t *next_pak_offset = (struct pak2segmentHeader_t*) (epk2headerOffset + pakHeader->nextPAKfileOffset + signature_sum); unsigned int distance_between_paks = (next_pak_offset->name) - (pak2segmentHeader->name); // Last contained PAK... if ((count == (fwInfo->pakCount - 1))) distance_between_paks = next_pak_length + pak2segmentHeaderSignatureLength; unsigned int max_distance = pakHeader->maxPAKsegmentSize + sizeof(struct pak2segmentHeader_t); while (!verified) { unsigned int PAKsegment_length = distance_between_paks; bool is_next_segment_needed = FALSE; if (PAKsegment_length > max_distance) { PAKsegment_length = max_distance; is_next_segment_needed = TRUE; } unsigned int signed_length = next_pak_length; if (signed_length > max_distance) { signed_length = max_distance; } if (count == 0) signed_length = PAKsegment_length; if (!verified && (verified = API_SWU_VerifyImage(pak2segmentHeader->signature, signed_length)) != 1) { printf("Verification of the PAK segment #%u failed (size=0x%X). Trying to fallback...\n", pak->segment_count + 1, signed_length); while (((verified = API_SWU_VerifyImage(pak2segmentHeader->signature, signed_length)) != 1) && (signed_length > 0)) signed_length--; if (verified) printf("Successfully verified with size: 0x%X\n", signed_length); else { printf("Fallback failed. Sorry, aborting now.\n"); if (munmap(buffer, fileLength) == -1) printf("Error un-mmapping the file"); close(file); free(fwInfo); int i; for (i=0; i<count; ++i) free(pakArray[i]); free(pakArray); exit(1); } } // Sum signature lengths signature_sum += pak2segmentHeaderSignatureLength; unsigned int PAKsegment_content_length = (PAKsegment_length - pak2segmentHeaderSignatureLength); if (is_next_segment_needed) { distance_between_paks -= PAKsegment_content_length; next_pak_length -= PAKsegment_content_length; verified = 0; } else next_pak_length = pakHeader->nextPAKlength + pak2segmentHeaderSignatureLength; pak->segment_count++; pak->segments = realloc(pak->segments, pak->segment_count * sizeof(struct pak2segment_t*)); struct pak2segment_t *PAKsegment = malloc(sizeof(struct pak2segment_t)); PAKsegment->header = pak2segmentHeader; PAKsegment->content = pak2segmentHeader->unknown4 + (sizeof(pak2segmentHeader->unknown4)); PAKsegment->content_file_offset = PAKsegment->content - epk2headerOffset; PAKsegment->content_len = signed_length - sizeof(struct pak2segmentHeader_t); pak->segments[pak->segment_count - 1] = PAKsegment; // Move pointer to the next pak segment offset pak2segmentHeader = (struct pak2segmentHeader_t *) (pak2segmentHeader->signature + PAKsegment_length); } pak2headerOffset += sizeof(struct pak2header_t); count++; // File truncation check if ((pakHeader->nextPAKfileOffset + next_pak_length) > fileLength) break; } int last_index = count - 1; struct pak2_t *last_pak = pakArray[last_index]; int PAKsegment_index = last_pak->segment_count - 1; struct pak2segment_t *last_PAKsegment = last_pak->segments[PAKsegment_index]; int last_extracted_file_offset = (last_PAKsegment->content_file_offset + last_PAKsegment->content_len); printf("Last extracted file offset: %d\n\n", last_extracted_file_offset); char fwVersion[1024]; sprintf(fwVersion, "%02x.%02x.%02x.%02x-%s", fwInfo->fwVersion[3], fwInfo->fwVersion[2], fwInfo->fwVersion[1], fwInfo->fwVersion[0], fwInfo->otaID); char targetFolder[1024]=""; constructPath(targetFolder, config_opts->dest_dir, fwVersion, NULL); createFolder(targetFolder); SelectAESkey(pakArray[0], config_opts); int index; for (index = 0; index < last_index + 1; index++) { printPAKinfo(pakArray[index]); const char *pak_type_name; char filename[1024] = ""; char name[4]; sprintf(name, "%.4s", pakArray[index]->header->name); constructPath(filename, targetFolder, name, ".pak"); printf("#%u/%u saving PAK (%s) to file %s\n", index + 1, fwInfo->pakCount, name, filename); int length = writePAKsegment(pakArray[index], filename); free(pakArray[index]); processExtractedFile(filename, targetFolder, name); } if (munmap(buffer, fileLength) == -1) printf("Error un-mmapping the file"); close(file); free(fwInfo); free(pakArray); }
string WebPathSegment::getPath() const { string path; constructPath(path); return path; }
int handle_file(const char *file, struct config_opts_t *config_opts) { const char *dest_dir = config_opts->dest_dir; const char *file_name = basename(strdup(file)); char dest_file[1024] = ""; char lz4pack[1024] = ""; if (check_lzo_header(file)) { constructPath(dest_file, dest_dir, file_name, ".lzounpack"); printf("Extracting LZO file to: %s\n", dest_file); if (lzo_unpack(file, dest_file) == 0) { handle_file(dest_file, config_opts); return EXIT_SUCCESS; } } else if (is_nfsb(file)) { constructPath(dest_file, dest_dir, file_name, ".unnfsb"); printf("Extracting nfsb image to: %s.\n\n", dest_file); unnfsb(file, dest_file); handle_file(dest_file, config_opts); return EXIT_SUCCESS; } else if (is_lz4(file)) { constructPath(dest_file, dest_dir, file_name, ".unlz4"); printf("UnLZ4 file to: %s\n", dest_file); decode_file(file, dest_file); return EXIT_SUCCESS; } else if (is_squashfs(file)) { constructPath(dest_file, dest_dir, file_name, ".unsquashfs"); printf("Unsquashfs file to: %s\n", dest_file); rmrf(dest_file); unsquashfs(file, dest_file); return EXIT_SUCCESS; } else if (is_gzip(file)) { constructPath(dest_file, dest_dir, "", ""); printf("Extracting gzip file %s\n", file_name); strcpy(dest_file, file_uncompress_origname((char *)file, dest_file)); return EXIT_SUCCESS; } else if(is_cramfs_image(file, "be")) { constructPath(dest_file, dest_dir, file_name, ".cramswap"); printf("Swapping cramfs endian for file %s\n",file); cramswap(file, dest_file); return EXIT_SUCCESS; } else if(is_cramfs_image(file, "le")) { constructPath(dest_file, dest_dir, file_name, ".uncramfs"); printf("Uncramfs %s to folder %s\n", file, dest_file); rmrf(dest_file); uncramfs(dest_file, file); return EXIT_SUCCESS; } else if (isFileEPK2(file)) { extractEPK2file(file, config_opts); return EXIT_SUCCESS; } else if (isFileEPK1(file)) { extract_epk1_file(file, config_opts); return EXIT_SUCCESS; } else if (is_kernel(file)) { constructPath(dest_file, dest_dir, file_name, ".unpaked"); printf("Extracting boot image to: %s.\n\n", dest_file); extract_kernel(file, dest_file); handle_file(dest_file, config_opts); return EXIT_SUCCESS; } else if(isPartPakfile(file)) { constructPath(dest_file, dest_dir, remove_ext(file_name), ".txt"); printf("Saving Partition info to: %s\n", dest_file); dump_partinfo(file, dest_file); return EXIT_SUCCESS; } else if(is_jffs2(file)) { constructPath(dest_file, dest_dir, file_name, ".unjffs2"); printf("jffs2extract %s to folder %s\n", file, dest_file); rmrf(dest_file); jffs2extract(file, dest_file, "1234"); return EXIT_SUCCESS; } else if(isSTRfile(file)) { constructPath(dest_file, dest_dir, file_name, ".ts"); setKey(); printf("\nConverting %s file to TS: %s\n", file, dest_file); convertSTR2TS(file, dest_file, 0); return EXIT_SUCCESS; } else if(!memcmp(&file[strlen(file)-3], "PIF", 3)) { constructPath(dest_file, dest_dir, file_name, ".ts"); setKey(); printf("\nProcessing PIF file: %s\n", file); processPIF(file, dest_file); return EXIT_SUCCESS; } else if(symfile_load(file) == 0) { constructPath(dest_file, dest_dir, file_name, ".idc"); printf("Converting SYM file to IDC script: %s\n", dest_file); symfile_write_idc(dest_file); return EXIT_SUCCESS; } return EXIT_FAILURE; }
Path::Path(Unit* unit, Tile* targetTile, bool attack) : m_path (), m_attack(attack) { Q_ASSERT(unit != nullptr); Q_ASSERT(targetTile != nullptr); OpenList open; open.push(new Node(unit->tile(), 0, distance(unit->tile(), targetTile))); ClosedList closed; while (!open.isEmpty()) { Node* current = open.pop(); closed.push(current); if (current->tile() == targetTile) { // Target found constructPath(current); break; } for (Tile* neighbour : current->tile()->neighbours()) { if (closed.contains(neighbour)) { continue; } QScopedPointer<Node> newNode(new Node( neighbour, current->g() + 1, distance(neighbour, targetTile), current )); auto existing = open.find(neighbour); if (existing == open.end()) { // Tile we haven't check before - make sure we can enter if (unit->canEnter(neighbour) || (attack && neighbour == targetTile)) { open.push(newNode.take()); } else { // If we can't enter, don't check again closed.push(newNode.take()); } } else if (newNode->g() < (*existing)->g()) { open.replace(existing, newNode.take()); } } } }
int handle_file(const char *file, struct config_opts_t *config_opts) { const char *dest_dir = config_opts->dest_dir; const char *file_name = basename(strdup(file)); char dest_file[1024] = ""; char lz4pack[1024] = ""; if (check_lzo_header(file)) { constructPath(dest_file, dest_dir, file_name, ".lzounpack"); printf("Extracting LZO file to: %s\n", dest_file); if (lzo_unpack(file, dest_file) == 0) { handle_file(dest_file, config_opts); return EXIT_SUCCESS; } } else if (is_nfsb(file)) { constructPath(dest_file, dest_dir, file_name, ".unnfsb"); printf("Extracting nfsb image to: %s.\n\n", dest_file); unnfsb(file, dest_file); handle_file(dest_file, config_opts); return EXIT_SUCCESS; } else if (is_lz4(file)) { constructPath(dest_file, dest_dir, file_name, ".unlz4"); printf("UnLZ4 file to: %s\n", dest_file); decode_file(file, dest_file); return EXIT_SUCCESS; } else if (is_squashfs(file)) { constructPath(dest_file, dest_dir, file_name, ".unsquashfs"); printf("Unsquashfs file to: %s\n", dest_file); rmrf(dest_file); unsquashfs(file, dest_file); return EXIT_SUCCESS; } else if (is_cramfs_image(file)) { constructPath(dest_file, dest_dir, file_name, ".uncramfs"); printf("Uncramfs file to: %s\n", dest_file); rmrf(dest_file); uncramfs(dest_file, file); return EXIT_SUCCESS; } else if (isFileEPK2(file)) { extractEPK2file(file, config_opts); return EXIT_SUCCESS; } else if (isFileEPK1(file)) { extract_epk1_file(file, config_opts); return EXIT_SUCCESS; } else if (is_kernel(file)) { constructPath(dest_file, dest_dir, file_name, ".unpaked"); printf("Extracting boot image to: %s.\n\n", dest_file); extract_kernel(file, dest_file); handle_file(dest_file, config_opts); return EXIT_SUCCESS; } else if(isSTRfile(file)) { constructPath(dest_file, dest_dir, file_name, ".ts"); setKey(); printf("\nConverting %s file to TS: %s\n", file, dest_file); convertSTR2TS(file, dest_file, 0); return EXIT_SUCCESS; } else if(!memcmp(&file[strlen(file)-3], "PIF", 3)) { constructPath(dest_file, dest_dir, file_name, ".ts"); setKey(); printf("\nProcessing PIF file: %s\n", file); processPIF(file, dest_file); return EXIT_SUCCESS; } else if(symfile_load(file) == 0) { constructPath(dest_file, dest_dir, file_name, ".idc"); printf("Converting SYM file to IDC script: %s\n", dest_file); symfile_write_idc(dest_file); return EXIT_SUCCESS; } return EXIT_FAILURE; }
std::vector<Pnt3f> OctreeAStarAlgorithm::search(OctreePtr Tree, const Pnt3f& Start, const Pnt3f& Goal, PathCostHeuristicFunc CostHeuristicFunc) { _Tree = Tree; _StartNode = _Tree->getNodeThatContains(Start); _GoalNode = _Tree->getNodeThatContains(Goal); _CostHeuristicFunc = CostHeuristicFunc; if(!_StartNode || !_GoalNode) { return _SolutionPath; } //clear lists _SolutionPath.clear(); _OpenNodes.clear(); _ClosedNodes.clear(); //initialize start node ASNodePtr startNode = ASNodePtr(new ASNode); startNode->_OctreeNode = _StartNode; startNode->_CostFromStart = 0; Pnt3f Center; _StartNode->getVolume().getCenter(Center); startNode->_CostToGoal = _CostHeuristicFunc(_Tree, _StartNode, Center); startNode->_Parent.reset(); _OpenNodes.push_back(startNode); while(!_OpenNodes.empty()) { ASNodePtr Node = _OpenNodes.front(); _OpenNodes.erase(_OpenNodes.begin()); if(Node->_OctreeNode == _GoalNode) { constructPath(Node); return _SolutionPath;//success } else { for(Int8 i = 0; i < Node->_OctreeNode->getNeighbors().size(); ++i) { if(!Node->_OctreeNode->getNeighbor(i)->getContainsObstacles()) {//Node->_OctreeNode->getNeighbor(i)->children.size() == 0 Node->_OctreeNode->getNeighbor(i)->getVolume().getCenter(Center); Real32 NewCost = Node->_CostFromStart + _CostHeuristicFunc(_Tree, _StartNode, Center); Int32 locInOpen = inOpen(Node->_OctreeNode->getNeighbor(i)); Int32 locInClosed = inClosed(Node->_OctreeNode->getNeighbor(i)); if((locInOpen >= 0 && _OpenNodes[locInOpen]->_CostFromStart <= NewCost) || (locInClosed >= 0 && _ClosedNodes[locInClosed]->_CostFromStart <= NewCost)) { continue; } else { //initialize a new node ASNodePtr NewNode = ASNodePtr(new ASNode()); NewNode->_OctreeNode = Node->_OctreeNode->getNeighbor(i); NewNode->_OctreeNode->getVolume().getCenter(Center); NewNode->_CostFromStart = _CostHeuristicFunc(_Tree, _StartNode, Center); NewNode->_CostToGoal = _CostHeuristicFunc(_Tree, _GoalNode, Center); NewNode->_Parent = Node; if(locInClosed >= 0) { _ClosedNodes.erase(_ClosedNodes.begin() + locInClosed); } /*if(locInOpen >= 0) * { _OpenNodes.erase(_OpenNodes.begin() + locInOpen); }else{ pushOnOpen(NewNode); }*/ if(locInOpen >= 0) { _OpenNodes.erase(_OpenNodes.begin() + locInOpen); } pushOnOpen(NewNode); }//endif }//endif... }//end for loop...dont with Node }//endif _ClosedNodes.push_back(Node); }//end while loop return _SolutionPath; }
std::vector <int>& CPathAI::getPaths(POINT& O, POINT& D) { initEntrys(); INT offsets[] = {-COLS, 1, COLS, -1}; std::map <int, set <int>, less<int> > operations; std::set <int> initOrigins; initOrigins.insert(point2Index(origin)); operations[0] = initOrigins; TCHAR szDebug[256] = {0}; for (INT i = 0; i < INT_MAX ; i++) { std::set <INT>& pts = operations[i]; std::set <INT> nextStepOrigins; #ifdef _DEBUG wsprintf(szDebug, "-------------------------------\nNO. step: %02d\n", i); OutputDebugString(szDebug); #endif bool runNext = false; for (si = pts.begin(); si != pts.end(); si++) { #ifdef _DEBUG POINT p = {-1, -1}; index2Point(*si, p); wsprintf(szDebug, "\nO (%02d, %02d)\n", p.x, p.y); OutputDebugString(szDebug); #endif for (INT j = 0; j < 4; j++) { INT index = *si + offsets[j]; #ifdef _DEBUG index2Point(index, p); wsprintf(szDebug, "%02d -> (%02d, %02d)\n", j, p.x, p.y); OutputDebugString(szDebug); #endif if (index < 0 || index >= COLS * ROWS) { continue; } if (s.find(index) != s.end()) { continue; } if (entrys[*si].rights + entrys[index].right < entrys[index].rights) { entrys[index].rights = entrys[*si].rights + entrys[index].right; entrys[index].lasts[0] = *si; nextStepOrigins.insert(index); runNext = true; } } } if (!runNext) { break; } for (si = nextStepOrigins.begin(); si != nextStepOrigins.end(); si++) { if (*si == point2Index(destination)) {//destination reached constructPath(); return paths; } s.insert(*si); } operations[i + 1] = nextStepOrigins; } return paths; }