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);
}
