#include <vis.h>
#ifdef DMALLOC
#include "dmalloc.h"
#endif
#ifdef TRANSPARENT
#define INTERSECT(a,b,c,d,e) intersect1((a),(b),(c),(d),(e))
#else
#define INTERSECT(a,b,c,d,e) intersect((a),(b),(c),(d))
#endif
static array2 allocArray (int V, int extra)
{
int i, k;
array2 arr;
COORD* p;
arr = (COORD**)malloc((V + extra)*sizeof(COORD *));
for (i=0; i < V; i++) {
p = (COORD*)malloc(V*sizeof(COORD));
arr[i] = p;
for (k=0; k < V; k++) {
*p++ = 0;
}
}
for (i=V; i < V+extra; i++)
arr[i] = (COORD*)0;
return arr;
}
COORD area2(Ppoint_t a, Ppoint_t b, Ppoint_t c)
{
return ((a.y - b.y)*(c.x - b.x) - (c.y - b.y)*(a.x - b.x));
}
static int wind(Ppoint_t a, Ppoint_t b, Ppoint_t c)
{
COORD w;
w = ((a.y - b.y)*(c.x - b.x) - (c.y - b.y)*(a.x - b.x));
return (w > .0001) ? 1 : ((w < -.0001) ? -1 : 0);
}
#if 0
static int open_intersect(Ppoint_t a,Ppoint_t b,Ppoint_t c,Ppoint_t d)
{
return (((area2(a,b,c) > 0 && area2(a,b,d) < 0) ||
(area2(a,b,c) < 0 && area2(a,b,d) > 0))
&&
((area2(c,d,a) > 0 && area2(c,d,b) < 0 ) ||
(area2(c,d,a) < 0 && area2(c,d,b) > 0 )));
}
#endif
int inBetween (Ppoint_t a,Ppoint_t b,Ppoint_t c)
{
if (a.x != b.x)
return (((a.x < c.x) && (c.x < b.x)) || ((b.x < c.x) && (c.x < a.x)));
else
return (((a.y < c.y) && (c.y < b.y)) || ((b.y < c.y) && (c.y < a.y)));
}
#ifdef TRANSPARENT
static int intersect1(Ppoint_t a,Ppoint_t b,Ppoint_t q,Ppoint_t n, Ppoint_t p)
{
int w_abq;
int w_abn;
int w_qna;
int w_qnb;
w_abq = wind(a,b,q);
w_abn = wind(a,b,n);
if ((w_abq == 0) && inBetween (a,b,q)) {
return ((w_abn * wind(a,b,p) < 0) || (wind(p,q,n) > 0));
}
else {
w_qna = wind(q,n,a);
w_qnb = wind(q,n,b);
return (((w_abq * w_abn) < 0) && ((w_qna * w_qnb) < 0));
}
}
#else
int intersect(Ppoint_t a,Ppoint_t b,Ppoint_t c,Ppoint_t d)
{
int a_abc;
int a_abd;
int a_cda;
int a_cdb;
a_abc = wind(a,b,c);
if ((a_abc == 0) && inBetween (a,b,c)) {
return 1;
}
a_abd = wind(a,b,d);
if ((a_abd == 0) && inBetween (a,b,d)) {
return 1;
}
a_cda = wind(c,d,a);
a_cdb = wind(c,d,b);
return (((a_abc * a_abd) < 0) && ((a_cda * a_cdb) < 0));
}
#endif
static int in_cone (Ppoint_t a0,Ppoint_t a1,Ppoint_t a2,Ppoint_t b)
{
int m = wind(b,a0,a1);
int p = wind(b,a1,a2);
if (wind(a0,a1,a2) > 0)
return ( m >= 0 && p >= 0 );
else
return ( m >= 0 || p >= 0 );
}
#if 0
static int in_open_cone (Ppoint_t a0,Ppoint_t a1,Ppoint_t a2,Ppoint_t b)
{
int m = wind(b,a0,a1);
int p = wind(b,a1,a2);
if (wind(a0,a1,a2) >= 0)
return ( m > 0 && p > 0 );
else
return ( m > 0 || p > 0 );
}
#endif
COORD dist2 (Ppoint_t a, Ppoint_t b)
{
COORD delx = a.x - b.x;
COORD dely = a.y - b.y;
return (delx*delx + dely*dely);
}
static COORD dist (Ppoint_t a, Ppoint_t b)
{
return sqrt(dist2(a,b));
}
static int inCone (int i, int j, Ppoint_t pts[], int nextPt[], int prevPt[])
{
return in_cone (pts[prevPt[i]],pts[i],pts[nextPt[i]],pts[j]);
}
static int clear (Ppoint_t pti, Ppoint_t ptj,
int start, int end,
int V, Ppoint_t pts[], int nextPt[], int prevPt[])
{
int k;
for (k=0; k < start; k++) {
if (INTERSECT(pti,ptj,pts [k], pts[nextPt [k]], pts[prevPt [k]]))
return 0;
}
for (k=end; k < V; k++) {
if (INTERSECT(pti,ptj,pts [k], pts[nextPt [k]], pts[prevPt [k]]))
return 0;
}
return 1;
}
static void compVis (vconfig_t *conf, int start)
{
int V = conf->N;
Ppoint_t* pts = conf->P;
int* nextPt = conf->next;
int* prevPt = conf->prev;
array2 wadj = conf->vis;
int j, i, previ;
COORD d;
for (i = start; i < V; i++) {
previ = prevPt[i];
d = dist (pts [i], pts [previ]);
wadj[i][previ] = d;
wadj[previ][i] = d;
if (previ == i-1) j = i-2;
else j = i-1;
for (; j >= 0; j--) {
if (inCone(i,j,pts,nextPt,prevPt) &&
inCone(j,i,pts,nextPt,prevPt) &&
clear(pts[i],pts[j],V,V,V,pts,nextPt,prevPt)) {
d = dist (pts [i], pts [j]);
wadj[i][j] = d;
wadj[j][i] = d;
}
}
}
}
void visibility (vconfig_t *conf)
{
conf->vis = allocArray (conf->N, 2);
compVis(conf, 0);
}
static int polyhit(vconfig_t *conf, Ppoint_t p)
{
int i;
Ppoly_t poly;
for (i = 0; i < conf->Npoly; i++) {
poly.ps = &(conf->P[conf->start[i]]);
poly.pn = conf->start[i+1] - conf->start[i];
if (in_poly(poly, p))
return i;
}
return POLYID_NONE;
}
COORD* ptVis (vconfig_t *conf, int pp, Ppoint_t p)
{
int V = conf->N;
Ppoint_t* pts = conf->P;
int* nextPt = conf->next;
int* prevPt = conf->prev;
int k;
int start, end;
COORD* vadj;
Ppoint_t pk;
COORD d;
vadj = (COORD*)malloc((V+2)*sizeof(COORD));
if (pp == POLYID_UNKNOWN) pp = polyhit(conf,p);
if (pp >= 0) {
start = conf->start[pp];
end = conf->start[pp+1];
}
else {
start = V;
end = V;
}
for (k = 0; k < start; k++) {
pk = pts[k];
if (in_cone (pts[prevPt[k]],pk,pts[nextPt[k]],p) &&
clear(p,pk,start,end,V,pts,nextPt,prevPt)) {
d = dist (p, pk);
vadj[k] = d;
}
else vadj[k] = 0;
}
for (k = start; k < end; k++)
vadj[k] = 0;
for (k = end; k < V; k++) {
pk = pts[k];
if (in_cone (pts[prevPt[k]],pk,pts[nextPt[k]],p) &&
clear(p,pk,start,end,V,pts,nextPt,prevPt)) {
d = dist (p, pk);
vadj[k] = d;
}
else vadj[k] = 0;
}
vadj[V] = 0;
vadj[V+1] = 0;
return vadj;
}
int directVis (Ppoint_t p, int pp, Ppoint_t q, int qp, vconfig_t *conf)
{
int V = conf->N;
Ppoint_t* pts = conf->P;
int* nextPt = conf->next;
int k;
int s1, e1;
int s2, e2;
if (pp < 0) {
s1 = 0;
e1 = 0;
if (qp < 0) {
s2 = 0;
e2 = 0;
}
else {
s2 = conf->start[qp];
e2 = conf->start[qp+1];
}
}
else if (qp < 0) {
s1 = 0;
e1 = 0;
s2 = conf->start[pp];
e2 = conf->start[pp+1];
}
else if (pp <= qp) {
s1 = conf->start[pp];
e1 = conf->start[pp+1];
s2 = conf->start[qp];
e2 = conf->start[qp+1];
}
else {
s1 = conf->start[qp];
e1 = conf->start[qp+1];
s2 = conf->start[pp];
e2 = conf->start[pp+1];
}
for (k=0; k < s1; k++) {
if (INTERSECT(p,q,pts [k], pts[nextPt [k]], pts[prevPt [k]]))
return 0;
}
for (k=e1; k < s2; k++) {
if (INTERSECT(p,q,pts [k], pts[nextPt [k]], pts[prevPt [k]]))
return 0;
}
for (k=e2; k < V; k++) {
if (INTERSECT(p,q,pts [k], pts[nextPt [k]], pts[prevPt [k]]))
return 0;
}
return 1;
}