void TimerFunction(int value) { updateCoordinate(&x5,&y5,&x5step,&y5step); updateCoordinate(&x2,&y2,&x2step,&y2step); updateCoordinate(&x3,&y3,&x3step,&y3step); updateCoordinate(&x4,&y4,&x4step,&y4step); glutPostRedisplay(); glutTimerFunc(33,TimerFunction,1); }
// Function is called by Owen to return next rover direction char approachIntersection(char left, char right, char forward, int Distance) { static int Total_Points = 1; static int intersection = 0; static int point = 1; static int xcoor = 10; xcoor = updateXCOOR(0, false); static int ycoor = 10; ycoor = updateYCOOR(0, false); int Coordinate = updateCoordinate(xcoor, ycoor, point, false); static int getXCoor[50]; static int getYCoor[50]; static int pointArray[50]; static int typeArray[50]; static int exploredArray[50]; static bool initialize = true; while (initialize == true) { exploredArray[0] = 1; getXCoor[0] = xcoor; getYCoor[0] = ycoor; initialize = false; } intersection++; typeArray[intersection] = getType(left, right, forward); if (Coordinate > 0) { // Previous node identified pointArray[intersection] = Coordinate; exploredArray[Coordinate]++; exploredArray[intersection] = exploredArray[Coordinate] + 1; return old_DetermineDirection(left, right, forward, xcoor, ycoor, intersection, Distance, exploredArray, typeArray, pointArray, point, getXCoor, getYCoor); } else { // New node identified pointArray[intersection] = Total_Points; exploredArray[intersection]++; Total_Points++; // Save Coordinate updateCoordinate(xcoor, ycoor, point, true); //Coordinate[xcoor][ycoor] = point; getXCoor[point] = xcoor; getYCoor[point] = ycoor; point++; //Determine next direction return new_DetermineDirection(left, right, forward, xcoor, ycoor, Distance); } }
Color OtfTransmittanceMapOccluder::sample(const OcclusionSampleState &state) const { // Error checking if (!m_camera) { throw MissingCameraException(""); } // Transform to camera space for depth and raster space for pixel coordinate Vector csP = m_camera->worldToCamera(state.wsP, state.rayState.time); Vector rsP = m_camera->worldToRaster(state.wsP, state.rayState.time); // Bounds checks if (m_clipBehindCamera && csP.z < 0.0) { return Colors::one(); } if (rsP.x < 0.0 || rsP.x >= m_floatRasterBounds.x || rsP.y < 0.0 || rsP.y >= m_floatRasterBounds.y) { return Colors::one(); } // Compute depth to sample at Vector wsCamPosition = m_camera->position(state.rayState.time); float depth = (state.wsP - wsCamPosition).length(); // Ensure all samples are available updateCoordinate(rsP); // Finally interpolate return m_transmittanceMap.lerp(rsP.x, rsP.y, depth); }
// This is the function that makes the algorith work!!! // Determines the direction to take after coming to an intersection // North=2, South=4, East=1, West=3 char old_DetermineDirection(char left, char right, char forward, int xcoor, int ycoor, int intersection, int Distance, int exploredArray[], int typeArray[], int pointArray[], int point, int getXCoor[], int getYCoor[]) { int Direction = updateDirection(0, false); bool directionNew = true; int compareType = 0; int Coordinate = updateCoordinate(xcoor, ycoor, point, false); if (Direction == 1) { if (forward) { directionNew = true; int i = 0; while (i < intersection) { if (xcoor < getXCoor[i] && ycoor == getYCoor[i]) { if (exploredArray[i] >= typeArray[i] && Coordinate != 0) { directionNew = false; } } i++; } if (directionNew == true) { Direction = updateDirection(1, true); updateXCOOR(Distance, true); return directionToRover(Direction); } else if (directionNew == false) { compareType++; } } if (left) { directionNew = true; int i = 0; while (i < intersection) { if (ycoor > getYCoor[i] && xcoor == getXCoor[i]) { if (exploredArray[i] >= typeArray[i] && Coordinate != 0) { directionNew = false; } } i++; } if (directionNew == true) { Direction = updateDirection(2, true); updateYCOOR((-1 * Distance), true); return directionToRover(Direction); } else if (directionNew == false) { compareType++; } } if (right) { directionNew = true; int i = 0; while (i < intersection) { if (ycoor < getYCoor[i] && xcoor == getXCoor[i]) { if (exploredArray[i] >= typeArray[i] && Coordinate != 0) { directionNew = false; } } i++; } if (directionNew == true) { Direction = updateDirection(4, true); updateYCOOR(Distance, true); return directionToRover(Direction); } else if (directionNew == false) { compareType++; } } if (compareType == getType(left, right, forward)) { return traversedAdjacentPath(left, right, forward, xcoor, ycoor, intersection, Distance, exploredArray, typeArray, pointArray, point, getXCoor, getYCoor); } } else if (Direction == 2) { if (right) { directionNew = true; int i = 0; while (i < intersection) { if (xcoor < getXCoor[i] && ycoor == getYCoor[i]) { if (exploredArray[i] >= typeArray[i] && Coordinate != 0) { directionNew = false; } } i++; } if (directionNew == true) { Direction = updateDirection(1, true); updateXCOOR(Distance, true); return directionToRover(Direction); } else if (directionNew == false) { compareType++; } } if (forward) { directionNew = true; int i = 0; while (i < intersection) { if (ycoor > getYCoor[i] && xcoor == getXCoor[i]) { if (exploredArray[i] >= typeArray[i] && Coordinate != 0) { directionNew = false; } } i++; } if (directionNew == true) { Direction = updateDirection(2, true); updateYCOOR((-1 * Distance), true); return directionToRover(Direction); } else if (directionNew == false) { compareType++; } } if (left) { directionNew = true; int i = 0; while (i < intersection) { if (xcoor > getXCoor[i] && ycoor == getYCoor[i]) { if (exploredArray[i] >= typeArray[i] && Coordinate != 0) { directionNew = false; } } i++; } if (directionNew == true) { Direction = updateDirection(3, true); updateXCOOR((-1 * Distance), true); return directionToRover(Direction); } else if (directionNew == false) { compareType++; } } if (compareType == getType(left, right, forward)) { return traversedAdjacentPath(left, right, forward, xcoor, ycoor, intersection, Distance, exploredArray, typeArray, pointArray, point, getXCoor, getYCoor); } } else if (Direction == 3) { if (right) { directionNew = true; int i = 0; while (i < intersection) { if (ycoor > getYCoor[i] && xcoor == getXCoor[i]) { if (exploredArray[i] > 0) { directionNew = false; } } i++; } if (directionNew == true) { Direction = updateDirection(2, true); updateYCOOR((-1 * Distance), true); return directionToRover(Direction); } else if (directionNew == false) { compareType++; } } if (forward) { directionNew = true; int i = 0; while (i < intersection) { if (xcoor > getXCoor[i] && ycoor == getYCoor[i]) { if (exploredArray[i] > 0 && typeArray[i] != 0) { directionNew = false; } } i++; } if (directionNew == true) { Direction = updateDirection(3, true); updateXCOOR((-1 * Distance), true); return directionToRover(Direction); } else if (directionNew == false) { compareType++; } } if (left) { directionNew = true; int i = 0; while (i < intersection) { if (ycoor < getYCoor[i] && xcoor == getXCoor[i]) { if (exploredArray[i] > 0) { directionNew = false; } } i++; } if (directionNew == true) { Direction = updateDirection(4, true); updateYCOOR(Distance, true); return directionToRover(Direction); } else if (directionNew == false) { compareType++; } } if (compareType == getType(left, right, forward)) { return traversedAdjacentPath(left, right, forward, xcoor, ycoor, intersection, Distance, exploredArray, typeArray, pointArray, point, getXCoor, getYCoor); } } else if (Direction == 4) { if (left) { directionNew = true; int i = 0; while (i < intersection) { if (xcoor < getXCoor[i] && ycoor == getYCoor[i]) { if (exploredArray[i] > 0) { directionNew = false; } } i++; } if (directionNew == true) { Direction = updateDirection(1, true); updateXCOOR(Distance, true); return directionToRover(Direction); } else if (directionNew == false) { compareType++; } } if (right) { directionNew = true; int i = 0; while (i < intersection) { if (xcoor > getXCoor[i] && ycoor == getYCoor[i]) { if (exploredArray[i] > 0) { directionNew = false; } } i++; } if (directionNew == true) { Direction = updateDirection(3, true); updateXCOOR((-1 * Distance), true); return directionToRover(Direction); } else if (directionNew == false) { compareType++; } } if (forward) { directionNew = true; int i = 0; while (i < intersection) { if (ycoor < getYCoor[i] && xcoor == getXCoor[i]) { if (exploredArray[i] > 0) { directionNew = false; } } i++; } if (directionNew == true) { Direction = updateDirection(4, true); updateYCOOR(Distance, true); return directionToRover(Direction); } else if (directionNew == false) { compareType++; } } if (compareType == getType(left, right, forward)) { return traversedAdjacentPath(left, right, forward, xcoor, ycoor, intersection, Distance, exploredArray, typeArray, pointArray, point, getXCoor, getYCoor); } } }
// Determines what to do if all adjacent paths have already been traversed char traversedAdjacentPath(char left, char right, char forward, int xcoor, int ycoor, int intersection, int Distance, int exploredArray[], int typeArray[], int pointArray[], int point, int getXCoor[], int getYCoor[]) { int stop = 1; int Direction = updateDirection(0, false); int Coordinate = updateCoordinate(xcoor, ycoor, point, false); static bool endMap = false; int isDeadEnd = getType(left, right, forward); if (isDeadEnd == 1) { return new_DetermineDirection(left, right, forward, xcoor, ycoor, Distance); } else { int i = 0; while (i < intersection) { stop = 1; if (pointArray[i] == Coordinate) { if (exploredArray[i - 1] < typeArray[i - 1]) { if (ycoor == getYCoor[pointArray[i - 1]]) { if (xcoor > getXCoor[pointArray[i - 1]]) { Direction = updateDirection(3, true); updateXCOOR((-1 * Distance), true); } else if (xcoor < getXCoor[pointArray[i - 1]]) { Direction = updateDirection(1, true); updateXCOOR(Distance, true); } } else if (xcoor == getXCoor[pointArray[i - 1]]) { if (ycoor > getYCoor[pointArray[i - 1]]) { Direction = updateDirection(2, true); updateYCOOR((-1 * Distance), true); } else if (ycoor < getYCoor[pointArray[i - 1]]) { Direction = updateDirection(4, true); updateYCOOR(Distance, true); } } stop--; } else if (exploredArray[i + 1] < typeArray[i + 1]) { if (ycoor == getYCoor[pointArray[i + 1]]) { if (xcoor > getXCoor[pointArray[i + 1]]) { Direction = updateDirection(3, true); updateXCOOR((-1 * Distance), true); } else if (xcoor < getXCoor[pointArray[i + 1]]) { Direction = updateDirection(1, true); updateXCOOR(Distance, true); } } else if (xcoor == getXCoor[pointArray[i + 1]]) { if (ycoor > getYCoor[pointArray[i + 1]]) { Direction = updateDirection(2, true); updateYCOOR((-1 * Distance), true); } else if (ycoor < getYCoor[pointArray[i + 1]]) { Direction = updateDirection(4, true); updateYCOOR(Distance, true); } } stop--; } else if ((exploredArray[i - 1] >= typeArray[i - 1]) && (exploredArray[i + 1] >= typeArray[i + 1])) { stop++; } } i++; } } if (stop == 2) { // DONE MAPPING!!!! return 'E'; } else { return directionToRover(Direction); } }
static int gConfigure(int gFile) { CHECK(ITG3200_Init(gFile, ITG3200_ID, true)); struct ITG3200_Acquisition confAcquisition = { .lowPassFilter = ITG3200_LOWPASSFILTER_42, .sampleRateDivider = 0, }; CHECK(ITG3200_ConfigureAcquisition(gFile, &confAcquisition)); struct ITG3200_Power confPower = { .clockSource = ITG3200_CLOCKSOURCE_PLL_X, }; CHECK(ITG3200_ConfigurePower(gFile, &confPower)); return 0; } int main(void) { int adapter; char filename[20]; for (adapter = 0; adapter < MAX_ADAPTERS; ++adapter) { snprintf(filename, 20, "/dev/i2c-%d", adapter); if (!access(filename, R_OK | W_OK)) break; } if (adapter == MAX_ADAPTERS) { fprintf(stderr, "No I2C adapter found\n"); return -1; } aFile = open(filename, O_RDWR); if (aConfigure(aFile)) { fprintf(stderr, "Failed to initialize accelerometer\n"); return closeAndExit(-1); } cFile = open(filename, O_RDWR); if (cConfigure(cFile)) { fprintf(stderr, "Failed to initialize compass\n"); return closeAndExit(-1); } gFile = open(filename, O_RDWR); if (gConfigure(gFile)) { fprintf(stderr, "Failed to initialize gyroscope\n"); return closeAndExit(-1); } ADXL345_ReadData(aFile, &aX.data, &aY.data, &aZ.data); HMC5883L_ReadData(cFile, &cX.data, &cY.data, &cZ.data); ITG3200_ReadData(gFile, &gX.data, &gY.data, &gZ.data); ITG3200_ReadTemperature(gFile, &gT.data); initCoordinate(&aX); initCoordinate(&aY); initCoordinate(&aZ); initCoordinate(&cX); initCoordinate(&cY); initCoordinate(&cZ); initCoordinate(&gX); initCoordinate(&gY); initCoordinate(&gZ); initCoordinate(&gT); setupHandlers(); initscr(); noecho(); curs_set(false); running = 1; while (running) { ADXL345_ReadData(aFile, &aX.data, &aY.data, &aZ.data); HMC5883L_ReadData(cFile, &cX.data, &cY.data, &cZ.data); ITG3200_ReadData(gFile, &gX.data, &gY.data, &gZ.data); ITG3200_ReadTemperature(gFile, &gT.data); updateCoordinate(&aX); updateCoordinate(&aY); updateCoordinate(&aZ); updateCoordinate(&cX); updateCoordinate(&cY); updateCoordinate(&cZ); updateCoordinate(&gX); updateCoordinate(&gY); updateCoordinate(&gZ); updateCoordinate(&gT); clear(); mvprintw(0, 0, "%16s: %8d X %8d Y %8d Z\n", "Accelerometer", aX.data, aY.data, aZ.data); mvprintw(1, 0, "%16s: %8.1f X %8.1f Y %8.1f Z\n", "Smoothed", aX.smoothed, aY.smoothed, aZ.smoothed); mvprintw(2, 0, "%16s: %8.1f X %8.1f Y %8.1f Z\n", "Min", aX.min, aY.min, aZ.min); mvprintw(3, 0, "%16s: %8.1f X %8.1f Y %8.1f Z\n", "Max", aX.max, aY.max, aZ.max); mvprintw(4, 0, "%16s: %8.1f X %8.1f Y %8.1f Z\n", "Normalized", aX.normalized, aY.normalized, aZ.normalized); mvprintw(6, 0, "%16s: %8d X %8d Y %8d Z\n", "Compass", cX.data, cY.data, cZ.data); mvprintw(7, 0, "%16s: %8.1f X %8.1f Y %8.1f Z\n", "Smoothed", cX.smoothed, cY.smoothed, cZ.smoothed); mvprintw(8, 0, "%16s: %8.1f X %8.1f Y %8.1f Z\n", "Min", cX.min, cY.min, cZ.min); mvprintw(9, 0, "%16s: %8.1f X %8.1f Y %8.1f Z\n", "Max", cX.max, cY.max, cZ.max); mvprintw(10, 0, "%16s: %8.1f X %8.1f Y %8.1f Z\n", "Normalized", cX.normalized, cY.normalized, cZ.normalized); mvprintw(12, 0, "%16s: %8d X %8d Y %8d Z\n", "Gyroscope", gX.data, gY.data, gZ.data); mvprintw(13, 0, "%16s: %8.1f X %8.1f Y %8.1f Z\n", "Smoothed", gX.smoothed, gY.smoothed, gZ.smoothed); mvprintw(14, 0, "%16s: %8.1f X %8.1f Y %8.1f Z\n", "Min", gX.min, gY.min, gZ.min); mvprintw(15, 0, "%16s: %8.1f X %8.1f Y %8.1f Z\n", "Max", gX.max, gY.max, gZ.max); mvprintw(16, 0, "%16s: %8.1f X %8.1f Y %8.1f Z\n", "Normalized", gX.normalized, gY.normalized, gZ.normalized); mvprintw(18, 0, "%16s: %8d %8.1f °C\n", "Temperature", gT.data, ITG3200_ConvertTemperature(gT.data)); mvprintw(19, 0, "%16s: %8.1f %8.1f °C\n", "Smoothed", gT.smoothed, ITG3200_ConvertTemperature(gT.smoothed)); mvprintw(20, 0, "%16s: %8.1f %8.1f °C\n", "Min", gT.min, ITG3200_ConvertTemperature(gT.min)); mvprintw(21, 0, "%16s: %8.1f %8.1f °C\n", "Max", gT.max, ITG3200_ConvertTemperature(gT.max)); mvprintw(22, 0, "%16s: %8.1f\n", "Normalized", gT.normalized); refresh(); usleep(DELAY); } endwin(); close(aFile); close(cFile); close(gFile); return 0; }