bool linesIntersect(VectorF start1, VectorF end1, VectorF start2, VectorF end2, float endSpace)
{
#if 0
float divisor = (start2.x * start1.z - end2.x * start1.z - start2.x * end1.z + end2.x * end1.z -
start1.x * start2.z + end1.x * start2.z + start1.x * end2.z - end1.x * end2.z);
if(abs(divisor) < eps * eps || abs(end2.z - start2.z) < eps)
{
return false;
}
float t1 = (-start2.x * start1.z + end2.x * start1.z + start1.x * start2.z - end2.x * start2.z -
start1.x * end2.z + start2.x * end2.z) / -divisor;
float t2 = (-end1.x * start1.z + start2.x * start1.z + start1.x * end1.z - start2.x * end1.z -
start1.x * start2.z + end1.x * start2.z) / divisor;
if(t1 < endSpace || t1 > 1 - endSpace || t2 < endSpace || t2 > 1 - endSpace)
{
return false;
}
return absSquared(interpolate(t1, start1, end1) - interpolate(t2, start2, end2)) < eps * eps;
#else
if(endSpace > 0)
{
pair<VectorF, VectorF> line1 = make_pair(interpolate(endSpace, start1, end1), interpolate(endSpace, end1, start1));
pair<VectorF, VectorF> line2 = make_pair(interpolate(endSpace, start2, end2), interpolate(endSpace, end2, start2));
start1 = get<0>(line1);
end1 = get<1>(line1);
start2 = get<0>(line2);
end2 = get<1>(line2);
}
return CCW(start1, start2, end2) != CCW(end1, start2, end2) && CCW(start1, end1, start2) != CCW(start1, end1, end2);
#endif
}
static void trace(map_t *m,int dir, int n, int h, int px, int py, bool light_walls) {
int topx[n+2], topy[n+2], botx[n+2], boty[n+2]; // convex hull of obstructions
int curt = 0, curb = 0; // size of top and bottom convex hulls
int s[2][2] = {{0, 0}, {0, 0}}; // too lazy to think of real variable names, four critical points on the convex hulls - these four points determine what is visible
int ad1,ad2[2]={0,0},eps[2] = {0, n-1},i;
topx[0] = botx[0] = boty[0] = 0, topy[0] = 1;
for (ad1 = 1; ad1 <= n; ++ad1) {
for(i=0; i <2; i++) {
eps[i] += h; // good old Bresenham
if (eps[i] >= n) {
eps[i] -= n;
++ad2[i];
}
}
for(i=0; i <2; i++) if (CCW(topx[s[!i][1]], topy[s[!i][1]], botx[s[i][0]], boty[s[i][0]], ad1, ad2[i]+i) <= 0) return; // the relevant region is no longer visible. If we don't exit the loop now, strange things happen.
int cx[2] = {ad1, ad1}, cy[2] = {ad2[0], ad2[1]};
for(i=0; i <2; i++) {
if (dir&1) cx[i] = -cx[i];
if (dir&2) cy[i] = -cy[i];
if (dir&4) cx[i] ^= cy[i], cy[i] ^= cx[i], cx[i] ^= cy[i];
cx[i] += px, cy[i] += py;
if (CCW(topx[s[i][1]], topy[s[i][1]], botx[s[!i][0]], boty[s[!i][0]], ad1, ad2[i]+1-i) > 0) {
draw(m,cx[i], cy[i], n,px,py,light_walls);
}
}
if ( (unsigned)cx[0] < (unsigned)m->width && (unsigned)cy[0] < (unsigned)m->height)
if (!m->cells[m->width*cy[0]+cx[0]].transparent) { // new obstacle, update convex hull
++curb;
botx[curb] = ad1, boty[curb] = ad2[0]+1;
if (CCW(botx[s[0][0]], boty[s[0][0]], topx[s[1][1]], topy[s[1][1]], ad1, ad2[0]+1) >= 0) return; // the obstacle obscures everything
if (CCW(topx[s[0][1]], topy[s[0][1]], botx[s[1][0]], boty[s[1][0]], ad1, ad2[0]+1) >= 0) {
s[1][0] = curb; // updating visible region
while (s[0][1] < curt && CCW(topx[s[0][1]], topy[s[0][1]], topx[s[0][1]+1], topy[s[0][1]+1], ad1, ad2[0]+1) >= 0) ++s[0][1];
}
while (curb > 1 && CCW(botx[curb-2], boty[curb-2], botx[curb-1], boty[curb-1], ad1, ad2[0]+1) >= 0) { // not convex anymore, delete a point
if (s[1][0] == curb) --s[1][0]; // s[0][0] won't be a problem
--curb;
botx[curb] = botx[curb+1], boty[curb] = boty[curb+1];
}
}
if ( (unsigned)cx[1] < (unsigned)m->width && (unsigned)cy[1] < (unsigned)m->height)
if (!m->cells[m->width*cy[1]+cx[1]].transparent) { // same as above
++curt;
topx[curt] = ad1, topy[curt] = ad2[1];
if (CCW(botx[s[1][0]], boty[s[1][0]], topx[s[0][1]], topy[s[0][1]], ad1, ad2[1]) >= 0) return;
if (CCW(topx[s[1][1]], topy[s[1][1]], botx[s[0][0]], boty[s[0][0]], ad1, ad2[1]) >= 0) {
s[1][1] = curt;
while (s[0][0] < curb && CCW(botx[s[0][0]], boty[s[0][0]], botx[s[0][0]+1], boty[s[0][0]+1], ad1, ad2[1]) <= 0) ++s[0][0];
}
while (curt > 1 && CCW(topx[curt-2], topy[curt-2], topx[curt-1], topy[curt-1], ad1, ad2[1]) <= 0) {
if (s[1][1] == curt) --s[1][1];
--curt;
topx[curt] = topx[curt+1], topy[curt] = topy[curt+1];
}
}
}
}
//------------------------------------------------------------------------
gboolean scb_is_lines_intersect(const ScbPointPtr p1,
const ScbPointPtr p2,
const ScbPointPtr p3,
const ScbPointPtr p4)
{
gboolean bRet = ((( CCW(p1, p2, p3) * CCW(p1, p2, p4)) <= 0)
&& (( CCW(p3, p4, p1) * CCW(p3, p4, p2) <= 0) )) ;
return bRet;
}
sf::ConvexShape ConvexHull(vector<sf::Vector2f> Points)
{
iter_swap(Points.begin(), min_element(Points.begin(), Points.end(), BottomLeftComp));
AngleCompare AngleComp(Points.front());
sort(Points.begin() + 1, Points.end(), AngleComp);
int m = 0;
for (unsigned int i = 1; i < Points.size(); i++)
{
while (CCW(Points[m == 0 ? Points.size() - 1 : m - 1], Points[m], Points[i]) >= 0.f)
{
if (m > 0)
m--;
else if (i == Points.size() - 1)
break;
else
i++;
}
m++;
swap(Points[m], Points[i]);
}
sf::ConvexShape Shape(m + 1);
for (unsigned int i = 0; i < Shape.getPointCount(); i++)
Shape.setPoint(i, Points[i]);
return Shape;
}
Point * calcCenter (Triangle * T){
float m21,m32, m13;
Point * center;
m21 = (P1->y - P0->y) / (P1->x - P0->x);
m32 = (P2->y - P1->y) / (P2->x - P1->x);
m13 = (P0->y - P2->y) / (P0->x - P2->x);
if(CCW(P0, P1, P2) == 0){
return NULL;
}
center = (Point*) malloc(sizeof(Point));
if(!center) return NULL;
if(!VERTICAL(P0, P1) && !VERTICAL(P1, P2) && m21!=m32){//p2 common point
center->x = (m21 * m32 * (P2->y - P0->y) + m21 * (P1->x + P2->x) - m32 * (P0->x + P1->x)) / (2 * (m21- m32));
center->y = (-(1/(m21?m21:m32))) * (center->x - ((m21?P0->x:P2->x) + P1->x)/2) + ((m21?P0->y:P2->y) + P1->y) / 2;
}
else if(!VERTICAL(P0, P2) && !VERTICAL(P1, P2) && m32!=m13){//p3 common point
center->x = (m32 * m13 * (P0->y - P1->y) + m32 * (P2->x + P0->x) - m13 * (P1->x + P2->x)) / (2 * (m32- m13));
center->y = (-(1/(m32?m32:m13))) * (center->x - ((m32?P1->x:P0->x) + P2->x)/2) + ((m32?P1->y:P0->y) + P2->y) / 2;
}
else{
center->x = (m13 * m21 * (P1->y - P2->y) + m13 * (P0->x + P1->x) - m21 * (P2->x + P0->x)) / (2 * (m13- m21));
center->y = (-(1/(m13?m13:m21))) * (center->x - ((m13?P2->x:P1->x) + P0->x)/2) + ((m13?P2->y:P1->y) + P0->y) / 2;
}
return center;
}
int Intersect(MyPoint p1, MyPoint p2, MyPoint p3, MyPoint p4) {
int i;
i = CCW(p1, p2, p3);
i = CCW(p1, p2, p4);
i = CCW(p3, p4, p1);
i = CCW(p3, p4, p2);
return ((( CCW(p1, p2, p3) * CCW(p1, p2, p4)) <= 0)
&& (( CCW(p3, p4, p1) * CCW(p3, p4, p2) <= 0) )) ;
}
double DistSP(Segment s,Point p)
{
int ccw=CCW(s.pos,s.pos+s.dir,Proj(s,p));
if(ccw==-2)
return abs(p-s.pos);
if(ccw==2)
return abs(p-(s.pos+s.dir));
return DistLP(s,p);
}
Point2D<double> EulerSpiral::compute_end_pt(double k0, double gamma, double L, bool bNormalized)
{
Point2D<double> start_pt;
Point2D<double> end_pt;
double theta;
if (bNormalized) {
start_pt = Point2D<double>(0,0);
theta = CCW(params.psi, params.start_angle);
}
else {
start_pt = params.start_pt;
theta = params.start_angle;
}
if (L==0)
return start_pt;
if (fabs(gamma)<eGamma)
{
if (fabs(k0)<eK)
{
//straight line
end_pt.setX(start_pt.getX()+L*cos(theta));
end_pt.setY(start_pt.getY()+L*sin(theta));
}
else
{
//circle
double const_term = 1.0/k0;
end_pt.setX(start_pt.getX()+const_term*(sin(k0*L+theta)-sin(theta)));
end_pt.setY(start_pt.getY()-const_term*(cos(k0*L+theta)-cos(theta)));
}
return end_pt;
}
double const1 = sqrt(M_PI*fabs(gamma));
double const2 = sqrt(M_PI/fabs(gamma));
Point2D<double> fresnel1 = get_fresnel_integral((k0+gamma*L)/const1);
Point2D<double> fresnel2 = get_fresnel_integral(k0/const1);
double C = (fresnel1.getX() - fresnel2.getX());
if(gamma<0) {
C *= -1.;
}
double S = fresnel1.getY() - fresnel2.getY();
double cos_term = cos(theta-((k0*k0)/(2.0*gamma)));
double sin_term = sin(theta-((k0*k0)/(2.0*gamma)));
end_pt.setX(start_pt.getX() + const2*(C*cos_term - S*sin_term));
end_pt.setY(start_pt.getY() + const2*(C*sin_term + S*cos_term));
return end_pt;
}
short int pointInTriangle (Point * p, Triangle * T){
float a, b, c;
a = ((P1->y - P2->y) * (p->x-P2->x) + (P2->x - P1->x) * (p->y - P2->y)) /
((P1->y - P2->y) * (P0->x - P2->x) + (P2->x - P1->x) * (P0->y - P2->y));
b = ((P2->y - P0->y) * (p->x-P2->x) + (P0->x - P2->x) * (p->y - P2->y)) /
((P1->y - P2->y) * (P0->x - P2->x) + (P2->x - P1->x) * (P0->y - P2->y));
c = 1.0 - a - b;
if (CCW(P0, p, P1) == 0 && ON_EDGE(P0, P1, p))
{return 0;}
else if (CCW(P1, p, P2) == 0 && ON_EDGE(P1, P2, p))
{return 1;}
else if (CCW(P2, p, P0) == 0 && ON_EDGE(P2, P0, p))
{return 2;}
else if (a > 0 && b > 0 && c > 0)
{return -1;}
else
{return -2;}
}
void bordercross()
{
float sec1 = seconds();
float sec2;
float sec3;
claw_up();
sleep(0.5);
toTouch(500);
sleep(0.2);
reverseT(500, 0.5);
sleep(1);
CCW();
sleep(1);
forwardT(1000, 3);
sleep(0.5);
CCW();
reverseT(500, 1);
sec2 = seconds();
sec3 = sec2 - sec1;
while (sec3 < 15)
{
sec2 = seconds();
sec3 = sec2 - sec1;
}
beep();
forwardT(1000, 6);
sleep(0.3);
claw_down();
sleep(1);
if(IR_left > threshold && IR_right > threshold)
{
while (1)
{
ao();
}
}
sleep(1);
claw_up();
sleep(0.5);
reverseT(500, 1.5);
sleep(1);
}
void center_defense()
{
CW180();
sleep(0.5);
claw_down();
sleep(0.5);
turnStraight(200, 100);
forwardT(100, 1);
CCW();
patrol();
}
int netrule :: ConvexFreeZone () const
{
int n = transfreezone.Size();
for (int i = 1; i <= n; i++)
{
const bool counterclockwise = CCW (transfreezone.Get(i),
transfreezone.Get(i % n + 1),
transfreezone.Get( (i+1) % n + 1 ),
1e-7);
//(*testout) << "ccw " << counterclockwise << endl << " p1 " << transfreezone.Get(i) << " p2 " << transfreezone.Get(i % n + 1)
// << " p3 " << transfreezone.Get( (i+1) % n + 1 ) << endl;
if (!counterclockwise )
return 0;
}
return 1;
}
void handle(){
if(!(P1IN & BIT7)&& (P1IN & BIT0)){
CW(); //Go forward
}
else if((P1IN & BIT7)&& (!(P1IN & BIT0))){
CCW(); //Go reverse
}
else{
stop(); //stop
}
}
void biofuel_block()
{
CW();
sleep(0.5);
forwardT(500, 3.2);
sleep(0.5);
claw_down();
sleep(0.5);
turnStraight(500, 250);
sleep(0.1);
claw_up();
forwardT(100, 2);
sleep(1);
CCW();
forwardT(300, 2.5);
sleep(0.1);
patrol();
}
/*************************************************************************
* FUNCTION: Intersect
*
* PURPOSE
* Given two line segments, determine if they intersect.
*
* RETURN VALUE
* TRUE if they intersect, FALSE if not.
*************************************************************************/
gboolean Intersect(const ScbPointPtr p1, const ScbPointPtr p2, const ScbPointPtr p3, const ScbPointPtr p4)
{
return ((( CCW(p1, p2, p3) * CCW(p1, p2, p4)) <= 0)
&& (( CCW(p3, p4, p1) * CCW(p3, p4, p2) <= 0) )) ;
}
short int delaunay(){
Point * point, * p;
Triangle * T, * A, * B, * C;
int i, e, m;
float a, b;
short int len;
for(e=0, p=pHead; p; ++e, p=p->next){
if(ABS(p->x) > ABS(p->y)){
i = ABS(p->x) > i ? ABS(p->x) : i;
}
else{
i = ABS(p->y) > i ? ABS(p->y) : i;
}
}
len = e;
if(e==2) return -100;
T = tHead = NEWT;
T->next = NULL;
P0 = NEWP;
P0->id = -1;
P0->x = i * 3 + 1;
P0->y = 0;
P0->next = pHead;
pHead = P0;
P1 = NEWP;
P1->id = -2;
P1->x = 0;
P1->y = i * 3 + 1;
P1->next = pHead;
pHead = P1;
P2 = NEWP;
P2->id = -3;
P2->x = P2->y = -(3 * i) - 1;
P2->next = pHead;
pHead = P2;
for(point = pHead->next->next->next; point; point = point->next){
for(T=tHead; T; T = T->next){
e = pointInTriangle(point, T);
switch(e){
case -2: continue;
case -1: goto pit;
default: goto poe;
}
}
return -200;
poe:
ADJACENT(T, T->points[e], T->points[NEXT(e)], A)
if(!A) return -300;
for(m=0; m<3 && A->points[m] != T->points[e]; ++m);
if(m==3) return -400;
B = NEWT;
B->next = tHead;
tHead = B;
C = NEWT;
C->next = tHead;
tHead = C;
B->points[0] = C->points[0] = T->points[e];
B->points[1] = T->points[PREV(e)];
C->points[1] = A->points[NEXT(m)] == T->points[NEXT(e)] ? A->points[PREV(m)] : A->points[NEXT(m)];
T->points[e] = A->points[m] = B->points[2] = C->points[2] = point;
if((i=legalizeEdge(point, T))<0) return i-510;
if((i=legalizeEdge(point, A))<0) return i-520;
if((i=legalizeEdge(point, B))<0) return i-530;
if((i=legalizeEdge(point, C))<0) return i-540;
continue;
pit:
A = NEWT;
B = NEWT;
A->next=tHead;
tHead = A;
B->next = tHead;
tHead=B;
A0 = P1;
A1 = B->points[0] = P2;
B->points[1] = P0;
P2 = A2 = B->points[2] = point;
if((i=legalizeEdge(point, T))<0) return i-610;
if((i=legalizeEdge(point, A))<0) return i-620;
if((i=legalizeEdge(point, B))<0) return i-630;
}
/*Calculate peak memory allocation*/
mem=0;
for(T=tHead, i=0; T; T=T->next, ++i);
for(point=pHead, e=0; point; point=point->next, ++e);
mem = i*sizeof(Triangle) + e*sizeof(Point);
/*Patch*/
for(T=tHead; T; T = T->next){
/*Find # of external points*/
for(i=0, e=0; i<3; e+=(T->points[i]->id < 0 ? 1 : 0), ++i);
/*If # of external points is not equal to 1 continue*/
if(e != 1) continue;
check:
/*Find external point*/
for(e=0; e<3 && T->points[e]->id > -1; e++);
/*Next mutual case*/
/*Find the adjacent triangle*/
ADJACENT(T, T->points[e], T->points[NEXT(e)], A)
if(!A) return -710;
/*Find the # of external points in the adjacent triangle*/
for(i=0, m=0; i<3; m+=(A->points[i]->id < 0 ? 1 : 0), ++i);
if(m==1){
/*Find adjacent triangles external point*/
for(m=0; m<3 && A->points[m]->id > -1; m++);
/*Check whether the convex hull criteria apply*/
a = CCW(T->points[PREV(e)], T->points[e], A->points[T->points[NEXT(e)] == A->points[NEXT(m)] ? PREV(m) : NEXT(m)]);
b = CCW(T->points[PREV(e)], T->points[NEXT(e)], A->points[T->points[NEXT(e)] == A->points[NEXT(m)] ? PREV(m) : NEXT(m)]);
a*=b;
if(a<0){
if((i=swap(T, A))<0) return -711;
T = P0->id<0 || P1->id<0 || P2->id<0 ? T : A;
goto check;
}
}
/*Find external point*/
for(e=0; e<3 && T->points[e]->id > -1; e++);
/*Prev mutual case*/
/*Find the adjacent triangle*/
ADJACENT(T, T->points[e], T->points[PREV(e)], A)
if(!A) return -720;
/*Find the external point in the adjacent triangle*/
for(i=0, m=0; i<3; m+=(A->points[i]->id < 0 ? 1 : 0), ++i);
if(m==1){
/*Find adjacent triangles external point*/
for(m=0; m<3 && A->points[m]->id > -1; m++);
/*Check whether the convex hull criteria apply*/
a = CCW(T->points[NEXT(e)], T->points[e], A->points[T->points[NEXT(e)] == A->points[NEXT(m)] ? NEXT(m) : PREV(m)]);
b = CCW(T->points[NEXT(e)], T->points[PREV(e)], A->points[T->points[NEXT(e)] == A->points[NEXT(m)] ? NEXT(m) : PREV(m)]);
a*=b;
if(a<0){
if((i=swap(T, A))<0)return -721;;
T = P0->id<0 || P1->id<0 || P2->id<0 ? T : A;
goto check;
}
}
}
for(T=tHead, A=NULL; T; A=T, T=(A==NULL?T:T->next)){
for(i=0, e=0; i<3; e+=(T->points[i]->id<0?1:0), ++i);
if(e==0) continue;
if(!A){
A=T;
T=T->next;
free(A);
A=NULL;
tHead=T;
} else {
A->next = T->next;
free(T);
T=A;
}
}
for(i=0; i<3; ++i){
point=pHead;
pHead=pHead->next;
free(point);
}
//return 1;
return len;
}
