bool GetDistance::isBetween(std::shared_ptr<EdgeClass> edge, std::shared_ptr<VertexClass>v) { std::shared_ptr<VertexClass> r = projectionPointOntoLine(edge, v); if(whichSide(v, r, edge->A) * whichSide(v, r, edge->B) <= 0) { return true; } else { return false; } }
void specialRun(float x, float y,float speed) { rotateTo(x,y,ANGLESPEED,ROTATE_ACCURATE); int cell = 6; float speedL = speed; float speedR = speed; if(GoalInFront(x,y,0) == true) return; typeCoordinate nowp = getNowPos(x,y); while (1) { float dist = getDistance2(nowp.x, nowp.y, x, y); if (dist < MIN_RANGE) break; while((dist <= 0.06)){ rotateTo(x,y,FASTSPEED,ROTATE_ACCURATE); rotateToSafe(x,y); SetLeftWheelGivenSpeed(speedL); SetRightWheelGivenSpeed(speedR); vTaskDelay(300); if((GoalInFront(x,y,0) == true)) return; } rotateToSafe(x,y); whichSide(x, y); SetLeftWheelGivenSpeed(speed); SetRightWheelGivenSpeed(speed); vTaskDelay(300); if((GoalInFront(x,y,0) == true)) return; int side = whichSide(x, y); if (side == RIGHT_HAND) { // right side speedL = speed; speedR = speed + cell; } else if (side == LEFT_HAND) { // left side speedL = speed + cell; speedR = speed; } else { speedL = speedR = speed; } rotateToSafe(x,y); SetLeftWheelGivenSpeed(30); SetRightWheelGivenSpeed(30); vTaskDelay(300); if((GoalInFront(x,y,0) == true)) return; nowp = getNowPos(x,y); } } /*end of ControlRobotgo2Position*/
//------------------------------------------------------------------------------ void ShadowVolumeBSP::clipPoly(SVNode * root, SVPoly ** store, SVPoly * poly) { if(!root) { recyclePoly(poly); return; } const PlaneF & plane = getPlane(root->mPlaneIndex); switch(whichSide(poly, plane)) { case SVNode::On: case SVNode::Back: if(root->mBack) clipPoly(root->mBack, store, poly); else addToPolyList(store, poly); break; case SVNode::Front: // encountered POLY node? if(!root->mFront) { recyclePoly(poly); return; } else clipPoly(root->mFront, store, poly); break; case SVNode::Split: { SVPoly * front; SVPoly * back; splitPoly(poly, plane, &front, &back); AssertFatal(front && back, "ShadowVolumeBSP::clipPoly: invalid split"); recyclePoly(poly); // front if(!root->mFront) { recyclePoly(front); return; } else clipPoly(root->mFront, store, front); // back if(root->mBack) clipPoly(root->mBack, store, back); else addToPolyList(store, back); break; } } }
void runStraight(float x, float y, float speed) { float speedL = speed; float speedR = speed; typeCoordinate start = GetCoordinate(); SetLeftWheelGivenSpeed(1); SetRightWheelGivenSpeed(1); vTaskDelay(500); SetLeftWheelGivenSpeed(3); SetRightWheelGivenSpeed(3); vTaskDelay(500); SetLeftWheelGivenSpeed(5); SetRightWheelGivenSpeed(5); vTaskDelay(500); SetLeftWheelGivenSpeed(10); SetRightWheelGivenSpeed(10); vTaskDelay(300); SetLeftWheelGivenSpeed(15); SetRightWheelGivenSpeed(15); vTaskDelay(300); SetLeftWheelGivenSpeed(20); SetRightWheelGivenSpeed(20); typeCoordinate nowp = GetCoordinate(); while (1) { sleepSafe(FRONT_SIGHT_LENGTH); int side = whichSide(start.x,start.y, x, y, nowp.x, nowp.y); float dist = getDistance2(nowp.x, nowp.y, x, y); if (dist >= 0.08){ rotateTo(x,y,ANGLESPEED,2); //#ifdef CONFIG_ROBOT1 // rotateTo(posM.x,posM1.y,ANGLESPEED,2); //#elif defined(CONFIG_ROBOT2) // rotateTo(posM.x,posM2.y,ANGLESPEED,2); //#elif defined(CONFIG_ROBOT3) // rotateTo(posM.x,posM3.y,ANGLESPEED,2); //#elif defined(CONFIG_ROBOT4) // rotateTo(posM.x,posM4.y,ANGLESPEED,2); //#endif } if (side == -2 || dist < EPS) break; if (side == -1) { // right side speedL = speed; speedR = speed+3; } else if (side == 1) { // left side speedL = speed+3; speedR = speed; } else { speedL = speedR = speed; } SetLeftWheelGivenSpeed(speedL); SetRightWheelGivenSpeed(speedR); vTaskDelay(300); nowp = GetCoordinate(); } #ifdef DEBUG halt(2); #endif }
//------------------------------------------------------------------------------ bool ShadowVolumeBSP::testPoly(SVNode * root, SVPoly * poly) { const PlaneF & plane = getPlane(root->mPlaneIndex); switch(whichSide(poly, plane)) { case SVNode::On: case SVNode::Front: if(root->mFront) return(testPoly(root->mFront, poly)); recyclePoly(poly); return(true); case SVNode::Back: if(root->mBack) return(testPoly(root->mBack, poly)); recyclePoly(poly); break; case SVNode::Split: { if(!root->mFront) { recyclePoly(poly); return(true); } SVPoly * front; SVPoly * back; splitPoly(poly, plane, &front, &back); recyclePoly(poly); if(testPoly(root->mFront, front)) { recyclePoly(back); return(true); } if(root->mBack) return(testPoly(root->mBack, back)); recyclePoly(back); break; } } return(false); }
void ShadowVolumeBSP::insertPoly(SVNode ** root, SVPoly * poly) { if(!(*root)) { insertShadowVolume(root, poly->mShadowVolume); if(poly->mSurfaceInfo && !mFirstInteriorNode) mFirstInteriorNode = mShadowVolumes[poly->mShadowVolume]; if(poly->mTarget) addUniqueVolume(poly->mTarget->mSurfaceInfo, poly->mShadowVolume); recyclePoly(poly); return; } const PlaneF & plane = getPlane((*root)->mPlaneIndex); // switch(whichSide(poly, plane)) { case SVNode::On: case SVNode::Front: insertPolyFront(root, poly); break; case SVNode::Back: insertPolyBack(root, poly); break; case SVNode::Split: { SVPoly * front; SVPoly * back; splitPoly(poly, plane, &front, &back); recyclePoly(poly); insertPolyFront(root, front); insertPolyBack(root, back); break; } } }
void runStraight(float x, float y, float speed, int flag) { float speedL = speed; float speedR = speed; typeCoordinate start = GetCoordinate(); typeCoordinate nowp = GetCoordinate(); while (1) { int side = whichSide(start.x,start.y, x, y, nowp.x, nowp.y); float dist = getDistance2(nowp.x, nowp.y, x, y); while(1){ if(sleepSafe(FRONT_SIGHT_LENGTH) == 0) break; } while(dist <= 0.10){ rotateTo(x,y,ANGLESPEED,1); SetLeftWheelGivenSpeed(speedL); SetRightWheelGivenSpeed(speedR); vTaskDelay(300); if(GoalInFront(x,y) == true) return; } if (side == -2 || dist < EPS) break; if (side == -1) { // right side speedL = speed; speedR = speed+5; } else if (side == 1) { // left side speedL = speed+5; speedR = speed; } else { speedL = speedR = speed; } SetLeftWheelGivenSpeed(speedL); SetRightWheelGivenSpeed(speedR); vTaskDelay(300); nowp = GetCoordinate(); } #ifdef DEBUG halt(2); #endif }
int main() { enable_servos(); claw_down(); whichSide(); strategySelect(); //wait_for_light(7); if (strategy == 1) { printf("Running luggage carts...\n"); run_for(117, strategy_1); start_process(*countdown); } if (strategy == 2) { printf("Running tower tip...\n"); run_for(117, strategy_2); start_process(*countdown); } if (strategy == 3) { printf("Running biofuel block...\n"); run_for(117, strategy_3); start_process(*countdown); } if (strategy == 4) { printf("Running center defense...\n"); run_for(117, strategy_4); start_process(*countdown); } }
// clip a poly to it's own shadow volume void ShadowVolumeBSP::clipToSelf(SVNode * root, SVPoly ** store, SVPoly * poly) { if(!root) { addToPolyList(store, poly); return; } const PlaneF & plane = getPlane(root->mPlaneIndex); switch(whichSide(poly, plane)) { case SVNode::Front: clipToSelf(root->mFront, store, poly); break; case SVNode::On: addToPolyList(store, poly); break; case SVNode::Back: recyclePoly(poly); break; case SVNode::Split: { SVPoly * front = 0; SVPoly * back = 0; splitPoly(poly, plane, &front, &back); AssertFatal(front && back, "ShadowVolumeBSP::clipToSelf: invalid split"); recyclePoly(poly); recyclePoly(back); clipToSelf(root->mFront, store, front); break; } } }
/* -------------------------------------------------------------------- */ static void TrackTimeSeries(int x, int y) { size_t i; PLOT_INFO *plot = NULL; float yDiff; struct axisInfo *axis; if (saveX) /* Erase previous lines. */ { XDrawLine(dpy, win, gc, mainPlot[0].x.LV, saveY, mainPlot[0].x.RV, saveY); for (i = 0; i < NumberOfPanels; ++i) XDrawLine(dpy, win, gc, saveX, mainPlot[i].x.TH, saveX, mainPlot[i].x.BH); XDrawString(dpy, win, gc, saveX+10, saveY+30, tStr[0], strlen(tStr[0])); XDrawString(dpy, win, gc, saveX-50, saveY-10, tStr[1], strlen(tStr[1])); } /* Locate which panel the cursor is in. */ if (x >= mainPlot[0].x.LV && x <= mainPlot[0].x.RV) for (i = 0; i < NumberOfPanels; ++i) if (y >= mainPlot[i].x.TH && y <= mainPlot[i].x.BH) plot = &mainPlot[i]; /* Bail out if we're outside all panels. */ if (!plot) { saveX = saveY = 0; tStr[0][0] = tStr[1][0] = tStr[2][0] = '\0'; PointerCursor(mainPlot[0].canvas); return; } CrossHairCursor(mainPlot[0].canvas); /* Calculate engineering units. */ MakeTimeTicLabel(tStr[0], x - mainPlot[0].x.LV, mainPlot[0].x.HD); XDrawString(dpy, win, gc, x+10, y+30, tStr[0], strlen(tStr[0])); axis = &plot->Yaxis[whichSide()]; if (axis->logScale) { float yScale = plot->x.VD / (log10(axis->max) - log10(axis->min)); if (axis->invertAxis) yDiff = ((y - plot->x.TH) / yScale) + log10(axis->min); else yDiff = -(((y - plot->x.BH) / yScale) - log10(axis->min)); sprintf(tStr[1], "%g", pow(10.0, yDiff)); } else { yDiff = axis->max - axis->min; if (axis->invertAxis) yDiff *= (float)(y - plot->x.TH) / plot->x.VD; else yDiff *= (float)(plot->x.BH - y) / plot->x.VD; sprintf(tStr[1], "%g", axis->min + yDiff); } XDrawString(dpy, win, gc, x-50, y-10, tStr[1], strlen(tStr[1])); /* Draw new lines and text. */ XDrawLine(dpy, win, gc, plot->x.LV, y, plot->x.RV, y); for (i = 0; i < NumberOfPanels; ++i) XDrawLine(dpy, win, gc, x, mainPlot[i].x.TH, x, mainPlot[i].x.BH); saveX = x; saveY = y; } /* END TRACKTIMESERIES */
int main() { whichSide(); //Lift(); ScorerNav(); }