int SegmentsIntersect(const double a[2], const double b[2], const double c[2], const double d[2], double *x, double *t){
double c0, c1, c2, c3;
c0 = LeftTurn(a,b,c);
c1 = LeftTurn(a,b,d);
if(0 == c0 && 0 == c1){ return 0; } // collinear -> no intersection
if(dsign(c0) != dsign(c1)){
c2 = LeftTurn(c, d, a);
c3 = LeftTurn(c, d, b);
if(dsign(c2) != dsign(c3)){
if(NULL != x){
double r;
c1 = c0-c1; c3 = c2-c3;
if(fabs(c1) > fabs(c3)){
r = c0/c1;
x[0] = c[0] + r*(d[0]-c[0]);
x[1] = c[1] + r*(d[1]-c[1]);
}else{
r = c2/c3;
x[0] = a[0] + r*(b[0]-a[0]);
x[1] = a[1] + r*(b[1]-a[1]);
}
if(NULL != t){ *t = r; }
}
return 1;
}else{ return 0; }
}else{ return 0; }
}
//Go to the start
void GoStart(void)
{
int count=0;
//turn left twice
LeftTurn();
LeftTurn();
//go back 8 squares
while(count<8)
{
//increment counters
count++;
Forward();
}
//reset current count back to zero
count=0;
//turn left
LeftTurn();
//go forward 3 squares you are at start
while(count<2)
{
//increment counters
count++;
Forward();
}
motor[motorB]=0;
motor[motorC]=0;
wait1Msec(1000);
}
void motor_control()
{
if ( runFlag == false ){
if ( p2_0 == 0 ){
Motor(FORWARD, 55, 55);
runFlag = true ;
}
}
else if ( p2_0 == 0 ){
Motor(STOP,0,0);
runFlag = false ;
}
else if ( ad_data[1]<ad_data[2]-50 ){
// Speed down ;
RightTurn(20, 10);
}
else if ( ad_data[1]>ad_data[2]+50 ){
// Speed down ;
LeftTurn(10, 20);
}
/* else if (ad_data[0]!=0x01FF & ad_data[1]!=0x01FF & ad_data[2]!=0x01FF & ad_data[3]!=0x01FF){
Motor(FORWARD, 0, 0);
} */
else {
// Speed Up;
Motor(FORWARD, 55,55);
}
}
void first(){
switch(condition) {
case 0:
lineTracking();
switch(sensor.middle_sensor) {
case 0b00001111:
case 0b00011111:
case 0b00111111:
case 0b01111111:
case 0b00011110:
case 0b00111110:
condition = 1;
break;
}
break;
case 1:
wheels.stop();
RightTurn();
break;
case 2: CheckStop(); break;
case 3: Forward(); break;
case 4: BackToLine(); break;
case 5: LeftTurn(); break;
case 6:
lineTracking();
if(sensor.front_sensor == 0b00111100 || sensor.front_sensor == 0b00011100 || sensor.front_sensor == 0b00111000)
{
wheels.forward(0,0);
condition = 6;
}
break;
}
}
void
ThermalAssistantWindow::UpdateLiftPoints()
{
lift_point_avg.x = 0;
lift_point_avg.y = 0;
for (unsigned i = 0; i < 36; i++) {
Angle d = Angle::Degrees(fixed(i * 10));
lift_points[i].x = (int)((d - direction).cos() *
RangeScale(derived.lift_database[i]));
lift_points[i].y = (int)((d - direction).sin() *
RangeScale(derived.lift_database[i]));
if (!LeftTurn()) {
lift_points[i].x *= -1;
lift_points[i].y *= -1;
}
lift_points[i].x += mid.x;
lift_points[i].y += mid.y;
lift_point_avg.x += lift_points[i].x;
lift_point_avg.y += lift_points[i].y;
}
lift_points[36] = lift_points[0];
lift_point_avg.x /= 36;
lift_point_avg.y /= 36;
}
//Move next line on left
int NextLineLeft(int direction)
{
if(SensorValue(S1)<45)
{
blacksq++;
//store position to array
grid[pos1][pos2]=1;
}
//increment total
total++;
ForwardSQ();
LeftTurn();
ForwardSQ();
LeftTurn();
direction=2;
return(direction);
}//end NextLine Left()
// returns 1 if inside or on boundary, 0 otherwise
int TriangleContainsPoint(
const double org[2], // triangle vertices are {org,org+u,org+v}, in CCW orientation
const double u[2],
const double v[2],
const double p[2] // query point
){
if(LeftTurn(org,u,p) >= 0){
if(LeftTurn(org,v,p) <= 0){
// treat org as origin, we have points u and v, just need p-org
double x[2] = {p[0] - org[0], p[1] - org[1]};
if(LeftTurn(u,v,x) >= 0){
return 1;
}
}
}
return 0;
}
/* AvoidObject
A task to avoid objects. Whenever SonarSensor() returns true it will stop task follow().
It will turn left , the robot will then drive forward for a certain amount of rotations.
The robot will turn right and stop. It will ask for input from SonarSensor(), while it returns true
the robot will take small steps forward, turning and checking if the object is still there. If the object is cleared, the robot will
take a small step forward to make sure its body width doesn't chip the object. It will do the same for the length of the object.
If the robot clears the length of the object, it drives along the width of the object untill the line is found. When found, follow() is activated.
*/
task AvoidObject(){
stopTask(Follow);
LeftTurnAxis();
wait1Msec(100);
SetDistanceFwd(500,1500);
Straight(0);
wait1Msec(100);
RightTurnAxis();
wait1Msec(100);
Straight(0);
wait1Msec(100);
while(SonarSensor()){
LittleDriveForward();
wait1Msec(100);
}
if(!SonarSensor()){
TinyDriveForward();
wait1Msec(100);
}
SetDistanceFwd(1800,3500);
Straight(0);
wait1Msec(100);
RightTurnAxis();
wait1Msec(100);
while(SonarSensor()){
LittleDriveForward();
wait1Msec(100);
}
if(!SonarSensor()){
TinyDriveForward();
wait1Msec(100);
}
while((Convert(SensorValue[S3],minlight,maxlight) > 70) && (Convert(SensorValue[S4],mincolor,maxcolor) > 70)){
Straight(15);
}
nMotorEncoderTarget[motorB]=(nMotorEncoderTarget[motorC]=85);
motor[motorB]=(motor[motorC]=-10);
while((nMotorRunState[motorB] != runStateIdle) && (nMotorRunState[motorC] != runStateIdle)) wait1Msec(1);
LeftTurn();
startTask(Follow);
}
void
ThermalAssistantWindow::PaintRadarPlane(Canvas &canvas) const
{
canvas.Select(look.plane_pen);
PixelScalar x = mid.x + (LeftTurn() ? radius : -radius);
canvas.DrawLine(x + Layout::FastScale(small ? 5 : 10),
mid.y - Layout::FastScale(small ? 1 : 2),
x - Layout::FastScale(small ? 5 : 10),
mid.y - Layout::FastScale(small ? 1 : 2));
canvas.DrawLine(x,
mid.y - Layout::FastScale(small ? 3 : 6),
x,
mid.y + Layout::FastScale(small ? 3 : 6));
canvas.DrawLine(x + Layout::FastScale(small ? 2 : 4),
mid.y + Layout::FastScale(small ? 2 : 4),
x - Layout::FastScale(small ? 2 : 4),
mid.y + Layout::FastScale(small ? 2 : 4));
}
void
ThermalAssistantWindow::CalculateLiftPoints(LiftPoints &lift_points,
fixed max_lift) const
{
for (unsigned i = 0; i < lift_points.size(); i++) {
Angle d = Angle::Degrees(fixed(i * 10));
auto sincos = (d - direction).SinCos();
auto scale = NormalizeLift(vario.lift_database[i], max_lift) * fixed(radius);
lift_points[i].x = (int)(sincos.second * scale);
lift_points[i].y = (int)(sincos.first * scale);
if (!LeftTurn()) {
lift_points[i].x *= -1;
lift_points[i].y *= -1;
}
lift_points[i].x += mid.x;
lift_points[i].y += mid.y;
}
}
/*
This task makes the robot follow the line,
turn around corners,
handle crossings
and detect an obstacle.
*/
task Follow(){
int i = 0;
startTask(Sound);
while (true){
status = "active";
/*
Objects are detected below here.
When an object is detected the user can either choose to avoid it or charge.
*/
if(SonarSensor()){
while(speed > 0){
speed = Brake(i,speed);
i--;
Straight(speed);
}
status = "engage";
StopSound();
s = "";
while((s != "FIRE")&&(s != "B")){
}
if (s == "FIRE"){
startTask(Sound);
startTask(AvoidObject);
}
}
else if(speed < max_Speed){
speed = Accelerate(i,speed);
i++;
}
Straight(speed);
/*
Crossings are handled below here.
Once both lightsensors detect the line at the same time the robot will brake unless it was told to go straight.
It will then drive backward to find the line again after which it will continue untill it has crossed it.
The robot will then look at the last given command to determine which way it will go.
*/
if((Convert(SensorValue[S3],minlight,maxlight) < 70) && (Convert(SensorValue[S4],mincolor,maxcolor) < 70)){
if (command != "straight"){
speed = EmergencyBrake(speed);
wait1Msec(1000);
while(Convert(SensorValue[S3],minlight,maxlight) > 30 || Convert(SensorValue[S4],mincolor,maxcolor) > 30){
Straight((-10));
}
if (Convert(SensorValue[S3],minlight,maxlight) < 70 || Convert(SensorValue[S4],mincolor,maxcolor) < 70){
nMotorEncoderTarget[motorB]=(nMotorEncoderTarget[motorC]=105);
motor[motorB]=(motor[motorC]=-10);
while((nMotorRunState[motorB] != runStateIdle) && (nMotorRunState[motorC] != runStateIdle)) wait1Msec(1);
}
if ((s == "FIRE")||(s == "")){
StopSound();
while ((s == "FIRE")||(s == "")){
Straight(0);
}
startTask(Sound);
}
if (command == "left"){
LeftTurn();
}
else if (command == "right"){
RightTurn();
}
}
else{
wait1Msec(200);
}
}
//The code below here makes sure the robot stays on the line.
if(Convert(SensorValue[S3],minlight,maxlight) < 80){
Left(Convert(SensorValue[S3],minlight,maxlight),speed);
}
if(Convert(SensorValue[S4],mincolor,maxcolor) < 80){
Right(Convert(SensorValue[S4],mincolor,maxcolor),speed);
}
wait1Msec(1.5);
}
}
task main()
{
int count=0;
int direction=1;
int end=0;
int doubleline=0;
int pause=0;
//turn right go forward until hit double line go back half a square then turn left (You are at bottom left sq now
RightTurn();
while(pause==0)
{
motor(motorB)=SPEED;
motor(motorC)=SPEED;
wait1Msec(1);
if(SensorValue(S3)<45&&doubleline!=1)
{
if(time1[T1]<200)
{
pause=1;
}
doubleline=1;
}
if(SensorValue(S3)>45&&doubleline==1)
{
doubleline=0;
clearTimer(T1);
}
}
motor[motorB]=-20;
motor[motorC]=-20;
wait1Msec(700);
LeftTurn();
while(end!=7)
{
//traverse right and count lines
if(direction==1)
{
end++;
while(count<8)
{
if(SensorValue(S3)<45)
{
blacksq++;
//store position to array
grid[pos1][pos2]=1;
}
//increment counters
count++;
total++;
displayBigTextLine(2,"Black:%d",blacksq);
displayBigTextLine(5,"Count:%d ",count);
displayBigTextLine(8,"Total:%d",total);
//move forward one square
Forward();
pos2++;
}
//Switches to next line
if(end!=6)
{
direction=NextLineLeft(direction);
//Resets count
count=ResetCount(count);
pos2=0;
pos1++;
}
}
if(direction==2)
{
end++;
while(count<8)
{
if(SensorValue(S3)<45)
{
blacksq++;
//store position to array
grid[pos1][pos2]=1;
}
//increment counters
count++;
total++;
displayBigTextLine(2,"Black:%d",blacksq);
displayBigTextLine(5,"Count:%d ",count);
displayBigTextLine(8,"Total:%d",total);
//move forward one square
Forward();
pos2++;
}
//Switches to next line
if(end!=7)
{
//Switches to next line
direction=NextLineRight(direction);
//Resets count
count=ResetCount(count);
pos2=0;
pos1++;
}
}//end if()
//go back 7 suares and take away distance from bottom col from start from 7 and go to start
}//end while()
}//end main()
int convex_polygon_intersection(
int n, // n >= 3
const double *P,
int m, // m >= 3
const double *Q,
int *ni, // on input, size of Pi, on output, numer of points in Pi
double *Pi // output intersection polygon
){
int i, j;
if(n < 3){ return -1; }
if(NULL == P){ return -2; }
if(m < 3){ return -3; }
if(NULL == Q){ return -4; }
if(NULL == ni){ return -5; }
if(NULL == Pi){ return -6; }
// Implementation of:
// "A new linear algorithm for intersecting convex polygons"
// Joseph O'Rourke, Chi-Bin Chien, Thomas Olson, and David Naddor
// Computer Graphics and Image Processing 19, pp. 384-391 (1982)
const int nPi = *ni; *ni = 0;
int ip = 1, iq = 1;
int ipp = 0, iqp = 0; // prev of ip and iq
char inside = ' ';
// record first intersection
int first_xsected = 0;
int ipf = n, iqf = m;
int first_iter = 0;
int Pi_full = 0;
int iter;
// First, a bounding box check
{
int iP_x_min = 0, iP_x_max = 0, iP_y_min = 0, iP_y_max = 0;
int iQ_x_min = 0, iQ_x_max = 0, iQ_y_min = 0, iQ_y_max = 0;
for(i = 1; i < n; ++i){
if(P[2*i+0] < P[2*iP_x_min+0]){ iP_x_min = i; }
if(P[2*i+1] < P[2*iP_y_min+1]){ iP_y_min = i; }
if(P[2*i+0] > P[2*iP_x_max+0]){ iP_x_max = i; }
if(P[2*i+1] > P[2*iP_y_max+1]){ iP_y_max = i; }
}
for(i = 1; i < m; ++i){
if(Q[2*i+0] < Q[2*iQ_x_min+0]){ iQ_x_min = i; }
if(Q[2*i+1] < Q[2*iQ_y_min+1]){ iQ_y_min = i; }
if(Q[2*i+0] > Q[2*iQ_x_max+0]){ iQ_x_max = i; }
if(Q[2*i+1] > Q[2*iQ_y_max+1]){ iQ_y_max = i; }
}
if(
( Q[2*iQ_x_min+0] > P[2*iP_x_max+0] ) ||
( P[2*iP_x_min+0] > Q[2*iQ_x_max+0] ) ||
( Q[2*iQ_y_min+1] > P[2*iP_y_max+1] ) ||
( P[2*iP_y_min+1] > Q[2*iQ_y_max+1] )
){ return 0; }
}
for(iter = 0; iter <= 2*(m+n); ++iter){
//fprintf(stderr, "iter %d, ip = %d, iq = %d, inside = %c\n", iter, ip, iq, inside);
double xp[2];
if(SegmentsIntersect(&P[2*ipp],&P[2*ip],&Q[2*iqp],&Q[2*iq],xp, NULL)){
//fprintf(stderr, " xsect! %f,%f %f,%f %f,%f %f,%f\n", P[2*ipp+0],P[2*ipp+1],P[2*ip+0],P[2*ip+1],Q[2*iqp+0],Q[2*iqp+1],Q[2*iq+0],Q[2*iq+1]);
if(first_xsected && first_iter != iter-1){ // if the first intersection was NOT found during the previous iteration
if(ip == ipf && iq == iqf){ break; } // if this intersection is the same as the first intersection
}
if(*ni >= nPi){ Pi_full = 1; }
if(!Pi_full){ Pi[2*(*ni)+0] = xp[0]; Pi[2*(*ni)+1] = xp[1]; (*ni)++; }
//fprintf(stderr, " Adding %f,%f\n", Pi[2*((*ni)-1)+0], Pi[2*((*ni)-1)+1]);
if(LeftTurn(&Q[2*iqp],&Q[2*iq],&P[2*ip]) >= 0){
inside = 'P';
}else{ inside = 'Q'; }
if(!first_xsected){
first_xsected = 1;
ipf = ip; iqf = iq;
first_iter = iter;
}
}
xp[0] = P[2*ip+0] + (Q[2*iq+0] - P[2*ipp+0]);
xp[1] = P[2*ip+1] + (Q[2*iq+1] - P[2*ipp+1]);
if(LeftTurn(&Q[2*iqp],&Q[2*iq],xp)/*Cross(Q[2*iq]-Q[2*iqp],P[2*ip]-P[2*ipp])*/ >= 0){
if(LeftTurn(&Q[2*iqp],&Q[2*iq],&P[2*ip]) >= 0){ // advance Q
if(inside == 'Q'){
if(*ni >= nPi){ Pi_full = 1; }
if(!Pi_full){ Pi[2*(*ni)+0] = Q[2*iq+0]; Pi[2*(*ni)+1] = Q[2*iq+1]; (*ni)++; }
}
iqp = iq;
iq = (iq+1)%m;
}else{ // advance P
if(inside == 'P'){
if(*ni >= nPi){ Pi_full = 1; }
if(!Pi_full){ Pi[2*(*ni)+0] = P[2*ip+0]; Pi[2*(*ni)+1] = P[2*ip+1]; (*ni)++; }
}
ipp = ip;
ip = (ip+1)%n;
}
}else{
if(LeftTurn(&P[2*ipp],&P[2*ip],&Q[2*iq]) >= 0){ // advance P
if(inside == 'P'){
if(*ni >= nPi){ Pi_full = 1; }
if(!Pi_full){ Pi[2*(*ni)+0] = P[2*ip+0]; Pi[2*(*ni)+1] = P[2*ip+1]; (*ni)++; }
}
ipp = ip;
ip = (ip+1)%n;
}else{ // advance Q
if(inside == 'Q'){
if(*ni >= nPi){ Pi_full = 1; }
if(!Pi_full){ Pi[2*(*ni)+0] = Q[2*iq+0]; Pi[2*(*ni)+1] = Q[2*iq+1]; (*ni)++; }
}
iqp = iq;
iq = (iq+1)%m;
}
}
}
// At this point, either P in Q, Q in P, or they don't intersect
if(*ni == 0){
int flag = 1;
for(j = 0; j < n; ++j){ // really we only need to check j == 0, but due to degeneracy, it is safest to check all
for(i = 0; i < m; ++i){
if(LeftTurn(&Q[2*i],&Q[2*((i+1)%m)], &P[2*j]) < 0){
flag = 0; j = n+1; break;
}
}
}
if(flag){ // P in Q
if(*ni+n >= nPi){ Pi_full = 1; }
if(!Pi_full){
for(i = 0; i < n; ++i){
Pi[2*(*ni)+0] = P[2*i+0]; Pi[2*(*ni)+1] = P[2*i+1]; (*ni)++;
}
return 1;
}
}else{
flag = 1;
for(j = 0; j < m; ++j){ // really we only need to check j == 0, but due to degeneracy, it is safest to check all
for(i = 0; i < n; ++i){
if(LeftTurn(&P[2*i],&P[2*((i+1)%n)],&Q[2*j]) < 0){
flag = 0; j = m+1; break;
}
}
}
if(flag){ // Q in P
if(*ni+m >= nPi){ Pi_full = 1; }
if(!Pi_full){
for(i = 0; i < m; ++i){
Pi[2*(*ni)+0] = Q[2*i+0]; Pi[2*(*ni)+1] = Q[2*i+1]; (*ni)++;
}
return 2;
}
}
}
}
if(Pi_full){
return -10;
}else{
return 0;
}
}
double intersection_area_circle_triangle(
double radius,
// triangle vertices: {org, org+u, org+v} are in CCW orientation
const double tri_org[2],
const double tri_u[2],
const double tri_v[2]
){
const double origin[2] = {0,0};
int i, j;
const double iradius = 1./radius;
const double iradius2 = 1./(radius*radius);
int inside = 0; // bitfield of which triangle vertices are in circle, 4th bit is if circle center is in triangle
double vert[6];
vert[2*0+0] = tri_org[0];
vert[2*0+1] = tri_org[1];
vert[2*1+0] = (tri_org[0] + tri_u[0]);
vert[2*1+1] = (tri_org[1] + tri_u[1]);
vert[2*2+0] = (tri_org[0] + tri_v[0]);
vert[2*2+1] = (tri_org[1] + tri_v[1]);
double vert_org[6];
vert_org[2*0+0] = 0;
vert_org[2*0+1] = 0;
vert_org[2*1+0] = tri_u[0];
vert_org[2*1+1] = tri_u[1];
vert_org[2*2+0] = tri_v[0];
vert_org[2*2+1] = tri_v[1];
double tri_r[3]; // distance from circle center of each vertex of triangle, normalized to radius
for(i = 0; i < 3; ++i){
tri_r[i] = hypot(vert[2*i+0], vert[2*i+1]) * iradius;
if(tri_r[i] <= 1.0){ inside |= (1<<i); }
}
if(TriangleContainsPoint(tri_org, tri_u, tri_v, origin)){ inside |= (1<<3); }
if((inside & 0x7) == 0x7){ // all triangle points in circle
return 0.5*fabs(LeftTurn(origin,tri_u,tri_v));
}
double seg[6];
seg[2*0+0] = tri_u[0];
seg[2*0+1] = tri_u[1];
seg[2*1+0] = (tri_v[0] - tri_u[0]);
seg[2*1+1] = (tri_v[1] - tri_u[1]);
seg[2*2+0] = -tri_v[0];
seg[2*2+1] = -tri_v[1];
double side_length[3]; // normalized to radius
for(i = 0; i < 3; ++i){
side_length[i] = hypot(seg[2*i+0], seg[2*i+1]) * iradius;
}
double seg_dot[3]; // p0.(p1-p0)/r^2
for(i = 0; i < 3; ++i){
seg_dot[i] = (vert[2*i+0]*seg[2*i+0] + vert[2*i+1]*seg[2*i+1]) * iradius2;
}
// Get intersections of each segment with each circle
// segment 0 is org to u, segment 1 is u to v, segment 2 is v to org
double xp[12]; // intersection points
int nxp[3]; // number of intersections with each segment
int nx = 0;
for(i = 0; i < 3; ++i){
int ip1 = (i+1)%3;
int in0 = (inside & (1<<i)) ? 1 : 0;
int in1 = (inside & (1<<ip1)) ? 1 : 0;
if(in0 && in1){
nxp[i] = 0;
}else{
// line: x = p0 + t(p1-p0)
// circle: x^2 = r^2
// (p0 + t(p1-p0))^2 = r^2
// t^2(p1-p0)^2 + 2t(p0).(p1-p0) + (p0)^2 - r^2 = 0
// t^2 * side_length^2 + 2*t*seg_dot + tri_r^2 - 1 = 0
// t = -seg_dot/side_length^2 +/- sqrt(seg_dot^2/side_length^4 - (tri_r^2-1)/side_length^2)
double isl2 = 1./(side_length[i]*side_length[i]);
double disc = (seg_dot[i]*seg_dot[i]*isl2 - (tri_r[i]*tri_r[i]-1)) * isl2;
double t0 = -seg_dot[i]*isl2;
double t, t2, tdist, t2dist;
if(in0 != in1){
// get the one intersection point
nxp[i] = 1;
nx += 1;
if(disc < 0){ disc = 0; }
disc = sqrt(disc);
t = t0+disc;
t2 = t0-disc;
if(t > 0.5){ tdist = fabs(t-1.); }else{ tdist = fabs(t); }
if(t2 > 0.5){ t2dist = fabs(t2-1.); }else{ t2dist = fabs(t2); }
if(t2dist < tdist){ t = t2; }
if(t < 0){ t = 0; }
if(t > 1){ t = 1; }
xp[2*2*i+0] = vert_org[2*i+0] + t*seg[2*i+0];
xp[2*2*i+1] = vert_org[2*i+1] + t*seg[2*i+1];
}else{
// possibly 0 or 2 intersection points; we count 1 degenerate point as none
if(disc <= 0){
nxp[i] = 0;
}else{
disc = sqrt(disc);
nxp[i] = 0;
t = t0-disc;
t2 = t0+disc;
if(0 < t && t < 1 && 0 < t2 && t2 < 1){
xp[2*(2*i+0)+0] = vert_org[2*i+0] + t*seg[2*i+0];
xp[2*(2*i+0)+1] = vert_org[2*i+1] + t*seg[2*i+1];
xp[2*(2*i+1)+0] = vert_org[2*i+0] + t*seg[2*i+0];
xp[2*(2*i+1)+1] = vert_org[2*i+1] + t*seg[2*i+1];
nxp[i] += 2;
nx += 2;
}
}
}
}
}
/*
printf("tri: (%f,%f) (%f,%f) (%f,%f)\n",
vert[0], vert[1], vert[2], vert[3], vert[4], vert[5]);
printf("rad: %f\n", radius);
printf("inside = %d, nx = %d\n", inside, nx);
printf("xp:");
for(i = 0; i < 3; ++i){
for(j = 0; j < nxp[i]; ++j){
printf(" (%d,%d,{%f,%f})", i, j, tri_org[0]+xp[2*(2*i+j)+0], tri_org[1]+xp[2*(2*i+j)+1]);
}
}printf("\n");
*/
if(0 == nx){ // either no intersection area, or triangle entirely in circle, or circle in triangle
if((inside & 0x7) == 0x7){ // all triangle points in circle
// we already dealt with this above; getting here would be an error.
return -1;
}else{ // either no intersection area, or circle in triangle
if(inside & 0x8){ // triangle contains circle center, intersection area is either circle area or triangle area
return M_PI*radius*radius;
}else{
return 0;
}
}
}else if(2 == nx){
// Either the 2 intersections are a single side or on two different sides
if(nxp[0] < 2 && nxp[1] < 2 && nxp[2] < 2){ // on different sides
// The area is determined by tracing
}else{
for(i = 0; i < 3; ++i){
if(nxp[i] > 1){ break; }
}
// Either the circle is mostly inside with a wedge poking out a side
// or the circle is mostly outside with a wedge poking inside
double sector_area = CircularSectorArea(radius, hypot(xp[2*(2*i+1)+0]-xp[2*2*i+0],xp[2*(2*i+1)+1]-xp[2*2*i+1]));
if(inside & (1 << 3)){
// Area of circle minus a wedge
return M_PI*radius*radius - sector_area;
}else{
return sector_area;
}
}
}else if(4 == nx){
// The area is determined by tracing
}else if(6 == nx){
// The area is determined by tracing
}else{
// There is no way we can get here
return -1;
}
// At this point we expect to just trace out the intersection shape
// The vertices of the intersection shape is either a triangle vertex
// or a intersection point on a triangle edge.
int vtype[6]; // 1 = triangle vertex, 0 = intersection point
double vp[12];
int nv = 0; // number of actual vertices
for(i = 0; i < 3; ++i){
if(inside & (1 << i)){
vp[2*nv+0] = vert_org[2*i+0];
vp[2*nv+1] = vert_org[2*i+1];
vtype[nv++] = 1;
}
for(j = 0; j < nxp[i]; ++j){
vp[2*nv+0] = xp[2*(2*i+j)+0];
vp[2*nv+1] = xp[2*(2*i+j)+1];
vtype[nv++] = 0;
}
}
if(nv < 3){ // this should not be possible
return -1;
}
// All neighboring points in v which are intersection points should have circular caps added
double area = polygon_area(nv, vp);
for(i = 0; i < nv; ++i){
int im1 = i-1; if(im1 < 0){ im1 = nv-1; }
if((0 == vtype[im1]) && (0 == vtype[i])){
area += CircularSectorArea(radius, hypot(vp[2*i+0]-vp[2*im1+0],vp[2*i+1]-vp[2*im1+1]));
}
}
return area;
}
task main()
{
int count=0;
int direction=1;
int end=0;
int doubleline=0;
int pause=0;
//open the file to write
fileHandle=fileOpenWrite(filename);
//turn right go forward until hit double line go back half a square then turn left You are at bottom left sq now
RightTurn();
while(pause==0)
{
motor(motorB)=SPEED;
motor(motorC)=SPEED;
wait1Msec(1);
if(SensorValue(S3)<45&&doubleline!=1)
{
if(time1[T1]<200)
{
pause=1;
}
doubleline=1;
}
if(SensorValue(S3)>45&&doubleline==1)
{
doubleline=0;
clearTimer(T1);
}
}
motor[motorB]=-20;
motor[motorC]=-20;
wait1Msec(700);
LeftTurn();
//start traversing grid
while(end!=7)
{
//traverse right and count lines
if(direction==1)
{
end++;
while(count<8)
{
if(SensorValue(S3)<45)
{
blacksq++;
//store position to array
grid[pos1][pos2]='1';
}
//increment counters
count++;
total++;
displayBigTextLine(2,"Black:%d",blacksq);
displayBigTextLine(5,"Count:%d ",count);
displayBigTextLine(8,"Total:%d",total);
//move forward one square
Forward();
pos2++;
}
//Switches to next line
if(end!=7)
{
direction=NextLineLeft(direction);
//Resets count
count=ResetCount(count);
pos1++;
}
}
if(direction==2)
{
end++;
while(count<8)
{
if(SensorValue(S3)<45)
{
blacksq++;
//store position to array
grid[pos1][pos2]='1';
}
//increment counters
count++;
total++;
displayBigTextLine(2,"Black:%d",blacksq);
displayBigTextLine(5,"Count:%d ",count);
displayBigTextLine(8,"Total:%d",total);
//move forward one square
Forward();
pos2--;
}
//Switches to next line
if(end!=7)
{
//Switches to next line
direction=NextLineRight(direction);
//Resets count
count=ResetCount(count);
pos1++;
}
}//end if()
}//end while()
checkarray();
//Goes back to the start function
GoStart();
//THIS IS THE SECOND PART OF THE PROGRAM TO MAP THE LOCATION OF THE OBJECT
while(1==1)
{
if(getTouchValue(S2)==1)
{
//reset all values
pos1=0;
pos2=0;
count=0;
direction=1;
end=0;
while(end!=7)
{
//traverse right and count lines
if(direction==1)
{
end++;
while(count<8)
{
//check to see if there is an object 140mm away
if(getUSDistance(S1)<14)
{
grid[pos1][pos2]='J';
motor[motorB]=0;
motor[motorC]=0;
wait1Msec(1000000);
}
//increment counters
count++;
Forward();
pos2++;
}
//Switches to next line
if(end!=7)
{
direction=NextLineLeft(direction);
//Resets count
count=ResetCount(count);
pos1++;
}
}
if(direction==2)
{
//check to see if there is an object 140mm away
end++;
while(count<8)
{
if(getUSDistance(S1)<14)
{
grid[pos1][pos2]='J';
motor[motorB]=0;
motor[motorC]=0;
wait1Msec(1000000);
}
//increment counters
count++;
//move forward one square
Forward();
pos2--;
}
//Switches to next line
if(end!=7)
{
//Switches to next line
direction=NextLineRight(direction);
//Resets count
count=ResetCount(count);
pos1++;
}
}//end if()
}//end while()
} }//end if()
}//end main()
bool LineSegment::intersects (LineSegment *l)
{
return
LeftTurn(p0, l->p0, l->p1) != LeftTurn(p1, l->p0, l->p1) &&
LeftTurn(l->p0, p0, p1) != LeftTurn(l->p1, p0, p1);
}
