const WdtTransferRequest &Receiver::init() {
if (validateTransferRequest() != OK) {
WLOG(ERROR) << "Couldn't validate the transfer request "
<< transferRequest_.getLogSafeString();
return transferRequest_;
}
checkAndUpdateBufferSize();
backlog_ = options_.backlog;
if (getTransferId().empty()) {
setTransferId(WdtBase::generateTransferId());
}
setProtocolVersion(transferRequest_.protocolVersion);
setDir(transferRequest_.directory);
auto numThreads = transferRequest_.ports.size();
// This creates the destination directory (which is needed for transferLogMgr)
fileCreator_.reset(new FileCreator(destDir_, numThreads, transferLogManager_,
options_.skip_writes));
// Make sure we can get the lock on the transfer log manager early
// so if we can't we don't generate a valid but useless url and end up
// starting a sender doomed to fail
if (options_.enable_download_resumption) {
WDT_CHECK(!options_.skip_writes)
<< "Can not skip transfers with download resumption turned on";
if (options_.resume_using_dir_tree) {
WDT_CHECK(!options_.shouldPreallocateFiles())
<< "Can not resume using directory tree if preallocation is enabled";
}
ErrorCode errCode = transferLogManager_.openLog();
if (errCode != OK) {
WLOG(ERROR) << "Failed to open transfer log " << errorCodeToStr(errCode);
transferRequest_.errorCode = errCode;
return transferRequest_;
}
ErrorCode code = transferLogManager_.parseAndMatch(
recoveryId_, getTransferConfig(), fileChunksInfo_);
if (code == OK && options_.resume_using_dir_tree) {
WDT_CHECK(fileChunksInfo_.empty());
traverseDestinationDir(fileChunksInfo_);
}
}
EncryptionType encryptionType = parseEncryptionType(options_.encryption_type);
// is encryption enabled?
bool encrypt = (encryptionType != ENC_NONE &&
protocolVersion_ >= Protocol::ENCRYPTION_V1_VERSION);
if (encrypt) {
WLOG(INFO) << encryptionTypeToStr(encryptionType)
<< " encryption is enabled for this transfer ";
if (!transferRequest_.encryptionData.isSet()) {
WLOG(INFO) << "Receiver generating encryption key for type "
<< encryptionTypeToStr(encryptionType);
transferRequest_.encryptionData =
EncryptionParams::generateEncryptionParams(encryptionType);
}
if (!transferRequest_.encryptionData.isSet()) {
WLOG(ERROR) << "Unable to generate encryption key for type "
<< encryptionTypeToStr(encryptionType);
transferRequest_.errorCode = ENCRYPTION_ERROR;
return transferRequest_;
}
} else {
if (encryptionType != ENC_NONE) {
WLOG(WARNING) << "Encryption is enabled, but protocol version is "
<< protocolVersion_
<< ", minimum version required for encryption is "
<< Protocol::ENCRYPTION_V1_VERSION;
}
transferRequest_.encryptionData.erase();
}
threadsController_ = new ThreadsController(numThreads);
threadsController_->setNumFunnels(ReceiverThread::NUM_FUNNELS);
threadsController_->setNumBarriers(ReceiverThread::NUM_BARRIERS);
threadsController_->setNumConditions(ReceiverThread::NUM_CONDITIONS);
// TODO: take transferRequest directly !
receiverThreads_ = threadsController_->makeThreads<Receiver, ReceiverThread>(
this, transferRequest_.ports.size(), transferRequest_.ports);
size_t numSuccessfulInitThreads = 0;
for (auto &receiverThread : receiverThreads_) {
ErrorCode code = receiverThread->init();
if (code == OK) {
++numSuccessfulInitThreads;
}
}
WLOG(INFO) << "Registered " << numSuccessfulInitThreads
<< " successful sockets";
ErrorCode code = OK;
const size_t targetSize = transferRequest_.ports.size();
// TODO: replace with getNumPorts/thread
if (numSuccessfulInitThreads != targetSize) {
code = FEWER_PORTS;
if (numSuccessfulInitThreads == 0) {
code = ERROR;
}
}
transferRequest_.protocolVersion = protocolVersion_;
transferRequest_.ports.clear();
for (const auto &receiverThread : receiverThreads_) {
transferRequest_.ports.push_back(receiverThread->getPort());
}
if (transferRequest_.hostName.empty()) {
char hostName[1024];
int ret = gethostname(hostName, sizeof(hostName));
if (ret == 0) {
transferRequest_.hostName.assign(hostName);
} else {
PLOG(ERROR) << "Couldn't find the host name";
code = ERROR;
}
}
transferRequest_.directory = getDir();
transferRequest_.errorCode = code;
return transferRequest_;
}
// TODO : improve, look in the file more
IdentifiedFileType Identify_File(std::string &filename)
{
if (filename.size() == 0) {
ERROR_LOG(LOADER, "invalid filename %s", filename.c_str());
return FILETYPE_ERROR;
}
FileInfo info;
if (!getFileInfo(filename.c_str(), &info)) {
return FILETYPE_ERROR;
}
std::string extension = filename.size() >= 5 ? filename.substr(filename.size() - 4) : "";
if (!strcasecmp(extension.c_str(),".iso"))
{
// may be a psx iso, they have 2352 byte sectors. You never know what some people try to open
if ((info.size % 2352) == 0)
{
FILE *f = File::OpenCFile(filename.c_str(), "rb");
if (!f) {
// File does not exists
return FILETYPE_ERROR;
}
unsigned char sync[12];
fread(sync,1,12,f);
fclose(f);
// each sector in a mode2 image starts with these 12 bytes
if (memcmp(sync,"\x00\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\x00",12) == 0)
{
return FILETYPE_ISO_MODE2;
}
// maybe it also just happened to have that size,
}
return FILETYPE_PSP_ISO;
}
else if (!strcasecmp(extension.c_str(),".cso"))
{
return FILETYPE_PSP_ISO;
}
// First, check if it's a directory with an EBOOT.PBP in it.
if (info.isDirectory) {
if (filename.size() > 4) {
FileInfo ebootInfo;
// Check for existence of EBOOT.PBP, as required for "Directory games".
if (getFileInfo((filename + "/EBOOT.PBP").c_str(), &ebootInfo)) {
if (ebootInfo.exists) {
return FILETYPE_PSP_PBP_DIRECTORY;
}
}
// check if it's a disc directory
if (getFileInfo((filename + "/PSP_GAME").c_str(), &ebootInfo)) {
if (ebootInfo.exists) {
return FILETYPE_PSP_DISC_DIRECTORY;
}
}
}
return FILETYPE_NORMAL_DIRECTORY;
}
FILE *f = File::OpenCFile(filename.c_str(), "rb");
if (!f) {
// File does not exists
return FILETYPE_ERROR;
}
u32_le id;
size_t readSize = fread(&id, 4, 1, f);
if (readSize != 1) {
fclose(f);
return FILETYPE_ERROR;
}
u32 psar_offset = 0, psar_id = 0;
u32 _id = id;
switch (_id) {
case 'PBP\x00':
fseek(f, 0x24, SEEK_SET);
fread(&psar_offset, 4, 1, f);
fseek(f, psar_offset, SEEK_SET);
fread(&psar_id, 4, 1, f);
break;
case '!raR':
return FILETYPE_ARCHIVE_RAR;
case '\x04\x03KP':
case '\x06\x05KP':
case '\x08\x07KP':
return FILETYPE_ARCHIVE_ZIP;
}
fclose(f);
if (id == 'FLE\x7F') {
// There are a few elfs misnamed as pbp (like Trig Wars), accept that.
if (!strcasecmp(extension.c_str(), ".plf") || strstr(filename.c_str(),"BOOT.BIN") ||
!strcasecmp(extension.c_str(), ".elf") || !strcasecmp(extension.c_str(), ".prx") ||
!strcasecmp(extension.c_str(), ".pbp")) {
return FILETYPE_PSP_ELF;
}
return FILETYPE_UNKNOWN_ELF;
}
else if (id == 'PBP\x00') {
// Do this PS1 eboot check FIRST before checking other eboot types.
// It seems like some are malformed and slip through the PSAR check below.
PBPReader pbp(filename.c_str());
if (pbp.IsValid()) {
if (!pbp.IsELF()) {
size_t sfoSize;
u8 *sfoData = pbp.GetSubFile(PBP_PARAM_SFO, &sfoSize);
{
recursive_mutex _lock;
lock_guard lock(_lock);
ParamSFOData paramSFO;
paramSFO.ReadSFO(sfoData, sfoSize);
// PS1 Eboots are supposed to use "ME" as their PARAM SFO category.
// If they don't, and they're still malformed (e.g. PSISOIMG0000 isn't found), there's nothing we can do.
if (paramSFO.GetValueString("CATEGORY") == "ME")
return FILETYPE_PSP_PS1_PBP;
}
delete[] sfoData;
}
}
if (psar_id == 'MUPN') {
return FILETYPE_PSP_ISO_NP;
}
// PS1 PSAR begins with "PSISOIMG0000"
if (psar_id == 'SISP') {
return FILETYPE_PSP_PS1_PBP;
}
// Let's check if we got pointed to a PBP within such a directory.
// If so we just move up and return the directory itself as the game.
std::string path = getDir(filename);
// If loading from memstick...
size_t pos = path.find("/PSP/GAME/");
if (pos != std::string::npos) {
filename = path;
return FILETYPE_PSP_PBP_DIRECTORY;
}
return FILETYPE_PSP_PBP;
}
else if (!strcasecmp(extension.c_str(),".pbp")) {
ERROR_LOG(LOADER, "A PBP with the wrong magic number?");
return FILETYPE_PSP_PBP;
} else if (!strcasecmp(extension.c_str(),".bin")) {
return FILETYPE_UNKNOWN_BIN;
} else if (!strcasecmp(extension.c_str(),".zip")) {
return FILETYPE_ARCHIVE_ZIP;
} else if (!strcasecmp(extension.c_str(),".rar")) {
return FILETYPE_ARCHIVE_RAR;
} else if (!strcasecmp(extension.c_str(),".r00")) {
return FILETYPE_ARCHIVE_RAR;
} else if (!strcasecmp(extension.c_str(),".r01")) {
return FILETYPE_ARCHIVE_RAR;
}
return FILETYPE_UNKNOWN;
}
// TODO : improve, look in the file more
EmuFileType Identify_File(std::string &filename)
{
if (filename.size() < 5) {
ERROR_LOG(LOADER, "invalid filename %s", filename.c_str());
return FILETYPE_ERROR;
}
std::string extension = filename.substr(filename.size() - 4);
if (!strcasecmp(extension.c_str(),".iso"))
{
return FILETYPE_PSP_ISO;
}
else if (!strcasecmp(extension.c_str(),".cso"))
{
return FILETYPE_PSP_ISO;
}
// First, check if it's a directory with an EBOOT.PBP in it.
FileInfo info;
if (!getFileInfo(filename.c_str(), &info)) {
return FILETYPE_ERROR;
}
if (info.isDirectory) {
FileInfo ebootInfo;
// Check for existence of EBOOT.PBP, as required for "Directory games".
if (getFileInfo((filename + "/EBOOT.PBP").c_str(), &ebootInfo)) {
if (ebootInfo.exists) {
return FILETYPE_PSP_PBP_DIRECTORY;
}
}
}
FILE *f = fopen(filename.c_str(), "rb");
if (!f) {
// File does not exists
return FILETYPE_ERROR;
}
u32 id;
size_t readSize = fread(&id, 4, 1, f);
if (readSize != 1) {
fclose(f);
return FILETYPE_ERROR;
}
u32 psar_offset = 0, psar_id = 0;
if (id == 'PBP\x00') {
fseek(f, 0x24, SEEK_SET);
fread(&psar_offset, 4, 1, f);
fseek(f, psar_offset, SEEK_SET);
fread(&psar_id, 4, 1, f);
}
fclose(f);
if (id == 'FLE\x7F')
{
if (!strcasecmp(extension.c_str(), ".plf") || strstr(filename.c_str(),"BOOT.BIN") ||
!strcasecmp(extension.c_str(), ".elf") || !strcasecmp(extension.c_str(), ".prx") )
{
return FILETYPE_PSP_ELF;
}
else
return FILETYPE_UNKNOWN_ELF;
}
else if (id == 'PBP\x00')
{
if (psar_id == 'MUPN') {
return FILETYPE_PSP_ISO_NP;
} else {
// Let's check if we got pointed to a PBP within such a directory.
// If so we just move up and return the directory itself as the game.
std::string path = getDir(filename);
// If loading from memstick...
size_t pos = path.find("/PSP/GAME/");
if (pos != std::string::npos) {
filename = path;
return FILETYPE_PSP_PBP_DIRECTORY;
}
}
}
else
{
if (!strcasecmp(extension.c_str(),".pbp"))
{
ERROR_LOG(LOADER, "A PBP with the wrong magic number?");
return FILETYPE_PSP_PBP;
}
else if (!strcasecmp(extension.c_str(),".bin"))
{
return FILETYPE_UNKNOWN_BIN;
}
}
return FILETYPE_UNKNOWN;
}
// __________________________________________________________________________________________________
void KikiBot::performAction ( KikiAction * action )
{
int actionId = action->getId();
float relTime = action->getRelativeTime();
float dltTime = action->getRelativeDelta();
switch (actionId)
{
case ACTION_SHOOT:
if (relTime == 0)
{
KikiBullet::shootFromBot (this);
}
case ACTION_NOOP:
return;
case ACTION_FORWARD:
left_tire_rot += dir_sgn * dltTime;
right_tire_rot += dir_sgn * dltTime;
current_position = position + relTime * getDir();
return;
case ACTION_JUMP:
current_position = position + cos(M_PI_2 - M_PI_2 * relTime) * getUp();
return;
case ACTION_JUMP_FORWARD:
left_tire_rot += cos(M_PI_2 - M_PI_2 * dltTime);
right_tire_rot += cos(M_PI_2 - M_PI_2 * dltTime);
current_position = position + (1.0 - cos(M_PI_2 * relTime)) * getDir()
+ cos(M_PI_2 - M_PI_2 * relTime) * getUp();
return;
case ACTION_FALL_FORWARD:
current_position = position + cos(M_PI_2 - M_PI_2 * relTime) * getDir()
+ (1.0 - cos(M_PI_2 * relTime)) * getDown();
return;
case ACTION_FALL:
if (direction != KVector())
{
KikiPushable::performAction (action);
return;
}
current_position = position + relTime * getDown();
return;
case ACTION_CLIMB_UP:
left_tire_rot += dir_sgn * dltTime/2;
right_tire_rot += dir_sgn * dltTime/2;
climb_orientation = KQuaternion::rotationAroundVector(dir_sgn * relTime * -90.0, KVector(1,0,0));
break;
case ACTION_CLIMB_DOWN:
left_tire_rot += dir_sgn * dltTime;
right_tire_rot += dir_sgn * dltTime;
if (relTime <= 0.2)
{
current_position = position + (relTime/0.2)/2 * getDir();
}
else if (relTime >= 0.8)
{
climb_orientation = KQuaternion::rotationAroundVector(dir_sgn * 90.0, KVector(1,0,0));
current_position = position + getDir() + (0.5+(relTime-0.8)/0.2/2) * getDown();
}
else
{
climb_orientation = KQuaternion::rotationAroundVector(dir_sgn * (relTime-0.2)/0.6 * 90.0, KVector(1,0,0));
KVector rotVec = (orientation * climb_orientation).rotate(KVector(0.0, 1.0, 0.0));
current_position = position + 0.5 * ((KVector)getDir() + (KVector)getDown() + rotVec);
}
break;
case ACTION_TURN_LEFT:
case ACTION_TURN_RIGHT:
if (move_action == NULL && relTime == 0.0) // if not performing move action and start of rotation
{
// update orientation now, so next move action will move in desired direction
if (actionId == ACTION_TURN_LEFT)
{
orientation *= KQuaternion::rotationAroundVector(90.0, KVector(0,1,0));
rest_orientation = KQuaternion::rotationAroundVector(-90.0, KVector(0,1,0));
}
else
{
orientation *= KQuaternion::rotationAroundVector(-90.0, KVector(0,1,0));
rest_orientation = KQuaternion::rotationAroundVector(90.0, KVector(0,1,0));
}
}
if (actionId == ACTION_TURN_LEFT)
{
left_tire_rot += -dltTime;
right_tire_rot += dltTime;
rotate_orientation = KQuaternion::rotationAroundVector(relTime * 90.0, KVector(0,1,0));
}
else
{
left_tire_rot += dltTime;
right_tire_rot += -dltTime;
rotate_orientation = KQuaternion::rotationAroundVector(relTime * -90.0, KVector(0,1,0));
}
break;
default:
KikiPushable::performAction (action);
return;
}
current_orientation = orientation * climb_orientation * rotate_orientation * rest_orientation;
}
// __________________________________________________________________________________________________
void KikiBot::moveBot ()
{
move_action = NULL;
KikiPos forwardPos = position + getDir();
if ((jump || jump_once) && // jump mode or jump activated while moving
dir_sgn == 1.0 && // and moving forward
Controller.world->isUnoccupiedPos (position + getUp())) // and above empty
{
if (Controller.world->isUnoccupiedPos (forwardPos + getUp()) &&
Controller.world->isUnoccupiedPos (forwardPos)) // forward and above forward also empty
{
move_action = getActionWithId (KikiBot::ACTION_JUMP_FORWARD);
}
else // no space to jump forward -> jump up
{
move_action = getActionWithId (KikiBot::ACTION_JUMP);
}
}
else if (Controller.world->isUnoccupiedPos (forwardPos)) // forward is empty
{
if (Controller.world->isUnoccupiedPos (forwardPos + getDown()))
{ // below forward also empty
move_action = getActionWithId (KikiBot::ACTION_CLIMB_DOWN);
}
else // forward down is solid
{
move_action = getActionWithId (KikiBot::ACTION_FORWARD);
}
}
else // forward is not empty
{
KikiAction * moveAction = getActionWithId (KikiBot::ACTION_FORWARD);
if (push && Controller.world->mayObjectPushToPos (this, forwardPos, moveAction->getDuration()))
{
moveAction->reset();
// player in push mode and pushing object is possible
if (Controller.world->isUnoccupiedPos(forwardPos + getDown())) // below forward is empty
{
move_action = getActionWithId (KikiBot::ACTION_CLIMB_DOWN);
}
else
{
move_action = moveAction;
}
}
else // just climb up
{
move_action = getActionWithId (KikiBot::ACTION_CLIMB_UP);
}
}
// reset the jump once flag (either we jumped or it's not possible to jump at current position)
jump_once = false;
if (move_action)
{
move_action->keepRest(); // try to make subsequent actions smooth
Controller.timer_event->addAction (move_action);
}
}
FilePath Project::getBuildOutputDir(OovStringRef const buildDirClass)
{
return getDir("out-", buildDirClass, getProjectDirectory());
}
// 地磁気センサからデータ取得
unsigned int getCompass()
{
return getDir(0);
}
int
psp_file_request(char *out, char *pszStartPath)
{
static int sel=0;
gp2xCtrlData c;
int last_time;
int tmp;
long color;
int top, rows=20, x, y, i, up=0;
char path[PSP_FMGR_MAX_PATH];
char oldDir[PSP_FMGR_MAX_NAME];
char buffer[PSP_FMGR_MAX_NAME];
char *p;
long new_pad;
long old_pad;
int file_selected;
int check_last;
int danzeff_mode;
int danzeff_key;
danzeff_key = 0;
danzeff_mode = 0;
memset(files, 0x00, sizeof(struct dirent) * PSP_FMGR_MAX_ENTRY);
memset(sortfiles, 0x00, sizeof(struct dirent *) * PSP_FMGR_MAX_ENTRY);
nfiles = 0;
strcpy(path, pszStartPath);
getDir(path);
last_time = 0;
old_pad = 0;
top = 0;
file_selected = 0;
if (sel >= nfiles) sel = 0;
check_last = 0;
for(;;)
{
x = 0; y = 15;
psp_display_screen_fmgr();
for(i=0; i<rows; i++){
if(top+i >= nfiles) break;
if(top+i == sel) color = PSP_MENU_SEL_COLOR;
else color = PSP_MENU_TEXT_COLOR;
strncpy(buffer, sortfiles[top+i]->d_name, 28);
string_fill_with_space(buffer, 28);
psp_sdl_back2_print(x, y, buffer, color);
y += 10;
}
if (danzeff_mode) {
danzeff_moveTo(0, -90);
danzeff_render();
}
psp_sdl_flip();
while (1) {
gp2xCtrlPeekBufferPositive(&c, 1);
c.Buttons &= PSP_ALL_BUTTON_MASK;
new_pad = c.Buttons;
if ((old_pad != new_pad) || ((c.TimeStamp - last_time) > GP2X_FMGR_MIN_TIME)) {
last_time = c.TimeStamp;
old_pad = new_pad;
break;
}
}
if (danzeff_mode) {
danzeff_key = danzeff_readInput(c);
if (danzeff_key > DANZEFF_START) {
if (danzeff_key > ' ') {
int new_sel = psp_file_find_first(danzeff_key, check_last);
check_last = 1;
if (new_sel >= 0) {
sel = new_sel;
goto lab_end;
}
}
} else
if ((danzeff_key == DANZEFF_START ) ||
(danzeff_key == DANZEFF_SELECT))
{
danzeff_mode = 0;
old_pad = new_pad = 0;
psp_kbd_wait_no_button();
}
if (danzeff_key == ' ') {
new_pad |= GP2X_CTRL_CROSS;
} else
if (danzeff_key >= -1) {
continue;
}
}
if (new_pad & GP2X_CTRL_START) {
danzeff_mode = 1;
} else
if ((new_pad & GP2X_CTRL_CROSS) || (new_pad & GP2X_CTRL_CIRCLE)) {
if (sortfiles[sel]->d_type == DT_DIR) {
check_last = 0;
if(!strcmp(sortfiles[sel]->d_name,"..")){
up=1;
} else {
strcat(path,sortfiles[sel]->d_name);
getDir(path);
sel=0;
}
}else{
strcpy(out, path);
strcat(out, sortfiles[sel]->d_name);
strcpy(pszStartPath,path);
file_selected = 1;
break;
}
} else if(new_pad & GP2X_CTRL_TRIANGLE){
check_last = 0;
up=1;
} else if((new_pad & GP2X_CTRL_SQUARE) || (new_pad & GP2X_CTRL_SELECT)) {
/* Cancel */
file_selected = 0;
break;
} else if(new_pad & GP2X_CTRL_UP){
sel--;
check_last = 0;
} else if(new_pad & GP2X_CTRL_DOWN){
sel++;
check_last = 0;
} else if(new_pad == GP2X_CTRL_LEFT){
sel-=10;
check_last = 0;
} else if(new_pad == GP2X_CTRL_RIGHT){
sel+=10;
check_last = 0;
} else if(new_pad & GP2X_CTRL_RTRIGGER){
if (sortfiles[sel]->d_type != DT_DIR) {
strcpy(out, path);
strcat(out, sortfiles[sel]->d_name);
strcpy(pszStartPath,path);
if (psp_fmgr_ask_confirm()) {
for (tmp = sel; tmp < (nfiles - 1); tmp++) {
sortfiles[tmp] = sortfiles[tmp + 1];
}
nfiles--;
remove(out);
}
check_last = 0;
}
}
if(up) {
check_last = 0;
if(strcmp(path,"./")){
p=strrchr(path,'/');
*p=0;
p=strrchr(path,'/');
p++;
strcpy(oldDir,p);
strcat(oldDir,"/");
*p=0;
getDir(path);
sel=0;
for(i=0; i<nfiles; i++) {
if(!strcmp(oldDir, sortfiles[i]->d_name)) {
sel=i;
top=sel-3;
break;
}
}
}
up=0;
}
lab_end:
if(top > nfiles-rows) top=nfiles-rows;
if(top < 0) top=0;
if(sel >= nfiles) sel=nfiles-1;
if(sel < 0) sel=0;
if(sel >= top+rows) top=sel-rows+1;
if(sel < top) top=sel;
}
return file_selected;
}
void FindElevatorButtons::init()
{
bVerbose = true;
emg = new EMGridFit(bVerbose);
hmml = new HMMLabel();
find_button_pkg_path = ros::getPackagePath("find_elevator_button");
ros::Node *node = ros::Node::instance();
node->subscribe("elevator_buttons_request", button_request,
&FindElevatorButtons::findButtons, this, 10);
node->advertise<stair_msgs::Button>("single_button", 10);
node->advertise<stair_msgs::AllButtons>("all_buttons", 10);
// Find buttons the normal way
if (ONLINE_PROCESS == 1) {
findButtons();
// Use previously svl classification and loop over all images
} else {
string detectionsPath = find_button_pkg_path + "/Data/classify/final_detections";
string srcImageDir;
vector<string> srcImageFiles;
if (USE_TRAINING_DATA == 1)
srcImageDir= "/home/stair/ellen/stair/perception/find_elevator_button/Data/images/unwarped_train_low/";
else
srcImageDir= "/home/stair/ellen/stair/perception/find_elevator_button/Data/images/unwarped_test_low/";
getDir(srcImageDir, srcImageFiles);
size_t numImages = srcImageFiles.size();
srcImageFiles.clear();
srcImageFiles.resize(2);
srcImageFiles[0] = "DSCN5058_unwarped_low.jpg";
srcImageFiles[1] = "DSCN5099_unwarped_low.jpg";
// Loop over all images in sourceImageDir, getting imageName, imageFile and detections
for (size_t k=0; k<2; k++) //numImages; k++)
{
svlDetections.clear();
imageName = srcImageFiles[k].substr(0,srcImageFiles[k].length()-4);
imageFile = srcImageDir + srcImageFiles[k];
// imageName = "DSCN5099_unwarped_low";
// imageFile = srcImageDir + imageName + ".jpg";
if (USE_TRAINING_DATA == 1)
detectionsFile = "train_" + imageName + "_train-pos-1700";
else
detectionsFile = "test_" + imageName + "_train-pos-1700";
// Find buttons if detection file exists
cerr << "Reading in detections file: Path: " << detectionsPath << " File: " << detectionsFile << endl;
if (!readCacheObject2dFrame(detectionsPath.c_str(),detectionsFile.c_str(),svlDetections)) {
cout << "Error reading svl detections file." << endl;
} else {
cout << "Number of detections " << svlDetections.size() << endl;
findButtons();
}
}
}
// For debugging
// labelHMM();
}
/*
* Read size bytes from file into buf starting from offset
*
*/
static int cs1550_read(const char *path, char *buf, size_t size, off_t offset,
struct fuse_file_info *fi)
{
(void) fi;
int ret =0;
char dir[MAX_FILENAME + 1];
char fileName[MAX_FILENAME + 1];
char ext[MAX_EXTENSION + 1];
getPath(path, dir, fileName, ext);
int pathType = getPathType(path, dir, fileName, ext);
int fileSize = getFileSize(dir, fileName, ext, pathType);
if (pathType < 2) {
return -EISDIR;
}
else if (dirExists(dir) == 1 && fileSize != -1 && size > 0){
cs1550_directory_entry parentDir;
getDir(&parentDir, dir);
int i =0;
for (i = 0; i < parentDir.nFiles; i++) {
if ((pathType==2 && strcmp(parentDir.files[i].fname, fileName)==0) || (pathType==3 && strcmp(parentDir.files[i].fname, fileName) == 0 && strcmp(parentDir.files[i].fext, ext) == 0)) {
if (offset <= parentDir.files[i].fsize) {
long startBlock = parentDir.files[i].nStartBlock;
int blockNum = offset / BLOCK_SIZE;
long seek = startBlock;
long bStart = 0;
FILE *f = fopen(".disk", "rb+");
int j =0;
for (j = 0; j <= blockNum; j++) {
bStart = seek;
fseek(f, seek, SEEK_SET);
cs1550_disk_block fileBlock;
fread(&fileBlock, BLOCK_SIZE, 1, f);
seek = fileBlock.magic_number;
}
rewind(f);
int off = (int)offset - (blockNum * BLOCK_SIZE);
int count = off;
int index = 0;
seek = bStart;
while(seek != 0) {
fseek(f, seek, SEEK_SET);
cs1550_disk_block curBlock;
fread(&curBlock, BLOCK_SIZE, 1, f);
if (count < MAX_DATA_IN_BLOCK) {
buf[index] = (char)curBlock.data[count];
count++;
index++;
}
else {
seek = curBlock.magic_number;
count = 0;
}
}
fclose(f);
ret = size;
}
}
}
}
return ret;
}
/*
* Write size bytes from buf into file starting from offset
*
*/
static int cs1550_write(const char *path, const char *buf, size_t size,
off_t offset, struct fuse_file_info *fi)
{
int ret = 0;
(void) fi;
char dir[MAX_FILENAME + 1];
char fileName[MAX_FILENAME + 1];
char ext[MAX_EXTENSION + 1];
getPath(path, dir, fileName, ext);
int pathType = getPathType(path, dir, fileName, ext);
int fileSize = getFileSize(dir, fileName, ext, pathType);
if (dirExists(dir) == 1 && pathType >= 2 && fileSize != -1 && size > 0) {
cs1550_directory_entry parentDir;
getDir(&parentDir, dir);
int i =0;
for (i = 0; i < parentDir.nFiles; i++) {
if ((pathType==2 && strcmp(parentDir.files[i].fname, fileName)==0) || (pathType==3 && strcmp(parentDir.files[i].fname, fileName) == 0 && strcmp(parentDir.files[i].fext, ext) == 0)) {
if (offset > parentDir.files[i].fsize) {
return -EFBIG;
}
else {
long startBlock = parentDir.files[i].nStartBlock;
int blockNum = offset / BLOCK_SIZE;
long seek = startBlock;
long bStart = 0;
FILE *f = fopen(".disk", "rb+");
int j = 0;
for (j = 0; j <= blockNum; j++) {
bStart = seek;
fseek(f, seek, SEEK_SET);
cs1550_disk_block fileBlock;
fread(&fileBlock, BLOCK_SIZE, 1, f);
seek = fileBlock.magic_number;
}
rewind(f);
int off = (int)offset - (blockNum * BLOCK_SIZE);
int index;
int count = off;
seek = bStart;
fseek(f, seek, SEEK_SET);
cs1550_disk_block curBlock;
fread(&curBlock, BLOCK_SIZE, 1, f);
for (index = 0; index < strlen(buf); index++) {
if (count < MAX_DATA_IN_BLOCK) {
curBlock.data[count] = (char)buf[index];
count++;
}
else {
count = 0;
if (curBlock.magic_number != 0) {
writeBlock(&curBlock, seek);
seek = curBlock.magic_number;
fseek(f, seek, SEEK_SET);
fread(&curBlock, BLOCK_SIZE, 1, f);
}
else {
long cSeek = seek;
int nextBlock = getNextBlock();
seek = nextBlock * BLOCK_SIZE;
curBlock.magic_number = seek;
writeBlock(&curBlock, cSeek);
fseek(f, seek, SEEK_SET);
fread(&curBlock, BLOCK_SIZE, 1, f);
updateBitmap(nextBlock, 1);
}
}
if (index == strlen(buf)-1) {
writeBlock(&curBlock, seek);
count = 0;
}
}
fclose(f);
int old = parentDir.files[i].fsize;
int newSize = (size - (old - offset)) + (old - (old - offset));
parentDir.files[i].fsize = newSize;
updateDir(&parentDir, dir);
ret = newSize;
}
}
}
}
return ret;
}
char * totalPath(){
char pathname[1024] = { 0 };
char temp[50][100] = { { 0 } };
char name0[124] ={ 0 };
MINODE *mip = running->cwd;
INODE * ip = 0;
// Obtain the name of the mip
if(strcmp(mip->name, "")==0){
if(mip->ino> 2){
// Assign a name to the bitch.
DIR * dp = (DIR*)getDir(&(mip->INODE), mip->ino );
if(dp != 0){
strcpy(name0, dp->name);
}
}
}
else {
strcpy(name0, running->cwd->name);
}
strcpy(temp[0],name0);
printf("The temp[0] = %s \n", temp[0]);
// dp = Directory of minode
// ip = parent node of mip
int i=1;
mip = (MINODE*)getParentMinode(mip, mip->ino);
while(mip->ino != 2){
//strcpy(temp[i], "/");
strcpy(temp[i], mip->name);
//printf("temp[i]=%s\n", temp[i]);
mip = (MINODE*)getParentMinode(mip, mip->ino);
i++;
iput(mip);
}
if(mip != NULL){
iput(mip);
}
// Add Slashes to total path
strcpy(pathname, "/");
if(strcmp("/", temp[0]) != 0){
int j = 0;
for (j = i; j > 0; j--){
strcat(pathname, temp[j-1]);
if( j-1 > 0){
strcat(pathname, "/");
}
}
}
printf(" * * * * The Pwd: %s * * * * * * *\n", pathname);
int x =0;
for(x =0; x< 1024; x++){
completePath[x] = 0;
}
// Re-write the global variable:
strcpy(completePath, pathname);
return pathname;
}
int tickProcessBullet(gnode * grid,bullet * b){
if (b->detonate==0){
bullet_type * type=typesBulletGet(b->type);
if (type==0){
b->detonate=1;
return 0;
}
//vec dir={0,0};
// printDebug("!!%g %g\n",b->position.x,b->position.y);
//float length=getDir(&b->position,&b->destination,&dir);
float delta=1;
if (type->move_type!=SHOT){
b->position.x+=b->direction.x*type->speed;
b->position.y+=b->direction.y*type->speed;
delta=type->speed;
if (delta<0.2)
delta=0.2;
}else{
b->position.x=b->destination.x;
b->position.y=b->destination.y;
}
setMask(b,BULLET_POSITION);
if (eqInD(b->position.x,b->destination.x,delta)&&
eqInD(b->position.y,b->destination.y,delta)){
int i,j,k;
int multiple=0;
int x=(int)b->position.x;
int y=(int)b->position.y;
int xid,yid;
int _id=to2d(x,y);
if ( x<0 || y<0 || x>=config.gridsize || y>=config.gridsize)
goto out;
//tower search
{
tower * tmp;
if (_id<sqr(config.gridsize))
if ((tmp=grid[_id].tower)>0)
if(config.players[tmp->owner].group!=b->group){
damageTower(tmp,b);
multiple++;
}
}
if (type->attack_type==SINGLE && multiple>0)
goto out;
//npc search
{
npc* tmp;
if (_id<sqr(config.gridsize))
for(j=0;j<MAX_GROUPS;j++)
for(tmp=grid[_id].npcs[j];
tmp!=0;tmp=tmp->next)
if (config.players[tmp->owner].group!=b->group)
if (eqInD(tmp->position.x,b->position.x,delta)&&
eqInD(tmp->position.y,b->position.y,delta))
//attack npc near the destination
{
damageNpc(tmp,b);
multiple++;
if (type->attack_type==SINGLE)
goto out;
if (type->attack_type==MULTIPLE &&
multiple>type->area)
goto out;
}
//if npc not in node, see nodes near
i=0;
for(j=0;j<config.area_size[i];j++)
if (((xid=x+config.area_array[i][j].x)>=0 && x+config.area_array[i][j].x<config.gridsize) &&
((yid=y+config.area_array[i][j].y)>=0 && y+config.area_array[i][j].y<config.gridsize))
for(k=0;k<MAX_GROUPS;k++)
for(tmp=grid[to2d(xid,yid)].npcs[k];
tmp!=0;tmp=tmp->next)
if (config.players[tmp->owner].group!=b->group)
if (eqInD(tmp->position.x,b->position.x,delta)&&
eqInD(tmp->position.y,b->position.y,delta))
//attack first npc in gnode, need to correct
{
damageNpc(tmp,b);
multiple++;
if (type->attack_type==SINGLE)
goto out;
if (type->attack_type==MULTIPLE &&
multiple>type->area)
goto out;
}
}
//add area damage to Npc
//add area damage to towers
if (type->attack_type==AREA ||
type->attack_type==AREA_FF){
//add area gamage
npc* tmp;
for (i=0;i<type->area;i++)
for(j=0;j<config.area_size[i];j++)
if (((xid=x+config.area_array[i][j].x)>=0 && x+config.area_array[i][j].x<config.gridsize) &&
((yid=y+config.area_array[i][j].y)>=0 && y+config.area_array[i][j].y<config.gridsize)){
//tower
if (grid[to2d(xid,yid)].tower!=0)
if (canSee(grid,&b->position,&(vec){xid+0.5,yid+0.5})>0)
if(type->attack_type==AREA?
config.players[grid[to2d(xid,yid)].tower->owner].group!=b->group
:1){
damageTower(grid[to2d(xid,yid)].tower,b);
}
//npc
for (k=0;k<MAX_GROUPS;k++)
if (type->attack_type==AREA?k!=b->group:1)
for(tmp=grid[to2d(xid,yid)].npcs[k];
tmp!=0;tmp=tmp->next)
if (canSee(grid,&b->position,&tmp->position)>0)
damageNpc(tmp,b);
}
}
out:
b->detonate++;
setMask(b,BULLET_DETONATE);
}else{
if (b->ntarget!=0){
if (glength(&b->ntarget->position,&b->source)>b->max_dist){
b->ntarget=0;
}else{
//TODO: add folow attr
memcpy(&b->destination,&b->ntarget->position,sizeof(b->destination));
getDir(&b->position,&b->destination,&b->direction);
}
}
}
}
return 0;
}
// TODO : improve, look in the file more
IdentifiedFileType Identify_File(std::string &filename)
{
if (filename.size() == 0) {
ERROR_LOG(LOADER, "invalid filename %s", filename.c_str());
return FILETYPE_ERROR;
}
FileInfo info;
if (!getFileInfo(filename.c_str(), &info)) {
return FILETYPE_ERROR;
}
std::string extension = filename.size() >= 5 ? filename.substr(filename.size() - 4) : "";
if (!strcasecmp(extension.c_str(),".iso"))
{
// may be a psx iso, they have 2352 byte sectors. You never know what some people try to open
if ((info.size % 2352) == 0)
{
FILE *f = File::OpenCFile(filename.c_str(), "rb");
if (!f) {
// File does not exists
return FILETYPE_ERROR;
}
unsigned char sync[12];
fread(sync,1,12,f);
fclose(f);
// each sector in a mode2 image starts with these 12 bytes
if (memcmp(sync,"\x00\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\xFF\x00",12) == 0)
{
return FILETYPE_ISO_MODE2;
}
// maybe it also just happened to have that size,
}
return FILETYPE_PSP_ISO;
}
else if (!strcasecmp(extension.c_str(),".cso"))
{
return FILETYPE_PSP_ISO;
}
// First, check if it's a directory with an EBOOT.PBP in it.
if (info.isDirectory) {
if (filename.size() > 4) {
FileInfo ebootInfo;
// Check for existence of EBOOT.PBP, as required for "Directory games".
if (getFileInfo((filename + "/EBOOT.PBP").c_str(), &ebootInfo)) {
if (ebootInfo.exists) {
return FILETYPE_PSP_PBP_DIRECTORY;
}
}
// check if it's a disc directory
if (getFileInfo((filename + "/PSP_GAME").c_str(), &ebootInfo)) {
if (ebootInfo.exists) {
return FILETYPE_PSP_DISC_DIRECTORY;
}
}
}
return FILETYPE_NORMAL_DIRECTORY;
}
FILE *f = File::OpenCFile(filename.c_str(), "rb");
if (!f) {
// File does not exists
return FILETYPE_ERROR;
}
u32_le id;
size_t readSize = fread(&id, 4, 1, f);
if (readSize != 1) {
fclose(f);
return FILETYPE_ERROR;
}
u32 psar_offset = 0, psar_id = 0;
switch (id) {
case 'PBP\x00':
fseek(f, 0x24, SEEK_SET);
fread(&psar_offset, 4, 1, f);
fseek(f, psar_offset, SEEK_SET);
fread(&psar_id, 4, 1, f);
break;
case '!raR':
return FILETYPE_ARCHIVE_RAR;
case '\x04\x03KP':
case '\x06\x05KP':
case '\x08\x07KP':
return FILETYPE_ARCHIVE_ZIP;
}
fclose(f);
if (id == 'FLE\x7F') {
// There are a few elfs misnamed as pbp (like Trig Wars), accept that.
if (!strcasecmp(extension.c_str(), ".plf") || strstr(filename.c_str(),"BOOT.BIN") ||
!strcasecmp(extension.c_str(), ".elf") || !strcasecmp(extension.c_str(), ".prx") ||
!strcasecmp(extension.c_str(), ".pbp")) {
return FILETYPE_PSP_ELF;
}
return FILETYPE_UNKNOWN_ELF;
}
else if (id == 'PBP\x00') {
if (psar_id == 'MUPN') {
return FILETYPE_PSP_ISO_NP;
}
// PS1 PSAR begins with "PSISOIMG0000"
if (psar_id == 'SISP') {
return FILETYPE_PSP_PS1_PBP;
}
// Let's check if we got pointed to a PBP within such a directory.
// If so we just move up and return the directory itself as the game.
std::string path = getDir(filename);
// If loading from memstick...
size_t pos = path.find("/PSP/GAME/");
if (pos != std::string::npos) {
filename = path;
return FILETYPE_PSP_PBP_DIRECTORY;
}
return FILETYPE_PSP_PBP;
}
else if (!strcasecmp(extension.c_str(),".pbp")) {
ERROR_LOG(LOADER, "A PBP with the wrong magic number?");
return FILETYPE_PSP_PBP;
} else if (!strcasecmp(extension.c_str(),".bin")) {
return FILETYPE_UNKNOWN_BIN;
} else if (!strcasecmp(extension.c_str(),".zip")) {
return FILETYPE_ARCHIVE_ZIP;
} else if (!strcasecmp(extension.c_str(),".rar")) {
return FILETYPE_ARCHIVE_RAR;
} else if (!strcasecmp(extension.c_str(),".r00")) {
return FILETYPE_ARCHIVE_RAR;
} else if (!strcasecmp(extension.c_str(),".r01")) {
return FILETYPE_ARCHIVE_RAR;
}
return FILETYPE_UNKNOWN;
}
void Ship::adjustSpd(int spd){
setSpd(sf::Vector2f(spd * cos(getDir()*M_PI/180), spd * sin(getDir()*M_PI/180)));
}
/*
* When we come here, we've already matched either a location block or an extension
*/
static int run(request_rec *r)
{
EjsDirConfig *dir;
EjsServerConfig *server;
cchar *ext;
MaRequest *req;
MaResponse *resp;
MaConn *conn;
MaAlias *alias;
MaLocation *location;
EjsWeb *web;
cchar *sep, *prefix;
char *urlBase, *dir, *app, *url, *cp;
int flags, locFlags;
if (!r->handler || strcasecmp(r->handler, "ejs") != 0) {
return DECLINED;
}
dir = getDir(r)
server = getServer(r->XXX);
// EjsAlias should probably be creating a directory block. These flags should probably be in a directory
if (loc->flags & (MA_LOC_APP | MA_LOC_APP_DIR)) {
/*
* Send non-ejs content under web to another handler, typically the file handler.
*/
if (strncmp(&req->url[loc->prefixLen], "web/", 4) == 0) {
if (!(ext && strcmp(ext, "ejs") == 0)) {
return DECLINED;
}
} else {
if (ext && strcmp(ext, "ejs") == 0) {
maFormatBody(conn, "Bad Request", "Can't serve *.ejs files outside web directory");
maFailRequest(conn, MPR_HTTP_CODE_BAD_REQUEST, "Can't server *.ejs files outside web directory");
return HTTP_XXX;
}
}
}
flags = 0;
url = 0;
locFlags = location->flags;
if (locFlags & MA_LOC_APP) {
app = mprStrTrim(mprStrdup(q, prefix), "/");
url = &req->url[alias->prefixLen];
dir = mprStrdup(resp, alias->filename);
if (*url != '/') {
url--;
}
urlBase = mprStrdup(resp, prefix);
} else if (locFlags & MA_LOC_APP_DIR) {
url = &req->url[alias->prefixLen];
app = mprStrdup(resp, url);
if ((cp = strchr(app, '/')) != 0) {
url = mprStrdup(resp, cp);
*cp = '\0';
}
sep = prefix[strlen(prefix) - 1] == '/' ? "" : "/";
dir = mprStrcat(resp, &dir, alias->filename, sep, app, NULL);
urlBase = mprStrcat(resp, prefix, sep, app, NULL);
} else {
app = 0;
dir = mprStrdup(resp, alias->filename);
url = &req->url[alias->prefixLen];
flags |= EJS_WEB_FLAG_SOLO;
if (*url != '/') {
url--;
}
urlBase = mprStrdup(resp, prefix);
}
mprStrTrim(urlBase, "/");
mprStrTrim(dir, "/");
if (location->flags & MA_LOC_BROWSER) {
flags |= EJS_WEB_FLAG_BROWSER_ERRORS;
}
if (location->flags & MA_LOC_AUTO_SESSION) {
flags |= EJS_WEB_FLAG_SESSION;
}
/*
* Var Stand-Alone App AppDir
* app 0 carmen carmen
* dir /Users/mob/.... /Users/mob/hg/carmen /Users/mob/hg/carmen
* urlBase /xg/carmen /carmen
* url stock stock
*/
web = ejsCreateWebRequest(req, q->stage->stageData, conn, app, dir, urlBase, url, req->cookie, flags);
if (web == 0) {
maFailRequest(conn, MPR_HTTP_CODE_INTERNAL_SERVER_ERROR, "Can't create Ejs web object for %s", req->url);
return;
}
q->queueData = web;
maSetHeader(conn, 0, "Last-Modified", req->host->currentDate);
maDontCacheResponse(conn);
if (r->method_number != M_GET) {
return HTTP_METHOD_NOT_ALLOWED;
}
if (ejsProcessWebRequest((EjsWeb*) r, &msg) < 0) {
if (web->flags & EJS_WEB_FLAG_BROWSER_ERRORS) {
maFormatBody(conn, "Request Failed", "%s", msg);
}
maFailRequest(conn, MPR_HTTP_CODE_BAD_GATEWAY, msg);
mprFree(msg);
}
ap_set_content_type(r, "text/html");
ap_rputs("<html><title>Hello World!</title><body><p>Hello World</p></body></html>\r\n", r);
#if 0
if ((err = set_content_length(r, r->finfo.st_stize)) || (err = set_last_modified(r, r->finfo.st_mtime)))
return err;
if (r->finof.st_mode == 0)
return NOT_FOUND;
fopen(r->filename, "r");
if (!r->main) {
/* Not internal redirect */
apr_table_set(r->headers_out, "X-ejs", "Under construction");
}
register_timeout("send", r);
send_http_header(r);
if (!r->header_only)
send_fd(f, r);
pfclose(r->pool, f);
#endif
return OK;
}
void Ship::updateGuideline(std::vector<Planet*>& planets, std::vector<Wormhole*>& wormholes,
std::vector<Asteroid*>& asteroids) {
float theta = getDir() * M_PI / 180;
_guideline->setLine(getPosition(), sf::Vector2f(cos(theta),sin(theta)), 0, planets, wormholes, asteroids, getRadius());
}
// returns angle (in radians) between direction vector
// and OY axis (clock-wise).
inline double getDirOYangle(void) const
{
return getDir().getOYangle();
};
FilePath Project::getIntermediateDir(OovStringRef const buildDirClass)
{
return getDir("bld-", buildDirClass, getProjectDirectory());
}
void FileLoader::loadNVM2(const char *fileName, MVS &mvs) {
vector<Camera> &cameras = mvs.cameras;
map<int, Patch> &patches = mvs.patches;
// reset container
cameras.clear();
patches.clear();
// open nvm file
ifstream file(fileName, ifstream::in);
if ( !file.is_open() ) {
printf("Can't open NVM2 file: %s\n", fileName);
return;
}
// get file path
char filePath [MAX_FILE_NAME_LENGTH];
getDir(fileName, filePath);
char *strip = NULL;
char strbuf[STRING_BUFFER_LENGTH];
int num;
bool loadCamera = false;
bool loadPatch = false;
while ( !file.eof() ) {
file.getline(strbuf, STRING_BUFFER_LENGTH);
strip = strtok(strbuf, DELIMITER);
if (strip == NULL) continue; // skip blank line
// start load camera
if (strcmp(strip, "NVM_V3") == 0) {
loadCamera = true;
continue;
}
if (loadCamera) {
// get number of cameras
strip = strtok(strbuf, DELIMITER);
num = atoi(strip);
cameras.reserve(num);
for (int i = 0; i < num; i++) {
printf("\rloading cameras: %d / %d", i+1, num);
cameras.push_back( loadNvm2Camera(file, filePath) );
}
printf("\n");
loadCamera = false;
loadPatch = true;
continue;
}
if (loadPatch) {
// get number of points
strip = strtok(strbuf, DELIMITER);
num = atoi(strip);
for (int i = 0; i < num; i++) {
printf("\rloading patches: %d / %d", i+1, num);
Patch p = loadNvmPatch(file, mvs);
patches.insert( pair<int, Patch>(p.getId(), p) );
}
printf("\n");
loadPatch = false;
break;
}
}
file.close();
}
static void preprocess(char *srcFileName){
char *buf;
//fprintf(stderr, "pp \"%s\"\n", srcFileName);
//init srcFile
if(findProcessedFileName(srcFileName)){
fprintf(output, "#include \"%s\"\n", srcFileName);
return;
}
SrcFile *src = malloc(sizeof(SrcFile));
if(src == NULL){
fprintf(stderr, "error: cannot alloc structure\n");
}
src->filename = srcFileName;
src->dir = getDir(srcFileName);
src->curLine = 1;
src->numCommentedLFs = 0;
src->numEscapedLFs = 0;
src->inStrLiteral = 0;
src->file = fopen(src->filename, "r");
if(src->file == NULL){
fprintf(stderr, "error: cannot open \"%s\"\n", src->filename);
exit(EXIT_FAILURE);
}
pushSrcFileName(srcFileName);
while( (buf = getLine(src)) != NULL){
int includeKind;
if(isPragma(buf)){
char *pragmaStr = getPragmaStr(buf);
if( startsWithWord(pragmaStr, "omp") || startsWithWord(pragmaStr, "acc")
|| (enableXMP && startsWithWord(pragmaStr, "xmp")) ){
fprintf(output, "%s(%s)\n", PRAGMA_NAME1, pragmaStr);
}else{
fprintf(output, "%s\n", buf);
}
}else if( (includeKind = isInclude(buf)) ){
char *includeFileName = getIncludeFileName(buf);
char *includeFilePath = findIncludeFilePath(src->dir, includeFileName, includeKind);
if(includeFilePath != NULL){
//get pos
fgetpos(src->file, &(src->pos));
//close current file
fclose(src->file);
//output filename
fprintf(output, "# 1 \"%s\"\n", includeFilePath);
preprocess(includeFilePath);
fprintf(output, "# %d \"%s\"\n", src->curLine , src->filename);
//reopen
src->file = fopen(src->filename, "r");
if(src->file == NULL){
fprintf(stderr, "error: cannot reopen \"%s\"\n", src->filename);
exit(EXIT_FAILURE);
}
//set pos
fsetpos(src->file, &(src->pos));
free(includeFilePath);
}else{
if(includeKind == USER_INCLUDE){
fprintf(output, "#include \"%s\"\n", includeFileName);
}else{
fprintf(output, "#include <%s>\n", includeFileName);
}
}
free(includeFileName);
}else{
fprintf(output, "%s\n", buf);
}
if(buf != staticBuffer){
free(buf);
}
}
popSrcFileName();
fclose(src->file);
free(src->dir);
free(src);
}
Vec3x2 Cylinder::getBeginPlaneV() const {
// 法線, 平面の一点
return std::make_pair(getDir(), from);
}
// __________________________________________________________________________________________________
void KikiBot::actionFinished ( KikiAction * action )
{
int actionId = action->getId();
if (isDead())
{
if (!died) die();
if (actionId != ACTION_PUSH && actionId != ACTION_FALL)
{
// dead player may only fall, nothing else
return;
}
}
if (spiked)
{
move_action = NULL;
startTimedAction (getActionWithId(ACTION_NOOP), 0);
return;
}
if (actionId == ACTION_PUSH || direction != KVector())
{
KikiPushable::actionFinished (action);
return;
}
if (move_action) return; // action was not a move action -> return
// find next action depending on type of finished action and surrounding environment
if (actionId == ACTION_JUMP_FORWARD)
{
KVector forwardPos = position + getDir();
if (Controller.world->isUnoccupiedPos (forwardPos))
{ // forward will be empty
if (Controller.world->isUnoccupiedPos (forwardPos - getUp()))
{ // below forward will also be empty
move_action = getActionWithId (ACTION_FALL_FORWARD);
move_action->takeRest (action);
}
else
{
move_action = getActionWithId (ACTION_FORWARD);
Controller.sound->playSoundAtPos (KikiSound::BOT_LAND, getPos(), 0.25);
}
}
else // forward will not be empty
{
if (Controller.world->isUnoccupiedPos (position - getUp())) // below is empty
{
move_action = getActionWithId (ACTION_CLIMB_UP);
Controller.sound->playSoundAtPos (KikiSound::BOT_LAND, getPos(), 0.5);
}
}
}
else if (Controller.world->isUnoccupiedPos (position - getUp())) // below will be empty
{
if (move) // sticky if moving
{
if (Controller.world->isUnoccupiedPos (position + getDir()))
{ // forward will be empty
if (Controller.world->isOccupiedPos (position + getDir() - getUp()))
{ // below forward is solid
KikiObject * occupant = Controller.world->getOccupantAtPos(position + getDir() - getUp());
if (occupant == NULL || !occupant->isSlippery())
move_action = getActionWithId (ACTION_FORWARD);
}
}
else
{
KikiObject * occupant = Controller.world->getOccupantAtPos(position + getDir());
if (occupant == NULL || !occupant->isSlippery())
move_action = getActionWithId (ACTION_CLIMB_UP);
}
}
if (move_action == NULL)
{
move_action = getActionWithId (ACTION_FALL);
move_action->takeRest (action);
}
}
else if (actionId == ACTION_FALL || actionId == ACTION_FALL_FORWARD) // landed
{
if (this == (KikiBot*)Controller.player)
{
Controller.sound->playSound (KikiSound::BOT_LAND);
}
else
{
Controller.sound->playSoundAtPos(KikiSound::BOT_LAND, getPos());
}
}
if (move_action)
{
Controller.timer_event->addAction (move_action);
return;
}
if (rotate_action) return;
if (move)
{
moveBot();
}
else
{
dir_sgn = 1.0;
if (actionId != ACTION_NOOP) jump_once = false;
// keep action chain flowing in order to detect environment changes
startTimedAction (getActionWithId (ACTION_NOOP), 0);
}
}
Vec3x2 Cylinder::getEndPlaneV() const {
// 法線, 平面の一点
return std::make_pair(getDir(), to);
}
int main(int argc, char *argv[])
{
/* Command line arguments
* -s simulator
* -n no of simulations
*/
void (*forwardSimulate)(st_part_at* prop_part, double tstep);
forwardSimulate = getForwardSimulator(argc, argv);
int n = getIters(argc, argv, 0, 1);
char *output_file = getOutputFile(argc, argv);
char *dir = getDir(argc, argv);
char *path = createPath(dir, output_file);
printf("Writing to %s\n", path);
FILE *out = fopen(path, "w");
int i, k;
r = gsl_rng_alloc(gsl_rng_mt19937);
/* For timings */
struct timeb t_start, t_end;
double t_diff;
/* Values for scaling parameter*/
int N_C = 41;
double sim_time;/*Simulation time in secs, for a single sim*/
double sc;
/*The following three vectors were generated in R and copied in */
double sc_vec[41] = {1,1.3,1.6,2,2.5,3.2,4,5,6.3,7.9,10,
12.6,15.8,20,25.1,31.6,39.8,50.1, 63.1,79.4,100,
125.9,158.5,199.5,251.2,316.2,398.1,501.2,631,794.3,
1000,1258.9,1584.9,1995.3,2511.9,3162.3,3981.1,5011.9,6309.6,7943.3,
10000};
double X1[41] = {1,1,1,2,2,3,4,5,6,7,9,11,12,14,16,17,19,20,
21,21,22,23,23,23,24,24,24, 24,24,24,24,24,24,24,24,24,24,24,24,24,24};
double X2[41] = {20,20,20,20,20,20,20,20,20,20,21,21,22,
24,26,28,32,36,43,50,60,73,89,110,135,168,209,260,325,407,
510,639,802,1007,1265,1591,2000,2515,3164,3981,5010};
/* Initialise LNA */
pSIv = SetUpSIRI();
pODEIv = pODEInfoAlloc(pgetvoidRI(pSIv));
/* Initialise parameters
* Other paramters and IC are changed in the
* loop
*/
st_part_at *part;
part = timingsInitPrior(0, 0);
VSET(part->params, 0, 2.0);
VSET(part->params, 2, 1.0/50.0);
VSET(part->params, 3, 1.0);
for(k=0; k<N_C; k++) {
sc = sc_vec[k];
VSET(part->params, 1,sc); VSET(part->params, 4, 1.0/(50*sc));
ftime(&t_start);
for(i=0; i<n; i++){
VSET(part->sps, 0, X1[k]); VSET(part->sps, 1, X2[k]);
//VSET(part->sps, 0, 0); VSET(part->sps, 1, 0);
updateRes(part->res);
forwardSimulate(part, 100);
}
ftime(&t_end);
t_diff = toMilliseconds(t_start, t_end);
sim_time = (t_diff/1000.0)/n;
printf("%f, %f\n", sc, sim_time);
fprintf(out, "%f, %f\n", sc, sim_time);
}
ODEInfoDeAlloc(pODEIv);
DestroySIRI(pSIv);
gsl_rng_free(r);
return(GSL_SUCCESS);
}
/*
Adds a single cycle region to regs if cycle is valid.
*/
void addRegion2(vector<pair<Region*, bool> >& regs, vector<Point>& cycle){
if(cycle.size()<4){ // at least 3 points
cerr << "Cycle with less than 3 different points detected" << endl;
return;
}
bool isFace = getDir(cycle);
// create a DBArray of halfsegments representing this cycle
DbArray<HalfSegment> segments1(0);
for(unsigned int i=0;i<cycle.size()-1;i++){
Point p1 = cycle[i];
Point p2 = cycle[i+1];
Point lp(0.0,0.0);
Point rp(0.0,0.0);
if(p1<p2){
lp = p1;
rp = p2;
} else {
lp = p2;
rp = p1;
}
HalfSegment hs1(true,lp,rp);
hs1.attr.edgeno = i;
hs1.attr.faceno = 0;
hs1.attr.cycleno =0;
hs1.attr.coverageno = 0;
hs1.attr.insideAbove = false;
hs1.attr.partnerno = -1;
HalfSegment hs2 = hs1;
hs2.SetLeftDomPoint(false);
segments1.Append(hs1);
segments1.Append(hs2);
}
segments1.Sort(HalfSegmentCompare);
// split the segments at crossings and overlappings
DbArray<HalfSegment>* segments = Split(segments1);
SetPartnerNo(*segments);
bool used[segments->Size()];
for(int i=0;i<segments->Size();i++){
used[i] = false;
}
// try to find cycles
vector< vector<Point> > cycles; // corrected (simple) cycles
vector<Point> path; // current path
set<Point> points; // index for points in path
bool subcycle; // a multiple point within the path?
for(int i=0; i< segments->Size(); i++){
if(!used[i]){ // start of a new path found
int pos = i;
path.clear();
points.clear();
bool done = false;
subcycle = false;
while(!done){
HalfSegment hs1;
segments->Get(pos,hs1);
Point dp = hs1.GetDomPoint();
Point ndp = hs1.GetSecPoint();
int partner = hs1.attr.partnerno;
path.push_back(dp);
points.insert(dp);
used[pos] = true;
used[partner] = true;
if(points.find(ndp)!=points.end()){ // (sub) cycle found
if(AlmostEqual(path[0],ndp)){ // cycle closed
path.push_back(ndp);
done = true;
} else { // subcycle found
subcycle = true;
}
}
if(!done){
// no cycle, try to extend
int nb = getUnusedExtension(*segments,partner,used);
if(nb>=0){ // extension found, continue
pos = nb;
} else { // dead end found, track back
cout << " ----> DEAD END FOUND <--- " << endl;
done = true; // should never occur
}
}
}
if(subcycle){
separateCycles(path,cycles);
} else if( (path.size()>3 ) && AlmostEqual(path[0],path[path.size()-1])){
vector<Point> cycle = path;
cycles.push_back(cycle);
} else {
cout << "remove invalid path of lengthh " << path.size() << endl;
}
}// new path found
} // for
delete segments;
// build the region from the corrected cycles
Region* result = 0;
for(unsigned int i = 0; i< cycles.size();i++){
vector<Point> cycle = cycles[i];
bool cw = getDir(cycle);
Region* reg = new Region(0);
reg->StartBulkLoad();
for(unsigned int j=0;j<cycle.size()-1;j++){
Point lp,rp;
bool smallb;
smallb = (cycle[j] < cycle[j+1]);
if(smallb){
lp = cycle[j];
rp = cycle[j+1];
} else {
lp = cycle[j+1];
rp = cycle[j];
}
HalfSegment hs(true,lp,rp);
hs.attr.edgeno = j;
hs.attr.insideAbove = (cw && !smallb) || (!cw && smallb);
hs.attr.faceno=0;
hs.attr.cycleno = 0;
hs.attr.coverageno = 0;
HalfSegment hs2(hs);
hs2.SetLeftDomPoint(false);
*reg += hs;
*reg += hs2;
}
reg->EndBulkLoad();
if(!result){
result = reg;
} else {
Region* tmp = SetOp(*result,*reg,avlseg::union_op);
delete result;
result = tmp;
delete reg;
}
}
if(result){
regs.push_back(make_pair(result,isFace));
}
}
