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