int Karte::kleinsterWeg(int startknoten, int zielknoten) {
vector<int>Q = vector<int>();
int abstand[13];
int vorgaenger[13];
int u = 0;
for (unsigned int i = 0; i < graph.size(); i++) {
abstand[i] = (numeric_limits<int>::max() / 2);
vorgaenger[i] = -1;
Q.push_back(i);
}
abstand[startknoten] = 0;
while (!(Q.empty())) {
u = kleinsterWert(Q, abstand);
deleteFromQ(Q, u);
if (u == zielknoten) {
if (abstand[u] != (numeric_limits<int>::max() / 2))
return abstand[u];
else {
return -1;
}
}
vector<int> nachbarn = getNachbarn(u);
for (unsigned int i = 0; i < nachbarn.size(); i++) {
if (isIn(Q, nachbarn.at(i))) {
updateDistance(u, nachbarn.at(i), abstand, vorgaenger);
}
}
}
return 0;
}
void BranchBase::update(vertexName name) {
if(!visited_nodes[name].first) {
unexplored_nodes--;
}
updateVisited(name);
map.updateEdgeCount(getLatestNode()->name_,name);
distance += updateDistance(name);
updateOdom(name);
// double odomUnc = updateOdom(name);
// odom_unc_cost += odomUnc;
std::shared_ptr<Node> node_ptr = std::make_shared<Node>(name,0,distance,visited_nodes[name].first);
node_ptr->stamp = *(timestamps_.rbegin());
node_ptr->position_ = node_list.size();
node_ptr->previous_offset_ = (double)getPreviousDistance(name);
node_list.push_back(node_ptr);
visited_nodes[name].second = node_ptr;
if(odom_unc_cost > max_odom_unc) {
please_kill_me = true;
}
if(max_odom_cost < odom_unc_cost) {
max_odom_cost = odom_unc_cost;
}
}
void dijkstra(graph * gr)
{
int min = 0;
unsigned int * set = (unsigned int *)malloc(gr->num * sizeof(int));
unsigned int * dist = (unsigned int *)malloc(gr->num * sizeof(int));
memset(set,0,gr->num);
memset(dist,INT_MAX,gr->num*sizeof(int));
printDistance(dist,gr->num);
dist[0] = 0;
min = mindistance(set,dist,gr->num);
dijkstra_algo(gr,min,set,dist);
updateDistance(set,dist,gr->num);
printDistance(dist,gr->num);
}
void basicDijkstra(Graph *graph, Route *route, int start){
clock_gettime(CLOCK_REALTIME, &CALCULATION_START);
int *q = malloc(graph->countNodes * sizeof(int));
init(route, start, q);
// As long there are any nodes left
while(!nodesEmpty(q, graph->countNodes)) {
int smallestDist = I;
int i;
int node1 = 0;
// Search node with smallest value in distance
// Do this in parallel
for (i = 0; i < graph->countNodes; i++) {
/*
* Usually we have just a smaller sign here,
* but the test data is not a valid graph.
*
*/
if(q[i] == 0 && route->distance[i] <= smallestDist) {
node1 = i;
smallestDist = route->distance[i];
}
}
// Remove node from node list
q[node1] = 1;
// Determine distance for every neighbor
int begin = graph->nodes[node1];
int end;
if(node1 + 1 < (graph->countNodes)) {
end = graph->nodes[node1 + 1];
} else {
end = graph->countEdges;
}
for (i = begin; i < end; i++) {
if(q[graph->edges[i]] == 0) {
updateDistance(route, graph->weights[i], node1, graph->edges[i]);
}
}
}
free(q);
clock_gettime(CLOCK_REALTIME, &CALCULATION_END);
}
/* Render::render: render all */
void Render::render(PMDObject *objs, short *order, int num, Stage *stage, bool useMMDLikeCartoon, bool useCartoonRendering, float lightIntensity, float *lightDirection, float *lightColor, bool useShadowMapping, int shadowMappingTextureSize, bool shadowMappingLightFirst, float shadowMappingSelfDensity, float shadowMappingFloorDensity, double ellapsedTimeForMove)
{
bool updated;
/* update camera view matrices */
updated = updateDistance(ellapsedTimeForMove);
updated |= updateTransRotMatrix(ellapsedTimeForMove);
if (updated == true)
updateModelViewMatrix();
if (updateFovy(ellapsedTimeForMove) == true)
updateProjectionMatrix();
if (isViewMoving() == false)
m_viewMoveTime = -1.0;
if (useShadowMapping)
renderSceneShadowMap(objs, order, num, stage, useMMDLikeCartoon, useCartoonRendering, lightIntensity, lightDirection, lightColor, shadowMappingTextureSize, shadowMappingLightFirst, shadowMappingSelfDensity);
else
renderScene(objs, order, num, stage, useMMDLikeCartoon, useCartoonRendering, lightIntensity, lightDirection, lightColor, shadowMappingFloorDensity);
}
static uint32_t updateBaseData(uint8_t datatype, uint32_t value)
{
if (datatype >= SPORTS_DATA_MAX)
return 0;
// if (datatype != 0)
// printf("updateBaseData(%u, %lu)\n", datatype, value);
uint32_t grids_mask = 0;
switch (datatype)
{
case SPORTS_TIME:
grids_mask |= (shiftval << GRID_WORKOUT);
base_data[datatype] = value; //the raw value must be updated at last
break;
case SPORTS_DISTANCE:
updateDistance(value, &grids_mask);
grids_mask |= (shiftval << GRID_DISTANCE);
base_data[SPORTS_TIME_LAST_GPS] = workout_time;
base_data[datatype] += value; //the raw value must be updated at last
break;
case SPORTS_HEARTRATE:
updateHeartRate(value, &grids_mask);
grids_mask |= (shiftval << GRID_HEARTRATE);
base_data[datatype] = value; //the raw value must be updated at last
break;
case SPORTS_SPEED:
if (value != 0xffffffff)
{
updateSpeed(value, &grids_mask);
grids_mask |= (shiftval << GRID_SPEED);
base_data[SPORTS_TIME_LAST_GPS] = workout_time;
base_data[datatype] = value; //the raw value must be updated at last
}
break;
case SPORTS_ALT:
if (value != 0xffffffff)
{
updateAlt(value, &grids_mask);
grids_mask |= (shiftval << GRID_ALTITUTE);
base_data[SPORTS_TIME_LAST_GPS] = workout_time;
base_data[datatype] = value; //the raw value must be updated at last
}
break;
case SPORTS_STEPS:
if (base_data[datatype] == 0)
{
base_data[SPORTS_PED_STEPS_START] = value;
base_data[datatype] = 1; //the raw value must be updated at last
}
else
{
base_data[datatype] = value - base_data[SPORTS_PED_STEPS_START]; //the raw value must be updated at last
}
grids_mask |= (shiftval << GRID_STEPS);
break;
case SPORTS_PED_SPEED:
updatePedSpeed(value, &grids_mask);
base_data[datatype] = value; //the raw value must be updated at last
break;
case SPORTS_PED_DISTANCE:
updatePedDist(value, &grids_mask);
base_data[datatype] = value; //the raw value must be updated at last
break;
case SPORTS_PED_CALORIES:
if (base_data[datatype] != 0)
{
if (value > base_data[datatype])
base_data[SPORTS_CALS] += (value - base_data[datatype]);
else
base_data[SPORTS_CALS] += value;
base_data[datatype] = value;
}
grids_mask |= (shiftval << GRID_CALS);
break;
default:
base_data[datatype] = value; //the raw value must be updated at last
break;
}
return grids_mask;
}
//This function clusters based on the single linkage method.
void SingleLinkage::update(double& cutOFF){
try {
getRowColCells();
vector<bool> deleted(nRowCells, false);
int rowInd;
int search;
bool changed;
// The vector has to be traversed in reverse order to preserve the index
// for faster removal in removeCell()
for (int i=nRowCells-1;i>=0;i--) {
if ((rowCells[i]->row == smallRow) && (rowCells[i]->column == smallCol)) {
rowInd = i; // The index of the smallest distance cell in rowCells
} else {
if (rowCells[i]->row == smallRow) {
search = rowCells[i]->column;
} else {
search = rowCells[i]->row;
}
for (int j=0;j<nColCells;j++) {
if (!((colCells[j]->row == smallRow) && (colCells[j]->column == smallCol))) {
if (colCells[j]->row == search || colCells[j]->column == search) {
changed = updateDistance(colCells[j], rowCells[i]);
// If the cell's distance changed and it had the same distance as
// the smallest distance, invalidate the mins vector in SparseMatrix
if (changed) {
if (colCells[j]->vectorMap != NULL) {
*(colCells[j]->vectorMap) = NULL;
colCells[j]->vectorMap = NULL;
}
}
removeCell(rowCells[i], i , -1);
deleted[i] = true;
break;
}
}
}
if (!deleted[i]) {
// Assign the cell to the new cluster
// remove the old cell from seqVec and add the cell
// with the new row and column assignment again
removeCell(rowCells[i], i , -1, false);
if (search < smallCol){
rowCells[i]->row = smallCol;
rowCells[i]->column = search;
} else {
rowCells[i]->row = search;
rowCells[i]->column = smallCol;
}
seqVec[rowCells[i]->row].push_back(rowCells[i]);
seqVec[rowCells[i]->column].push_back(rowCells[i]);
}
}
}
clusterBins();
clusterNames();
// remove also the cell with the smallest distance
removeCell(rowCells[rowInd], -1 , -1);
}
catch(exception& e) {
m->errorOut(e, "SingleLinkage", "update");
exit(1);
}
}
void Cluster::update(double& cutOFF){
try {
smallCol = dMatrix->getSmallestCell(smallRow);
nColCells = dMatrix->seqVec[smallCol].size();
nRowCells = dMatrix->seqVec[smallRow].size();
vector<int> foundCol(nColCells, 0);
//cout << dMatrix->getNNodes() << " small cell: " << smallRow << '\t' << smallCol << endl;
int search;
bool changed;
for (int i=nRowCells-1;i>=0;i--) {
if (m->control_pressed) { break; }
//if you are not the smallCell
if (dMatrix->seqVec[smallRow][i].index != smallCol) {
search = dMatrix->seqVec[smallRow][i].index;
bool merged = false;
for (int j=0;j<nColCells;j++) {
if (dMatrix->seqVec[smallCol][j].index != smallRow) { //if you are not the smallest distance
if (dMatrix->seqVec[smallCol][j].index == search) {
foundCol[j] = 1;
merged = true;
changed = updateDistance(dMatrix->seqVec[smallCol][j], dMatrix->seqVec[smallRow][i]);
dMatrix->updateCellCompliment(smallCol, j);
break;
}else if (dMatrix->seqVec[smallCol][j].index < search) { j+=nColCells; } //we don't have a distance for this cell
}
}
//if not merged it you need it for warning
if ((!merged) && (method == "average" || method == "weighted")) {
if (cutOFF > dMatrix->seqVec[smallRow][i].dist) {
cutOFF = dMatrix->seqVec[smallRow][i].dist;
//cout << "changing cutoff to " << cutOFF << endl;
}
}
dMatrix->rmCell(smallRow, i);
}
}
clusterBins();
clusterNames();
// Special handling for singlelinkage case, not sure whether this
// could be avoided
for (int i=nColCells-1;i>=0;i--) {
if (foundCol[i] == 0) {
if (method == "average" || method == "weighted") {
if (dMatrix->seqVec[smallCol][i].index != smallRow) { //if you are not hte smallest distance
if (cutOFF > dMatrix->seqVec[smallCol][i].dist) {
cutOFF = dMatrix->seqVec[smallCol][i].dist;
}
}
}
dMatrix->rmCell(smallCol, i);
}
}
}
catch(exception& e) {
m->errorOut(e, "Cluster", "update");
exit(1);
}
}
void speedRun(void)
{
resetSpeedProfile();
useIRSensors = 1;
useSpeedProfile = 1;
int nextDir[100] = {0};
int length = 0;
xPos = 0;
yPos = 0;
orientation = 'N';
// Close off untraced routes
closeUntracedCells();
updateDistance();
visualizeGrid();
// Simulate path
for (int i = 0; !atCenter(); i++) {
if (orientation == 'N') {
while (!hasNorth(block[yPos][xPos]) && (distance[yPos + 1][xPos] == distance[yPos][xPos] - 1)) {
length++;
yPos++;
}
}
else if (orientation == 'E') {
while (!hasEast(block[yPos][xPos]) && (distance[yPos][xPos + 1] == distance[yPos][xPos] - 1)) {
length++;
xPos++;
}
}
else if (orientation == 'S') {
while (!hasSouth(block[yPos][xPos]) && (distance[yPos - 1][xPos] == distance[yPos][xPos] - 1)) {
length++;
yPos--;
}
}
else if (orientation == 'W') {
while (!hasWest(block[yPos][xPos]) && (distance[yPos][xPos - 1] == distance[yPos][xPos] - 1)) {
length++;
xPos--;
}
}
distances[i] = length;
nextDir[i] = getNextDirection();
length = 0;
}
/* Print values
for (int i = 0; distances[i]; i++)
printf("distances[%d] = %d | nextDir[%d] = %d\n\r", i, distances[i], i, nextDir[i]);
*/
orientation = 'N';
// Run path
for (int i = 0; distances[i] != 0; i++) {
moveForward(distances[i]);
if (nextDir[i] == MOVEN) {
moveN();
}
else if (nextDir[i] == MOVEE) {
moveE();
}
else if (nextDir[i] == MOVES) {
moveS();
}
else if (nextDir[i] == MOVEW) {
moveW();
}
}
useSpeedProfile = 0;
turnMotorOff;
}
