void StateManagerWindow::eraseState(unsigned slot) {
if(isStateValid(slot) == false) return;
file fp;
fp.open(filename(), file::mode::readwrite);
unsigned size = SNES::system.serialize_size();
fp.seek(slot * size);
for(unsigned i = 0; i < size; i++) fp.write(0x00);
fp.close();
//shrink state archive as much as possible:
//eg if only slot #2 and slot #31 were valid, but slot #31 was erased,
//file can be resized to only hold blank slot #1 + valid slot #2
signed lastValidState = -1;
for(signed i = StateCount - 1; i >= 0; i--) {
if(isStateValid(i)) {
lastValidState = i;
break;
}
}
if(lastValidState == -1) {
//no states used, remove empty file
unlink(filename());
} else {
unsigned neededFileSize = (lastValidState + 1) * SNES::system.serialize_size();
file fp;
if(fp.open(filename(), file::mode::readwrite)) {
if(fp.size() > neededFileSize) fp.truncate(neededFileSize);
fp.close();
}
}
}
Plane2DEnvironment(const char *ppm_file)
{
bool ok = false;
try
{
ppm_.loadFile(ppm_file);
ok = true;
}
catch(ompl::Exception &ex)
{
OMPL_ERROR("Unable to load %s.\n%s", ppm_file, ex.what());
}
if (ok)
{
auto space(std::make_shared<ob::RealVectorStateSpace>());
space->addDimension(0.0, ppm_.getWidth());
space->addDimension(0.0, ppm_.getHeight());
maxWidth_ = ppm_.getWidth() - 1;
maxHeight_ = ppm_.getHeight() - 1;
ss_ = std::make_shared<og::SimpleSetup>(space);
// set state validity checking for this space
ss_->setStateValidityChecker([this](const ob::State *state) { return isStateValid(state); });
space->setup();
ss_->getSpaceInformation()->setStateValidityCheckingResolution(1.0 / space->getMaximumExtent());
// ss_->setPlanner(std::make_shared<og::RRTConnect>(ss_->getSpaceInformation()));
}
}
bool upo_RRT::StateSpace::setGoal(State* g) {
if(!isStateValid(g))
return false;
goal_ = g;
return true;
}
bool upo_RRT::StateSpace::setStart(State* s) {
if(!isStateValid(s))
return false;
start_ = s;
return true;
}
upo_RRT::State* upo_RRT::StateSpace::sampleStateFree()
{
float x, y;
State* sample;
do
{
x = random_.uniformReal(-size_x_, size_x_);
y = random_.uniformReal(-size_y_, size_y_);
sample = new upo_RRT::State(x, y);
} while(!isStateValid(sample));
if(dimensions_ == 2)
return sample;
if(dimensions_ == 3) {
float yaw = random_.uniformReal(-PI, PI);
sample->setYaw(yaw);
return sample;
}
float yaw = random_.uniformReal(-PI, PI);
sample->setYaw(yaw);
float lv = random_.uniformReal(min_lin_vel_, max_lin_vel_);
float av = random_.uniformReal(-max_ang_vel_, max_ang_vel_);
sample->setLv(lv);
sample->setAv(av);
return sample;
}
void StateManagerWindow::synchronize() {
QList<QTreeWidgetItem*> items = list->selectedItems();
if(items.count() > 0) {
QTreeWidgetItem *item = items[0];
unsigned n = item->data(0, Qt::UserRole).toUInt();
if(isStateValid(n)) {
descriptionText->setText(getStateDescription(n));
descriptionText->setEnabled(true);
loadButton->setEnabled(true);
eraseButton->setEnabled(true);
} else {
descriptionText->setText("");
descriptionText->setEnabled(false);
loadButton->setEnabled(false);
eraseButton->setEnabled(false);
}
saveButton->setEnabled(true);
} else {
descriptionText->setText("");
descriptionText->setEnabled(false);
loadButton->setEnabled(false);
saveButton->setEnabled(false);
eraseButton->setEnabled(false);
}
}
BOOL Intellivision::LoadState(const IntellivisionState* inState)
{
if (!isStateValid(inState)) {
return FALSE;
}
memcpy(&state, inState, sizeof(IntellivisionState));
return LoadState();
}
void StateManagerWindow::setStateDescription(unsigned slot, const string &text) {
if(isStateValid(slot) == false) return;
file fp;
fp.open(filename(), file::mode::readwrite);
char description[512];
memset(&description, 0, sizeof description);
strncpy(description, text, 512);
fp.seek(slot * SNES::system.serialize_size() + 28);
fp.write((uint8_t*)description, 512);
fp.close();
}
string StateManagerWindow::getStateDescription(unsigned slot) {
if(isStateValid(slot) == false) return "";
file fp;
fp.open(filename(), file::mode::read);
char description[512];
fp.seek(slot * SNES::system.serialize_size() + 28);
fp.read((uint8_t*)description, 512);
fp.close();
description[511] = 0;
return description;
}
bool
HibernationManager::validateState( HibernatorBase::SLEEP_STATE state ) const
{
if ( ! isStateValid( state ) ) {
dprintf( D_ALWAYS,
"Attempt to set invalid sleep state %d\n", (int)state );
return false;
}
if ( ! isStateSupported(state) ) {
dprintf( D_ALWAYS,
"Attempt to set unsupported sleep state %s\n",
sleepStateToString(state) );
return false;
}
return true;
}
BOOL Intellivision::LoadState()
{
if (!isStateValid(&state)) {
return FALSE;
}
cpu.setState(state.cpuState);
stic.setState(state.sticState);
psg.setState(state.psgState);
RAM8bit.setState(state.RAM8bitState, state.RAM8bitImage);
RAM16bit.setState(state.RAM16bitState, state.RAM16bitImage);
gram.setState(state.GRAMState, state.GRAMImage);
intellivoice.setState(state.ivoiceState);
ecs.setState(state.ecsState);
return TRUE;
}
void StateManagerWindow::loadState(unsigned slot) {
if(isStateValid(slot) == false) return;
file fp;
fp.open(filename(), file::mode::read);
fp.seek(slot * SNES::system.serialize_size());
unsigned size = SNES::system.serialize_size();
uint8_t *data = new uint8_t[size];
fp.read(data, size);
fp.close();
serializer state(data, size);
delete[] data;
if(SNES::system.unserialize(state) == true) {
//toolsWindow->close();
}
}
void StateManagerWindow::update() {
//iterate all list items
QList<QTreeWidgetItem*> items = list->findItems("", Qt::MatchContains);
for(unsigned i = 0; i < items.count(); i++) {
QTreeWidgetItem *item = items[i];
unsigned n = item->data(0, Qt::UserRole).toUInt();
if(isStateValid(n) == false) {
item->setForeground(0, QBrush(QColor(128, 128, 128)));
item->setForeground(1, QBrush(QColor(128, 128, 128)));
item->setText(1, "Empty");
} else {
item->setForeground(0, QBrush(QColor(0, 0, 0)));
item->setForeground(1, QBrush(QColor(0, 0, 0)));
item->setText(1, getStateDescription(n));
}
}
for(unsigned n = 0; n <= 1; n++) list->resizeColumnToContents(n);
}
void planWithSimpleSetup()
{
// construct the state space we are planning in
auto space(std::make_shared<ob::SE2StateSpace>());
// set the bounds for the R^2 part of SE(2)
ob::RealVectorBounds bounds(2);
bounds.setLow(-1);
bounds.setHigh(1);
space->setBounds(bounds);
// create a control space
auto cspace(std::make_shared<oc::RealVectorControlSpace>(space, 2));
// set the bounds for the control space
ob::RealVectorBounds cbounds(2);
cbounds.setLow(-0.3);
cbounds.setHigh(0.3);
cspace->setBounds(cbounds);
// define a simple setup class
oc::SimpleSetup ss(cspace);
// set the state propagation routine
ss.setStatePropagator(propagate);
// set state validity checking for this space
oc::SpaceInformation *si = ss.getSpaceInformation().get();
ss.setStateValidityChecker(
[si](const ob::State *state) { return isStateValid(si, state); });
// create a start state
ob::ScopedState<ob::SE2StateSpace> start(space);
start->setX(-0.5);
start->setY(0.0);
start->setYaw(0.0);
ob::ScopedState<ob::SE2StateSpace> goal(start);
goal->setX(0.5);
// set the start and goal states
ss.setStartAndGoalStates(start, goal, 0.05);
auto td(std::make_shared<MyTriangularDecomposition>(bounds));
// print the triangulation to stdout
td->print(std::cout);
// hand the triangulation to SyclopEST
auto planner(std::make_shared<oc::SyclopEST>(ss.getSpaceInformation(), td));
// hand the SyclopEST planner to SimpleSetup
ss.setPlanner(planner);
// attempt to solve the problem within ten seconds of planning time
ob::PlannerStatus solved = ss.solve(10.0);
if (solved)
{
std::cout << "Found solution:" << std::endl;
// print the path to screen
ss.getSolutionPath().asGeometric().print(std::cout);
}
else
std::cout << "No solution found" << std::endl;
}