void Scene3d::setRotation(double yaw, double pitch, double roll) { if ((yaw != mRotation[0]) || (pitch != mRotation[1]) || (roll != mRotation[2])) { mRotation[0] = correctAngle(yaw); mRotation[1] = correctAngle(pitch); mRotation[2] = correctAngle(roll); emit rotationChanged(mRotation); } }
/** * Tests if angle a is between a1 and a2. a, a1 and a2 must be in the * range between 0 and 2*PI. * All angles in rad. * * @param reversed true for clockwise testing. false for ccw testing. * @return true if the angle a is between a1 and a2. */ bool RS_Math::isAngleBetween(double a, double a1, double a2, bool reversed) { // bool ret = false; if (reversed) swap(a1,a2); if ( ( correctAngle(a2 -a1) >= correctAngle(a - a1) + RS_TOLERANCE_ANGLE && correctAngle(a - a1) >= RS_TOLERANCE_ANGLE ) || fabs( remainder(correctAngle(a2 - a1 ) , 2.*M_PI)) < RS_TOLERANCE_ANGLE) { // the |a2-a1| % (2 pi)=0 means the whole angular range return true; } else { return false; } }
void QtGradientWidgetPrivate::setAngleConical(double angle) { double a = correctAngle(angle); if (m_angleConical == a) return; m_angleConical = a; emit q_ptr->angleConicalChanged(m_angleConical); }
void MooreHallway() { if (direction == 1) { // Move to center moveToCenter(0.0); correctAngle('B'); int lostL, lostR; double wallL, wallR; wallR = 0; urgStart(&urg); // Go until right wall is greater than 6 feet away while(fabs(wallR) < (6 * feet2mm)) { Xprintf("Going until right wall > 5ft . . .\n"); scan_center(0.0, &wallL, &wallR, &lostL, &lostR, 1); } urgStop(&urg); current++; currentRoom = -1; return; } if (direction == -1) { // Move to center moveToCenter(0.0); correctAngle('B'); // Go until right wall breaks int lostL, lostR; double wallL, wallR; lostR = 0; urgStart(&urg); while(!lostR) { Xprintf("Going until right wall breaks . . .\n"); scan_center(0.0, &wallL, &wallR, &lostL, &lostR, 1); } urgStop(&urg); current--; currentRoom = -1; return; } }
void LevineLobby() { if (direction == 1) { // Go straight until wall are back currentRoom = -1; straightBlind(10*12); turn90('L'); // Check elevators currentRoom++; if (current == destination && destinationRoom == currentRoom) return; straightBlind(2.5*12); correctAngle('R'); straightBlind(7*12); turn90('R'); straightBlind(5*12); correctAngle('R'); straightBlind(5*12); correctAngle('R'); // Check for Levine Lobby currentRoom++; if (current == destination && destinationRoom == currentRoom) return; /* double currWall = 0; int lostR = 0; int lostL = 0; double wallL, wallR; // One-wall off the right wall at current distance until wall Lost scan_center(0.0, &wallL, &currWall, &lostL, &lostR, 0); printf("Curr wall dist: %lf", currWall); while (lostR == 0) { printf("Waiting to lose Right wall.\n"); scan_center(-fabs(currWall), &wallL, &wallR, &lostL, &lostR, 1); } straightBlind(6*12); turn90('R'); straightBlind(7*12); correctAngle('R'); // Set up position tracking int lCount, rCount; pollSensors(&lCount, &rCount); position = 0.0; lostL = lostR = 0; // One-wall off the right wall at current distance for x feet scan_center(0.0, &wallL, &currWall, &lostL, &lostR, 0); printf("Curr wall dist: %lf", currWall); while (position < 12 * 12) { printf("Waiting to go 12ft.\n"); lCount = rCount = 0; pollSensors(&lCount, &rCount); position = position + ((lCount + rCount) / 2.0) * ticks2inches; scan_center(-fabs(currWall), &wallL, &wallR, &lostL, &lostR, 1); } correctAngle('R'); */ currentRoom = -1; current++; return; } if (direction == -1) { correctAngle('L'); double currWall = 0; int lostR = 0; int lostL = 0; double wallL, wallR; // One-wall off the left wall at current distance until wall Lost urgStart(&urg); scan_center(0.0, &currWall, &wallR, &lostL, &lostR, 0); char string[50]; sprintf(string, "Curr wall dist: %lf", currWall); Xprintf(string); while (lostR == 0) { Xprintf("Waiting to lose left wall.\n"); scan_center(fabs(currWall), &wallL, &wallR, &lostL, &lostR, 1); } urgStop(&urg); straightBlind(5*12); turn90('L'); correctAngle('R'); urgStart(&urg); // One-wall off the right wall at current distance until wall Lost scan_center(0.0, &wallL, &currWall, &lostL, &lostR, 0); char string2[50]; sprintf(string2, "Curr wall dist: %lf", currWall); Xprintf(string2); while (lostR == 0) { Xprintf("Waiting to lose Right wall.\n"); scan_center(-fabs(currWall), &wallL, &wallR, &lostL, &lostR, 1); } urgStop(&urg); straightBlind(3.5*12); turn90('R'); urgStart(&urg); while (lostL || fabs(wallL) > (5*feet2mm)) { if (scan_center(0.0, &wallL, &wallR, &lostL, &lostR, 0) != -10) sendSpeed(100.0, 100.0); else sendSpeed(0.0, 0.0); } urgStop(&urg); straightBlind(12); correctAngle('L'); current--; currentRoom = -1; return; } }
void LevineHallway() { if (direction == 1) { currentRoom = -1; correctAngle('R'); straightBlind(3*12); currentRoom++; // Stop for Recycling Center if (current == destination && currentRoom == destinationRoom) return; moveToCenter(0.0); correctAngle('L'); straightBlind(8*12); moveToCenter(0.0); correctAngle('B'); // Keep going until left wall too far int lostL, lostR; lostL = 0; double wallL, wallR; urgStart(&urg); while (lostL != 1) { scan_center(0.0, &wallL, &wallR, &lostL, &lostR, 1); Xprintf("Scan centering until left lost\n"); } urgStop(&urg); current++; currentRoom = -1; return; } if (direction == -1) { moveToCenter(0.0); correctAngle('B'); // Keep going until hall width < 7ft int lostL, lostR; lostL = 0; double wallL = 10000; double wallR = 10000; urgStart(&urg); while (fabs(wallL) + fabs(wallR) > (7 * feet2mm)) { scan_center(0.0, &wallL, &wallR, &lostL, &lostR, 1); Xprintf("Scan centering until hallway narrows to 7ft\n"); } urgStop(&urg); moveToCenter(0.0); correctAngle('B'); // Go until right wall breaks urgStart(&urg); while (lostR != 1) { scan_center(0.0, &wallL, &wallR, &lostL, &lostR, 1); Xprintf("Scan centering until right wall breaks\n"); } urgStop(&urg); straightBlind(20); turn90('R'); // Go straight until walls are back lostL = lostR = 1; urgStart(&urg); while (!(!lostL && !lostR)) { scan_center(0.0, &wallL, &wallR, &lostL, &lostR, 1); Xprintf("Scan centering until right lost\n"); } urgStop(&urg); sendSpeed(0.0, 0.0); current--; currentRoom = -1; return; } }
void MooreLobby() { if (direction == 1) { // Go forward for 3 feet straightBlind(36.0); // Correct Angle correctAngle('B'); // Straight Blind for 3 feet, check SIG Center straightBlind(36); if (destination == 1 && destinationRoom == 0) { currentRoom = 0; return; } correctAngle('R'); // One-wall off the right wall at current distance until wall Lost int lostL, lostR; double wallL, wallR; double currWall = 0; urgStart(&urg); scan_center(0.0, &wallL, &currWall, &lostL, &lostR, 0); lostR = 0; while (!lostR) { Xprintf("Waiting to lose Right wall.\n"); scan_center(-fabs(currWall), &wallL, &wallR, &lostL, &lostR, 1); } urgStop(&urg); // Go blind straight 7 feet straightBlind(7*12.0); // Correct angle between the white pillars correctAngle('B'); // Go blind straight 5.5 feet straightBlind(5.5*12.0); // Turn left turn90('L'); // Check Moore 100 if (destination == 1 && destinationRoom == 1) { currentRoom = 1; return; } // Go straight until right wall comes within range wallR = 100000; lostR = 1; urgStart(&urg); while(lostR || fabs(wallR) > (34* feet2mm)) { Xprintf("Going straight until right wall closer than 3ft . . .\n"); if (scan_center(0.0, &wallL, &wallR, &lostL, &lostR, 0) != -10) { sendSpeed(100.0, 100.0); } else sendSpeed(0.0, 0.0); } urgStop(&urg); // Straight to view right wall better straightBlind(12); // Align against right wall correctAngle('R'); // Go straight until walls are less than 6 feet apart wallL = wallR = 100000; lostL = lostR = 1; currWall = 0; urgStart(&urg); scan_center(0.0, &wallL, &currWall, &lostL, &lostR, 0.0); while (((fabs(wallL) + fabs(wallR)) > (6 * feet2mm)) || lostL) { Xprintf("Searching for both walls to be less than 6ft apart . . .\n"); scan_center(-fabs(currWall), &wallL, &wallR, &lostL, &lostR, 1); } urgStop(&urg); currentRoom = -1; current++; // Enter next feature return; } if (direction == -1) { // Correct angle against left wall correctAngle('L'); // One wall along the left wall until it breaks int lostL, lostR; double wallL, wallR, currWall; lostL = 0; urgStart(&urg); scan_center(0.0, &currWall, &wallR, &lostL, &lostR, 0); while(!lostL) { scan_center(fabs(currWall), &wallL, &wallR, &lostL, &lostR, 1); } urgStop(&urg); // Turn Right turn90('R'); // Go straight between the pillars straightBlind(5*12); // Correct angle correctAngle('B'); // Straight blind into the opening in the left wall straightBlind(10*12); // Keep going straight blind until the left wall returns wallL = 100000; lostL = 1; urgStart(&urg); while(lostL) { Xprintf("Going straight until left wall returns . . .\n"); if (scan_center(0.0, &wallL, &wallR, &lostL, &lostR, 0) != -10) { sendSpeed(100.0, 100.0); } else sendSpeed(0.0, 0.0); } urgStop(&urg); // Correct angle correctAngle('L'); // Keep going at current distance until Detkin lab door wall breaks urgStart(&urg); scan_center(0.0, &currWall, &wallR, &lostL, &lostR, 0); lostL = 0; while(!lostL) { scan_center(fabs(currWall), &wallL, &wallR, &lostL, &lostR, 1); } urgStop(&urg); // Straight straightBlind(12); current--; currentRoom = -1; return; } }
void GRWHallway() { if (direction == 1) { // Set up distance counting int lCount, rCount; int lostL = 0; int lostR = 0; double wallL, wallR; pollSensors(&lCount, &rCount); position = 0.0; lostL = lostR = 0; currentRoom = -1; int roomCount = 0; // Move to center moveToCenter(0.0); pollSensors(&lCount, &rCount); position = position + ((lCount + rCount) / 2.0) * ticks2inches; correctAngle('B'); pollSensors(&lCount, &rCount); urgStart(&urg); // Keep going until both walls are lost or room found while (!lostL || !lostR) { if (scan_center(0.0, &wallL, &wallR, &lostL, &lostR, 1) != -10) { lCount = rCount = 0; pollSensors(&lCount, &rCount); position = position + ((lCount + rCount) / 2.0) * ticks2inches; char string[70]; sprintf(string, "Going Straight. Pos: %lf\n", position); Xprintf(string); } if (position > array[current].roomDist[roomCount]*12) { currentRoom++; roomCount++; } if (current == destination && currentRoom == destinationRoom) return; } urgStop(&urg); straightBlind(1.5 * 12); turn90('L'); sendSpeed(0.0, 0.0); current++; currentRoom = -1; return; } if (direction == -1) { straightBlind(6); correctAngle('B'); moveToCenter(0.0); correctAngle('B'); // Keep going down center of hall until wall both disappear int lostL, lostR; double wallL, wallR; lostL = lostR = 0; wallL = wallR = 1; urgStart(&urg); while(!(lostR || lostL || ((wallL > 5 * feet2mm) && (wallR > 5 * feet2mm)))) { scan_center(0.0, &wallL, &wallR, &lostL, &lostR, 1); Xprintf("Scan centering until both walls further than 5ft or either is lost\n"); } urgStop(&urg); current--; currentRoom = -1; return; } }
void MooreGRWLobby () { if (direction == 1) { currentRoom = 0; // Return if set as home if (destination == current && destinationRoom == 0) return; // Move to 4 feet from left wall moveToCenter(4 * feet2mm); correctAngle('B'); int lostL = 1; int lostR = 1; double wallL = 10000; double wallR = 10000; // Keep going until both walls are found and less than 8ft apart urgStart(&urg); while (lostL || lostR || (fabs(wallL) + fabs(wallR)) > (8 * feet2mm)) { Xprintf("Searching for both walls . . .\n"); if (scan_center(0.0, &wallL, &wallR, &lostL, &lostR, 0) != -10) { sendSpeed(100.0, 100.0); } else { sendSpeed(0.0, 0.0); } } urgStop(&urg); current++; currentRoom = -1; return; } if (direction == -1) { // Go straight until right wall appears at less than 5ft away int lostL, lostR; double wallL, wallR; lostR = 1; urgStart(&urg); while (lostR || wallR > (5 * feet2mm)) { Xprintf("Searching for right wall closer than 5ft . . .\n"); if (scan_center(0.0, &wallL, &wallR, &lostL, &lostR, 0) != -10) { sendSpeed(100.0, 100.0); } else { sendSpeed(0.0, 0.0); } } urgStop(&urg); straightBlind(6); correctAngle('R'); moveToCenter(-4.5 * feet2mm); correctAngle('R'); lostL = lostR = 1; urgStart(&urg); // Go forward at 4.5ft from right wall until both walls appear less than 6ft apart while (lostL || lostR || (fabs(wallL) + fabs(wallR)) > (6 * feet2mm)) { scan_center(-4.5*feet2mm, &wallL, &wallR, &lostL, &lostR, 0); } urgStop(&urg); sendSpeed(0.0, 0.0); currentRoom = -1; current--; return; } }
Angle Angle::operator-(const Angle & a){ //założenie, że kąty są poprawne, tj w przedziale -PI..PI double res = this->val - a.val; res = correctAngle(res); return Angle(res); }