int main() {
int n; /* The number of nodes in the graph */
//FILE *f = fopen("dist.txt", "r");
//fscanf(f, "%d", &n);
n = nondef();
int i, j;
for (i = 0; i < n; ++i)
for (j = 0; j < n; ++j)
weight[i][j] = nondef();
//fscanf(f, "%d", &weight[i][j]);
//fclose(f);
/* Initialise d with infinity */
for (i = 0; i < n; ++i)
d[i] = 100000;
/* Mark all nodes as NOT beeing in the minimum spanning tree */
for (i = 0; i < n; ++i)
inTree[i] = 0;
/* Add the first node to the tree */
printf("Adding node %c\n", 0 + 'A');
inTree[0] = 1;
updateDistances(0, n);
int total = 0;
int treeSize;
for (treeSize = 1; treeSize < n; ++treeSize) {
/* Find the node with the smallest distance to the tree */
int min = -1;
for (i = 0; i < n; ++i)
if (!inTree[i])
if ((min == -1) || (d[min] > d[i]))
min = i;
/* And add it */
printf("Adding edge %c-%c\n", whoTo[min] + 'A', min + 'A');
inTree[min] = 1;
total += d[min];
updateDistances(min, n);
}
printf("Total distance: %d\n", total);
return 0;
}
void Planner::plan() {
int closestSite;
if (! updateDistances(closestSite))
return; // unable to update distances
prepareModel();
if (mdp == NULL)
return; // no action to plan yet
int horizon = computePlan(MAX_PLAN_LENGTH); // max 10 actions long ?
//publishPlan(horizon); // plan was already published if ok
delete(mdp); mdp = NULL;
}
void RobotClass::updatePosition(RobotClass::sensorsAllowed sensors) {
deltaTime = millis() - lastTime;
delay(1);
if (sensors = RobotClass::sensorsAllowed::IMU_ENC) {
//Calculate distances based on encoders
calcRDistances();
//Calculate Velocities based on encoders
calcRVelocities(leftDistance, rightDistance, deltaTime);
//Update total distance counters
updateDistances(leftDistance, rightDistance, centerDistance);
//Second, read values from the IMU
readIMU();
//Calculate change in orientation using IMU
deltaTheta = (prevGyro.zRot + currGyro.zRot)*deltaTime/2000.0f;
//testing out the complimentary filter
//Update the robots pose
updateRobotPose(centerDistance, leftDistance, rightDistance, deltaTheta, deltaTime);
updateGlobalPose(centerDistance, leftDistance, rightDistance);
lcd.gotoXY(0, 0);
lcd.print("X:");
lcd.gotoXY(2, 0);
lcd.print(globalCurrentPos.x);
lcd.gotoXY(0, 1);
lcd.print("Y:");
lcd.gotoXY(2, 1);
lcd.print(globalCurrentPos.y);
}
lastTime = millis();
}
void fixPartialRegisterStalls(Code& code)
{
if (!isX86())
return;
PhaseScope phaseScope(code, "fixPartialRegisterStalls");
Vector<BasicBlock*> candidates;
for (BasicBlock* block : code) {
for (const Inst& inst : *block) {
if (hasPartialXmmRegUpdate(inst)) {
candidates.append(block);
break;
}
}
}
// Fortunately, Partial Stalls are rarely used. Return early if no block
// cares about them.
if (candidates.isEmpty())
return;
// For each block, this provides the distance to the last instruction setting each register
// on block *entry*.
IndexMap<BasicBlock, FPDefDistance> lastDefDistance(code.size());
// Blocks with dirty distance at head.
IndexSet<BasicBlock> dirty;
// First, we compute the local distance for each block and push it to the successors.
for (BasicBlock* block : code) {
FPDefDistance localDistance;
unsigned distanceToBlockEnd = block->size();
for (Inst& inst : *block)
updateDistances(inst, localDistance, distanceToBlockEnd);
for (BasicBlock* successor : block->successorBlocks()) {
if (lastDefDistance[successor].updateFromPrecessor(localDistance))
dirty.add(successor);
}
}
// Now we propagate the minimums accross blocks.
bool changed;
do {
changed = false;
for (BasicBlock* block : code) {
if (!dirty.remove(block))
continue;
// Little shortcut: if the block is big enough, propagating it won't add any information.
if (block->size() >= minimumSafeDistance)
continue;
unsigned blockSize = block->size();
FPDefDistance& blockDistance = lastDefDistance[block];
for (BasicBlock* successor : block->successorBlocks()) {
if (lastDefDistance[successor].updateFromPrecessor(blockDistance, blockSize)) {
dirty.add(successor);
changed = true;
}
}
}
} while (changed);
// Finally, update each block as needed.
InsertionSet insertionSet(code);
for (BasicBlock* block : candidates) {
unsigned distanceToBlockEnd = block->size();
FPDefDistance& localDistance = lastDefDistance[block];
for (unsigned i = 0; i < block->size(); ++i) {
Inst& inst = block->at(i);
if (hasPartialXmmRegUpdate(inst)) {
RegisterSet defs;
RegisterSet uses;
inst.forEachTmp([&] (Tmp& tmp, Arg::Role role, Arg::Type) {
if (tmp.isFPR()) {
if (Arg::isDef(role))
defs.set(tmp.fpr());
if (Arg::isAnyUse(role))
uses.set(tmp.fpr());
}
});
// We only care about values we define but not use. Otherwise we have to wait
// for the value to be resolved anyway.
defs.exclude(uses);
defs.forEach([&] (Reg reg) {
if (localDistance.distance[MacroAssembler::fpRegisterIndex(reg.fpr())] < minimumSafeDistance)
insertionSet.insert(i, MoveZeroToDouble, inst.origin, Tmp(reg));
});
}
updateDistances(inst, localDistance, distanceToBlockEnd);
}
insertionSet.execute(block);
}
}
// Generate a map based by comparing the given
// resolution to the map's absolute dimensions (in mm)
// Note: resolution must be a multiple of 5
Map* MapGenerator::generateMap(int resolution) {
int r, c;
// Adjust dimensions according to resolution
this->height = ABS_HEIGHT/resolution;
this->width = ABS_WIDTH/resolution;
this->border = ABS_BORDER/resolution;
this->centre_circle_radius = ABS_CENTRE_CIRCLE_RADIUS/resolution;
this->line_width = ABS_LINE_WIDTH/resolution;
this->point_width = ABS_POINT_WIDTH/resolution;
this->point_height = ABS_POINT_HEIGHT/resolution;
this->point_position = ABS_POINT_POSITION/resolution;
this->box_width = ABS_BOX_WIDTH/resolution;
this->box_height = ABS_BOX_HEIGHT/resolution;
this->stride = width+2*border;
this->true_height = height+border*2;
// Allocate and initialise map data array
this->handle = new int[stride*true_height];
this->map = handle+border*stride+border;
for (r = 0; r < true_height ; r++) {
for (c = 0; c < stride ; c++) {
handle[r*stride+c] = 0;
}
}
// Now draw lines...
int rmin = 0, rmax = 0, cmin = 0, cmax = 0; // Ranges
// Top Field Line
rmin = 0; rmax = line_width;
cmin = 0; cmax = width;
drawLine(rmin,rmax,cmin,cmax);
// Bottom Field Line
rmin = height-line_width; rmax = height;
cmin = 0; cmax = width;
drawLine(rmin,rmax,cmin,cmax);
// Left Field Line
rmin = 0; rmax = height;
cmin = 0; cmax = line_width;
drawLine(rmin,rmax,cmin,cmax);
// Right Field Line
rmin = 0; rmax = height;
cmin = width-line_width; cmax = width;
drawLine(rmin,rmax,cmin,cmax);
// Draw Centre Field Line
rmin = 0; rmax = height;
cmin = width/2-line_width/2 + OFFSET; cmax = width/2+line_width/2 + OFFSET;
drawLine(rmin,rmax,cmin,cmax);
int rc = height/2, cc = width/2 + OFFSET, radius = centre_circle_radius;
// Place each quadrant on a corner of square
drawQuadrant(rc-1,cc-1,centre_circle_radius,0);
drawQuadrant(rc-1,cc,centre_circle_radius,1);
drawQuadrant(rc,cc-1,centre_circle_radius,2);
drawQuadrant(rc,cc,centre_circle_radius,3);
// Draw Left Point
rmin = height/2-point_height/2; rmax = height/2+point_height/2;
cmin = point_position-point_height/2; cmax = point_position+point_height/2;
int blank_strip = (point_height-point_width)/2;
drawPoint(rmin,rmax,cmin,cmax,blank_strip);
// Draw Right Point
rmin = height/2-point_height/2; rmax = height/2+point_height/2;
cmin = width-point_position-point_height/2; cmax = width-point_position+point_height/2;
blank_strip = (point_height-point_width)/2;
drawPoint(rmin,rmax,cmin,cmax,blank_strip);
// Draw Centre Point
rmin = height/2-point_height/2; rmax = height/2+point_height/2;
cmin = width/2-point_height/2 + OFFSET; cmax = width/2+point_height/2 + OFFSET;
drawPoint(rmin,rmax,cmin,cmax,blank_strip);
// Left Box: Draw Top Line
rmin = (height-box_height)/2; rmax = (height-box_height)/2+line_width;
cmin = 0; cmax = box_width;
drawLine(rmin,rmax,cmin,cmax);
// Left Box: Draw Bottom Line
rmin = height-(height-box_height)/2-line_width; rmax = height-(height-box_height)/2;
cmin = 0; cmax = box_width;
drawLine(rmin,rmax,cmin,cmax);
// Left Box: Draw Right Line
rmin = (height-box_height)/2; rmax = height-(height-box_height)/2;
cmin = box_width-line_width; cmax = box_width;
drawLine(rmin,rmax,cmin,cmax);
// Right Box: Draw Top Line
rmin = (height-box_height)/2; rmax = (height-box_height)/2+line_width;
cmin = width-box_width; cmax = width;
drawLine(rmin,rmax,cmin,cmax);
// Right Box: Draw Bottom Line
rmin = height-(height-box_height)/2-line_width; rmax = height-(height-box_height)/2;
cmin = width-box_width; cmax = width;
drawLine(rmin,rmax,cmin,cmax);
// Right Box: Draw Left Line
rmin = (height-box_height)/2; rmax = height-(height-box_height)/2;
cmin = width-box_width; cmax = width-box_width+line_width;
drawLine(rmin,rmax,cmin,cmax);
// Now create new data array to place line distances...
int* distance_map = new int[stride*true_height];
for (r = 0; r < true_height ; r++) {
for (c = 0; c < stride ; c++) {
distance_map[r*stride+c] = -1;
}
}
// Update distances...
for (r = 0; r < true_height ; r++) {
for (c = 0; c < stride ; c++) {
if( handle[r*stride+c] == INTERESTING) {
// Line point found, update all relevant distances
updateDistances(distance_map,r,c);
}
}
}
// Using map data array, create, cleanup and return map
// Note: Map width is really stride (similarly adjusted height)
//Map* generated_map = new Map(stride,true_height,resolution,handle);
Map* generated_map = new Map(stride,true_height,resolution,distance_map);
delete[] handle;
return generated_map;
}
