164 |
|
insert_dsf(RBFNODE *newrbf) |
165 |
|
{ |
166 |
|
RBFNODE *rbf, *rbf_last; |
167 |
+ |
/* check for redundant meas. */ |
168 |
+ |
for (rbf = dsf_list; rbf != NULL; rbf = rbf->next) |
169 |
+ |
if (DOT(rbf->invec, newrbf->invec) >= 1.-FTINY) { |
170 |
+ |
fputs("Duplicate incident measurement (ignored)\n", stderr); |
171 |
+ |
free(newrbf); |
172 |
+ |
return; |
173 |
+ |
} |
174 |
|
/* keep in ascending theta order */ |
175 |
|
for (rbf_last = NULL, rbf = dsf_list; |
176 |
|
single_plane_incident & (rbf != NULL); |
568 |
|
{ |
569 |
|
static double *src_cost = NULL; |
570 |
|
int n_alloc = 0; |
571 |
< |
const double maxamt = 2./(mtx_nrows(mig)*mtx_ncols(mig)); |
571 |
> |
const double maxamt = .1; /* 2./(mtx_nrows(mig)*mtx_ncols(mig)); */ |
572 |
|
double amt = 0; |
573 |
|
struct { |
574 |
|
int s, d; /* source and destination */ |
630 |
|
return(best.amt); |
631 |
|
} |
632 |
|
|
633 |
+ |
#ifdef DEBUG |
634 |
+ |
static char * |
635 |
+ |
thetaphi(const FVECT v) |
636 |
+ |
{ |
637 |
+ |
static char buf[128]; |
638 |
+ |
double theta, phi; |
639 |
+ |
|
640 |
+ |
theta = 180./M_PI*acos(v[2]); |
641 |
+ |
phi = 180./M_PI*atan2(v[1],v[0]); |
642 |
+ |
sprintf(buf, "(%.0f,%.0f)", theta, phi); |
643 |
+ |
|
644 |
+ |
return(buf); |
645 |
+ |
} |
646 |
+ |
#endif |
647 |
+ |
|
648 |
|
/* Compute (and insert) migration along directed edge */ |
649 |
|
static MIGRATION * |
650 |
< |
make_migration(RBFNODE *from_rbf, RBFNODE *to_rbf) |
650 |
> |
make_migration(RBFNODE *from_rbf, RBFNODE *to_rbf, int creat_only) |
651 |
|
{ |
652 |
|
const double end_thresh = 0.02/(from_rbf->nrbf*to_rbf->nrbf); |
653 |
< |
float *pmtx = price_routes(from_rbf, to_rbf); |
654 |
< |
MIGRATION *newmig = (MIGRATION *)malloc(sizeof(MIGRATION) + |
655 |
< |
sizeof(float) * |
634 |
< |
(from_rbf->nrbf*to_rbf->nrbf - 1)); |
635 |
< |
double *src_rem = (double *)malloc(sizeof(double)*from_rbf->nrbf); |
636 |
< |
double *dst_rem = (double *)malloc(sizeof(double)*to_rbf->nrbf); |
653 |
> |
float *pmtx; |
654 |
> |
MIGRATION *newmig; |
655 |
> |
double *src_rem, *dst_rem; |
656 |
|
double total_rem = 1.; |
657 |
|
int i; |
658 |
< |
|
658 |
> |
/* check if exists already */ |
659 |
> |
for (newmig = from_rbf->ejl; newmig != NULL; |
660 |
> |
newmig = nextedge(from_rbf,newmig)) |
661 |
> |
if (newmig->rbfv[1] == to_rbf) |
662 |
> |
return(creat_only ? (MIGRATION *)NULL : newmig); |
663 |
> |
/* else allocate */ |
664 |
> |
pmtx = price_routes(from_rbf, to_rbf); |
665 |
> |
newmig = (MIGRATION *)malloc(sizeof(MIGRATION) + sizeof(float) * |
666 |
> |
(from_rbf->nrbf*to_rbf->nrbf - 1)); |
667 |
> |
src_rem = (double *)malloc(sizeof(double)*from_rbf->nrbf); |
668 |
> |
dst_rem = (double *)malloc(sizeof(double)*to_rbf->nrbf); |
669 |
|
if ((newmig == NULL) | (src_rem == NULL) | (dst_rem == NULL)) { |
670 |
|
fputs("Out of memory in make_migration()\n", stderr); |
671 |
|
exit(1); |
672 |
|
} |
673 |
|
#ifdef DEBUG |
674 |
|
{ |
675 |
< |
double theta, phi; |
676 |
< |
theta = acos(from_rbf->invec[2])*(180./M_PI); |
677 |
< |
phi = atan2(from_rbf->invec[1],from_rbf->invec[0])*(180./M_PI); |
649 |
< |
fprintf(stderr, "Building path from (theta,phi) (%d,%d) to ", |
650 |
< |
round(theta), round(phi)); |
651 |
< |
theta = acos(to_rbf->invec[2])*(180./M_PI); |
652 |
< |
phi = atan2(to_rbf->invec[1],to_rbf->invec[0])*(180./M_PI); |
653 |
< |
fprintf(stderr, "(%d,%d)", round(theta), round(phi)); |
675 |
> |
fprintf(stderr, "Building path from (theta,phi) %s ", |
676 |
> |
thetaphi(from_rbf->invec)); |
677 |
> |
fprintf(stderr, "to %s", thetaphi(to_rbf->invec)); |
678 |
|
} |
679 |
|
#endif |
680 |
|
newmig->next = NULL; |
691 |
|
while (total_rem > end_thresh) { |
692 |
|
total_rem -= migration_step(newmig, src_rem, dst_rem, pmtx); |
693 |
|
#ifdef DEBUG |
694 |
< |
fputc('.', stderr); |
695 |
< |
/*XXX*/break; |
694 |
> |
/* fputc('.', stderr); */ |
695 |
> |
fprintf(stderr, "\n%.9f remaining...", total_rem); |
696 |
|
#endif |
697 |
|
} |
698 |
|
#ifdef DEBUG |
797 |
|
static RBFNODE * |
798 |
|
find_chull_vert(const RBFNODE *rbf0, const RBFNODE *rbf1) |
799 |
|
{ |
800 |
< |
FVECT vmid, vor; |
800 |
> |
FVECT vmid, vejn, vp; |
801 |
|
RBFNODE *rbf, *rbfbest = NULL; |
802 |
< |
double dprod2, area2, bestarea2 = FHUGE, bestdprod2 = 0.5; |
802 |
> |
double dprod, area2, bestarea2 = FHUGE, bestdprod = -.5; |
803 |
|
|
804 |
+ |
VSUB(vejn, rbf1->invec, rbf0->invec); |
805 |
|
VADD(vmid, rbf0->invec, rbf1->invec); |
806 |
< |
if (normalize(vmid) == 0) |
806 |
> |
if (normalize(vejn) == 0 || normalize(vmid) == 0) |
807 |
|
return(NULL); |
808 |
|
/* XXX exhaustive search */ |
809 |
|
for (rbf = dsf_list; rbf != NULL; rbf = rbf->next) { |
810 |
|
if ((rbf == rbf0) | (rbf == rbf1)) |
811 |
|
continue; |
812 |
< |
tri_orient(vor, rbf0->invec, rbf1->invec, rbf->invec); |
813 |
< |
dprod2 = DOT(vor, vmid); |
789 |
< |
if (dprod2 <= FTINY) |
812 |
> |
tri_orient(vp, rbf0->invec, rbf1->invec, rbf->invec); |
813 |
> |
if (DOT(vp, vmid) <= FTINY) |
814 |
|
continue; /* wrong orientation */ |
815 |
< |
area2 = DOT(vor, vor); |
816 |
< |
dprod2 *= dprod2 / area2; |
817 |
< |
if (dprod2 > bestdprod2 + |
818 |
< |
FTINY*(1 - 2*(area2 < bestarea2)) && |
819 |
< |
!overlaps_tri(rbf0, rbf1, rbf)) { |
820 |
< |
rbfbest = rbf; |
821 |
< |
bestdprod2 = dprod2; |
822 |
< |
bestarea2 = area2; |
823 |
< |
} |
815 |
> |
area2 = DOT(vp,vp); |
816 |
> |
VSUB(vp, rbf->invec, rbf0->invec); |
817 |
> |
dprod = -DOT(vp, vejn); |
818 |
> |
VSUM(vp, vp, vejn, dprod); |
819 |
> |
dprod = DOT(vp, vmid) / VLEN(vp); |
820 |
> |
if (dprod <= bestdprod + FTINY*(1 - 2*(area2 < bestarea2))) |
821 |
> |
continue; /* found better already */ |
822 |
> |
if (overlaps_tri(rbf0, rbf1, rbf)) |
823 |
> |
continue; /* overlaps another triangle */ |
824 |
> |
rbfbest = rbf; |
825 |
> |
bestdprod = dprod; /* new one to beat */ |
826 |
> |
bestarea2 = area2; |
827 |
|
} |
828 |
|
return(rbfbest); |
829 |
|
} |
834 |
|
{ |
835 |
|
MIGRATION *ej0, *ej1; |
836 |
|
RBFNODE *tvert[2]; |
837 |
+ |
|
838 |
+ |
if (edge == NULL) |
839 |
+ |
return; |
840 |
|
/* triangle on either side? */ |
841 |
|
get_triangles(tvert, edge); |
842 |
|
if (tvert[0] == NULL) { /* grow mesh on right */ |
843 |
|
tvert[0] = find_chull_vert(edge->rbfv[0], edge->rbfv[1]); |
844 |
|
if (tvert[0] != NULL) { |
845 |
|
if (tvert[0] > edge->rbfv[0]) |
846 |
< |
ej0 = make_migration(edge->rbfv[0], tvert[0]); |
846 |
> |
ej0 = make_migration(edge->rbfv[0], tvert[0], 1); |
847 |
|
else |
848 |
< |
ej0 = make_migration(tvert[0], edge->rbfv[0]); |
848 |
> |
ej0 = make_migration(tvert[0], edge->rbfv[0], 1); |
849 |
|
if (tvert[0] > edge->rbfv[1]) |
850 |
< |
ej1 = make_migration(edge->rbfv[1], tvert[0]); |
850 |
> |
ej1 = make_migration(edge->rbfv[1], tvert[0], 1); |
851 |
|
else |
852 |
< |
ej1 = make_migration(tvert[0], edge->rbfv[1]); |
852 |
> |
ej1 = make_migration(tvert[0], edge->rbfv[1], 1); |
853 |
|
mesh_from_edge(ej0); |
854 |
|
mesh_from_edge(ej1); |
855 |
|
} |
856 |
< |
} |
827 |
< |
if (tvert[1] == NULL) { /* grow mesh on left */ |
856 |
> |
} else if (tvert[1] == NULL) { /* grow mesh on left */ |
857 |
|
tvert[1] = find_chull_vert(edge->rbfv[1], edge->rbfv[0]); |
858 |
|
if (tvert[1] != NULL) { |
859 |
|
if (tvert[1] > edge->rbfv[0]) |
860 |
< |
ej0 = make_migration(edge->rbfv[0], tvert[1]); |
860 |
> |
ej0 = make_migration(edge->rbfv[0], tvert[1], 1); |
861 |
|
else |
862 |
< |
ej0 = make_migration(tvert[1], edge->rbfv[0]); |
862 |
> |
ej0 = make_migration(tvert[1], edge->rbfv[0], 1); |
863 |
|
if (tvert[1] > edge->rbfv[1]) |
864 |
< |
ej1 = make_migration(edge->rbfv[1], tvert[1]); |
864 |
> |
ej1 = make_migration(edge->rbfv[1], tvert[1], 1); |
865 |
|
else |
866 |
< |
ej1 = make_migration(tvert[1], edge->rbfv[1]); |
866 |
> |
ej1 = make_migration(tvert[1], edge->rbfv[1], 1); |
867 |
|
mesh_from_edge(ej0); |
868 |
|
mesh_from_edge(ej1); |
869 |
|
} |
974 |
|
if (single_plane_incident) { |
975 |
|
for (rbf0 = dsf_list; rbf0 != NULL; rbf0 = rbf0->next) |
976 |
|
if (rbf0->next != NULL) |
977 |
< |
make_migration(rbf0, rbf0->next); |
977 |
> |
make_migration(rbf0, rbf0->next, 1); |
978 |
|
return; |
979 |
|
} |
980 |
|
/* start w/ shortest edge */ |
994 |
|
} |
995 |
|
/* build mesh from this edge */ |
996 |
|
if (shrt_edj[0] < shrt_edj[1]) |
997 |
< |
mesh_from_edge(make_migration(shrt_edj[0], shrt_edj[1])); |
997 |
> |
mesh_from_edge(make_migration(shrt_edj[0], shrt_edj[1], 0)); |
998 |
|
else |
999 |
< |
mesh_from_edge(make_migration(shrt_edj[1], shrt_edj[0])); |
999 |
> |
mesh_from_edge(make_migration(shrt_edj[1], shrt_edj[0], 0)); |
1000 |
|
/* draw edge list into grid */ |
1001 |
|
draw_edges(); |
1002 |
|
} |