inline int in_image(T const& x, T const& y, int proj, int north_square, int south_square)
{
static const T pi = detail::pi<T>();
static const T half_pi = detail::half_pi<T>();
static const T fourth_pi = detail::fourth_pi<T>();
if (proj == 0) {
T healpixVertsJit[][2] = {
{-pi - epsilon, fourth_pi},
{-3.0*fourth_pi, half_pi + epsilon},
{-half_pi, fourth_pi + epsilon},
{-fourth_pi, half_pi + epsilon},
{0.0, fourth_pi + epsilon},
{fourth_pi, half_pi + epsilon},
{half_pi, fourth_pi + epsilon},
{3.0*fourth_pi, half_pi + epsilon},
{pi + epsilon, fourth_pi},
{pi + epsilon, -fourth_pi},
{3.0*fourth_pi, -half_pi - epsilon},
{half_pi, -fourth_pi - epsilon},
{fourth_pi, -half_pi - epsilon},
{0.0, -fourth_pi - epsilon},
{-fourth_pi, -half_pi - epsilon},
{-half_pi, -fourth_pi - epsilon},
{-3.0*fourth_pi, -half_pi - epsilon},
{-pi - epsilon, -fourth_pi}
};
return pnpoly((int)sizeof(healpixVertsJit)/
sizeof(healpixVertsJit[0]), healpixVertsJit, x, y);
} else {
T rhealpixVertsJit[][2] = {
{-pi - epsilon, fourth_pi + epsilon},
{-pi + north_square*half_pi - epsilon, fourth_pi + epsilon},
{-pi + north_square*half_pi - epsilon, 3.0*fourth_pi + epsilon},
{-pi + (north_square + 1.0)*half_pi + epsilon, 3.0*fourth_pi + epsilon},
{-pi + (north_square + 1.0)*half_pi + epsilon, fourth_pi + epsilon},
{pi + epsilon, fourth_pi + epsilon},
{pi + epsilon, -fourth_pi - epsilon},
{-pi + (south_square + 1.0)*half_pi + epsilon, -fourth_pi - epsilon},
{-pi + (south_square + 1.0)*half_pi + epsilon, -3.0*fourth_pi - epsilon},
{-pi + south_square*half_pi - epsilon, -3.0*fourth_pi - epsilon},
{-pi + south_square*half_pi - epsilon, -fourth_pi - epsilon},
{-pi - epsilon, -fourth_pi - epsilon}
};
return pnpoly((int)sizeof(rhealpixVertsJit)/
sizeof(rhealpixVertsJit[0]), rhealpixVertsJit, x, y);
}
}
bool Cylinder::xiM(float i, float j, vector <point2d> const & convex)
{
double distanceSegment;
if (dss(i,j) > R*R)
{
return false;
}
else if (pnpoly(convex,i,j))
{
return true;
}
else
{
unsigned int last = convex.size() - 1;
distanceSegment = DistanceFromSegment(i,j,convex[last].x,convex[last].y,convex[0].x,convex[0].y);
if (distanceSegment <= OMEGA)
return true;
for (unsigned int k=0; k<last; ++k)
{
distanceSegment = DistanceFromSegment(i,j,convex[k].x,convex[k].y,convex[k+1].x,convex[k+1].y);
if (distanceSegment <= OMEGA)
return true;
}
return false;
}
}
void Dialog::update_bgr_table()
{
QVector< QPoint> pts;
QVector<QPoint> third_color;
pts.resize(3);
third_color.resize(3);
int idx=0;
for(std::size_t b =0 ;b < 64 ;b++)
for(std::size_t g=0 ;g < 64 ;g++)
for(std::size_t r=0 ;r < 64 ;r++)
{
cv::Vec3b color_bgr=color_space_bgr_[0].at<cv::Vec3b>(0,idx);
cv::Vec3b color_hsi=color_space_bgr_[1].at<cv::Vec3b>(0,idx);
cv::Vec3b color_yuv=color_space_bgr_[2].at<cv::Vec3b>(0,idx);
pts[1]=QPoint(color_hsi[0],color_hsi[1]); //H,S
pts[2]=QPoint(color_yuv[1],color_yuv[2]); //U V
third_color[1]=QPoint(color_hsi[2],0); //I
third_color[2]=QPoint(color_yuv[0],0); //Y
CTable_bgr_[idx]=3;
for(int k = 2 ;k >=0 ;k--)
{
bool is_inpoly = pnpoly(calib_final_result_[k],pts[calib_color[k]]);
bool is_line = pnline(calib_Line_final_result_[k],third_color[calib_color[k]]);
if(is_inpoly&&is_line)
CTable_bgr_[idx]=k;
}
idx++;
}
}
VALUE point_in_poly(VALUE self, VALUE rb_x, VALUE rb_y, VALUE rb_points) {
Check_Type(rb_x, T_FLOAT);
Check_Type(rb_y, T_FLOAT);
Check_Type(rb_points, T_ARRAY);
int i = 0;
int s = (int)RARRAY_LEN(rb_points);
double *vertx;
double *verty;
vertx = calloc(s, sizeof(double));
verty = calloc(s, sizeof(double));
double test_x = RFLOAT_VALUE(rb_x);
double test_y = RFLOAT_VALUE(rb_y);
ID i_x = rb_intern("x");
ID i_y = rb_intern("y");
for(i = 0; i < s; i++) {
VALUE point = RARRAY_PTR(rb_points)[i];
VALUE x = rb_funcall(point, i_x, 0);
VALUE y = rb_funcall(point, i_y, 0);
Check_Type(x, T_FLOAT);
Check_Type(y, T_FLOAT);
vertx[i] = RFLOAT_VALUE(x);
verty[i] = RFLOAT_VALUE(y);
}
return pnpoly(s, vertx, verty, test_x, test_y);
}
/**
* Return 1 if (x, y) lies in (the interior or boundary of) the image of the
* HEALPix projection (in case proj=0) or in the image the rHEALPix projection
* (in case proj=1), and return 0 otherwise.
* @param north_square the position of the north polar square (rHEALPix only)
* @param south_square the position of the south polar square (rHEALPix only)
**/
int in_image(double x, double y, int proj, int north_square, int south_square) {
if (proj == 0) {
double healpixVertsJit[][2] = {
{-1.0*M_PI- EPS, M_PI/4.0},
{-3.0*M_PI/4.0, M_PI/2.0 + EPS},
{-1.0*M_PI/2.0, M_PI/4.0 + EPS},
{-1.0*M_PI/4.0, M_PI/2.0 + EPS},
{0.0, M_PI/4.0 + EPS},
{M_PI/4.0, M_PI/2.0 + EPS},
{M_PI/2.0, M_PI/4.0 + EPS},
{3.0*M_PI/4.0, M_PI/2.0 + EPS},
{M_PI+ EPS, M_PI/4.0},
{M_PI+ EPS, -1.0*M_PI/4.0},
{3.0*M_PI/4.0, -1.0*M_PI/2.0 - EPS},
{M_PI/2.0, -1.0*M_PI/4.0 - EPS},
{M_PI/4.0, -1.0*M_PI/2.0 - EPS},
{0.0, -1.0*M_PI/4.0 - EPS},
{-1.0*M_PI/4.0, -1.0*M_PI/2.0 - EPS},
{-1.0*M_PI/2.0, -1.0*M_PI/4.0 - EPS},
{-3.0*M_PI/4.0, -1.0*M_PI/2.0 - EPS},
{-1.0*M_PI - EPS, -1.0*M_PI/4.0}
};
return pnpoly((int)sizeof(healpixVertsJit)/
sizeof(healpixVertsJit[0]), healpixVertsJit, x, y);
} else {
double rhealpixVertsJit[][2] = {
{-1.0*M_PI - EPS, M_PI/4.0 + EPS},
{-1.0*M_PI + north_square*M_PI/2.0- EPS, M_PI/4.0 + EPS},
{-1.0*M_PI + north_square*M_PI/2.0- EPS, 3*M_PI/4.0 + EPS},
{-1.0*M_PI + (north_square + 1.0)*M_PI/2.0 + EPS, 3*M_PI/4.0 + EPS},
{-1.0*M_PI + (north_square + 1.0)*M_PI/2.0 + EPS, M_PI/4.0 + EPS},
{M_PI + EPS, M_PI/4.0 + EPS},
{M_PI + EPS, -1.0*M_PI/4.0 - EPS},
{-1.0*M_PI + (south_square + 1.0)*M_PI/2.0 + EPS, -1.0*M_PI/4.0 - EPS},
{-1.0*M_PI + (south_square + 1.0)*M_PI/2.0 + EPS, -3.0*M_PI/4.0 - EPS},
{-1.0*M_PI + south_square*M_PI/2.0 - EPS, -3.0*M_PI/4.0 - EPS},
{-1.0*M_PI + south_square*M_PI/2.0 - EPS, -1.0*M_PI/4.0 - EPS},
{-1.0*M_PI - EPS, -1.0*M_PI/4.0 - EPS}};
return pnpoly((int)sizeof(rhealpixVertsJit)/
sizeof(rhealpixVertsJit[0]), rhealpixVertsJit, x, y);
}
}
bool Utility::boxContainsPoint(BoundingBox box, int x, int y) {
float xCoords[] = {box.corners[0].x,
box.corners[1].x,
box.corners[2].x,
box.corners[3].x};
float yCoords[] = {box.corners[0].y,
box.corners[1].y,
box.corners[2].y,
box.corners[3].y};
// pnpoly returns 1 iff point is within the polygon described
return static_cast<bool>(pnpoly(4, xCoords, yCoords, x, y));
}
/*! @brief show the segment result */
void
Dialog::on_segment_result_btn_clicked()
{
update_calibration_result();
/*! poly_pts_ is the color segment threa*/
cv::Mat image_tmp=orignal_img_[0].clone();
QVector< QPoint> pts;
QVector<QPoint> third_color;
pts.resize(3);
third_color.resize(3);
std::vector<cv::Vec3b> show_color;
show_color.push_back(cv::Vec3b(255,69,0));
show_color.push_back(cv::Vec3b(138,43,226));
show_color.push_back(cv::Vec3b(0,100,0));
for(std::size_t i =0 ; i < orignal_img_[Img_selected_].rows; i++ )
{
for(std::size_t j= 0 ; j < orignal_img_[Img_selected_].cols; j++)
{
cv::Vec3b color_rgb=orignal_img_[0].at<cv::Vec3b>(i,j);
cv::Vec3b color_hsi=orignal_img_[1].at<cv::Vec3b>(i,j);
cv::Vec3b color_yuv=orignal_img_[2].at<cv::Vec3b>(i,j);
double dis_center=std::sqrt((circle_center_.x()-j)*(circle_center_.x()-j)+
(circle_center_.y()-i)*(circle_center_.y()-i));
if(dis_center<circle_radius_)
{
pts[1]=QPoint(color_hsi[0],color_hsi[1]); //H,S
pts[2]=QPoint(color_yuv[1],color_yuv[2]); //U V
third_color[1]=QPoint(color_hsi[2],0); //I
third_color[2]=QPoint(color_yuv[0],0); //Y
for(int k = 2; k >=0 ;k--)
{
bool is_inpoly = pnpoly(calib_final_result_[k],pts[calib_color[k]]);
bool is_line = pnline(calib_Line_final_result_[k],third_color[calib_color[k]]);
if(is_inpoly&&is_line)
image_tmp.at<cv::Vec3b>(i,j)=show_color[k];
}
}
}
}
image[0] = QImage((const unsigned char*)(image_tmp.data),
image_tmp.cols,image_tmp.rows,
image_tmp.cols*image_tmp.channels(),
QImage::Format_RGB888);
bufferImg_[0] = image[0];
paint(bufferImg_[0]);
update_yuv_table();
update_bgr_table();
}
int main(int argc, char* argv[])
{
int i1, n1, i2, n2, nv, two, *trace;
float o1, o2, d1, d2, x, y, **vert;
sf_file inp, out, poly;
sf_init(argc,argv);
inp = sf_input("in");
out = sf_output("out");
if (!sf_histint(inp,"n1",&n1)) sf_error("No n1= in input");
if (!sf_histint(inp,"n2",&n2)) sf_error("No n2= in input");
if (!sf_histfloat(inp,"d1",&d1)) d1=1.;
if (!sf_histfloat(inp,"d2",&d2)) d2=1.;
if (!sf_histfloat(inp,"o1",&o1)) o1=0.;
if (!sf_histfloat(inp,"o2",&o2)) o2=0.;
poly = sf_input("poly");
/* list of polygon vertices */
if (SF_FLOAT != sf_gettype(poly)) sf_error("Need float type in poly");
if (!sf_histint(poly,"n1",&two) || 2 != two)
sf_error("Need n1=2 in poly");
if (!sf_histint(poly,"n2",&nv))
sf_error("No n2= in poly");
trace = sf_intalloc(n1);
vert = sf_floatalloc2(2,nv);
sf_floatread(vert[0],2*nv,poly);
sf_settype(out,SF_INT);
for (i2=0; i2 < n2; i2++) {
y = o2+i2*d2;
for (i1=0; i1 < n1; i1++) {
x = o1+i1*d1;
trace[i1] = pnpoly(nv, vert, x, y);
}
sf_intwrite(trace,n1,out);
}
exit(0);
}
TRIANGLE_MATRIX * findTriangle_MATRIX(const MESH_MATRIX * mesh_matrix_ptr, const MESH * mMesh, const VERTEX2D point){
TRIANGLE * meshTrianglePtr = mMesh->pTriArr;
for(int i = 0; i < mMesh->triangle_num; i++){
float vertx[3];
float verty[3];
for(int i = 0; i < 3; i++){
vertx[i] = mMesh->pVerArr[meshTrianglePtr->i1].x;
verty[i] = mMesh->pVerArr[meshTrianglePtr->i1].y;
}
if(!(pnpoly(3, vertx, verty, point.x, point.y) % 2)){
return &(mesh_matrix_ptr->triangle_matrix[i]);
}else{
meshTrianglePtr = meshTrianglePtr->pNext;
}
}
return NULL;
}
/* return value is the number of points left. */
int get_triangle(double x[], double y[], int pts, int n) {
double xa, xb, xc, ya, yb, yc, xmid, ymid;
int j;
/* Get three adjacent points on the polygon */
xa = x[n % pts]; ya = y[n % pts];
xb = x[(n + 1) % pts]; yb = y[(n + 1) % pts];
xc = x[(n + 2) % pts]; yc = y[(n + 2) % pts];
#ifdef DEBUG
printf("Testing points (%f, %f) (%f, %f) (%f, %f)\n",
n, xa, ya, xb, yb, xc, yc);
printf("n = %d, pts = %d\n", n, pts);
#endif
/* See if line segment ac intersects any lines on the polygon */
for(j = 0; j < pts - 1; j++) {
if(intersect(xa, ya, xc, yc, x[j], y[j], x[j+1], y[j+1])) {
return pts;
}
}
/* Find the midpoint of line ac */
xmid = (xa + xc) / 2; ymid = (ya + yc) / 2;
/* Test to see the midpoint is inside the polygon */
if(!pnpoly(pts, x, y, xmid, ymid)) {
return pts;
}
/* Print out the coordinates of the triangle.
* If we are going to do something with the triangle, we should
* do it here.
*/
printf("Triangle: (%f, %f) (%f, %f) (%f, %f)\n",
xa, ya, xb, yb, xc, yc);
/* Delete point xb, yb from the polygon */
delval(x, (n + 1) % pts, pts);
delval(y, (n + 1) % pts, pts);
return pts - 1;
}
//get the average color for a poly within a region
//for some definition of average ...
ofColor
getAverageColor(ofImage i, ofRectangle bounds, int nverts, float *vertx, float * verty)
{
float r, g, b;
r = g = b = 0;
float ncols = 0;
for (int x = bounds.x; x <= bounds.width; x++) {
for (int y = bounds.y; y <= bounds.height; y++) {
if (pnpoly(nverts, vertx, verty, x, y)) {
ofColor c = i.getColor(x, y);
r += c.r;
g += c.g;
b += c.b;
ncols += 1.0;
}
}
}
return(ofColor(r/ncols, g/ncols, b/ncols));
}
//pt ds frm
int point_dans_forme(Forme* forme,float x,float y){
//printf("point analyse : (%d,%d)\n",x,y);
//construire le point cherché
Point point={x,y,NULL};
//verifier d'abord les frontières
Point* liste_point=forme->points;
Point* pact=liste_point;
Point* psuiv=point_suivant(liste_point,pact);
while(pact!=NULL){
if(point_dans_segment(point,*pact,*psuiv)){
//puts("point sur un segment");
return 1;
}
psuiv=point_suivant(liste_point,psuiv);
pact=pact->suivant;
}
//puts("point n'est pas dans la frontire");
//maintenat, n'est pas dans la frontière, verifier l'intérieur
int r= pnpoly(forme,x,y);
//if(r) puts("et il'est a l'interieure");
//else puts("il est a l'exterieure");
return r;
}
void plot_boundary(){
// TH2F *h1 = new TH2F("h1","h1",210,50,260,20,-116,-96);
// h1->Draw();
// TLine *l1 = new TLine(77,-113,253,-96);
// l1->Draw();
std::vector<double> vertx = {3,77,253,253,97,3};
std::vector<double> verty = {-113,-113,-96,100,115,114};
TH2F *h1 = new TH2F("h1","h1",256,0,256,240,-120,120);
for (int i=0;i!=256;i++){
double x = h1->GetXaxis()->GetBinCenter(i+1);
for (int j=0;j!=240;j++){
double y = h1->GetYaxis()->GetBinCenter(j+1);
h1->SetBinContent(i+1,j+1,pnpoly(vertx,verty,x,y));
}
}
h1->Draw("COLZ");
// double x = 166.627;
// double y = -104.599;
//std::cout << pnpoly(vertx,verty,x,y) << std::endl;
// TGraph *g1 = new TGraph();
// for (int i=0;i!=240;i++){
// double y = -120+i;
// double c = pnpoly(vertx,verty,x,y);
// g1->SetPoint(i,y,c);
// }
// g1->Draw("A*");
// Double_t x[7]={3,77,253,253,97,3,3};
// Double_t y[7]={-113,-113,-96,100,115,114,-113};
//TGraph *g1 = new TGraph(7,x,y);
//g1->Draw("AL");
}
bool paths::pnpoly(const path::vertex_type& point) const
{
return pnpoly(point.x, point.y);
}
bool SegmentListOLCI::TestForSegmentGLerr(int x, int realy, float distance, const QMatrix4x4 &m, bool showallsegments, QString &segmentname)
{
bool isselected = false;
qDebug() << QString("Nbr of segments = %1").arg(segmentlist.count());
QList<Segment*>::iterator segit = segmentlist.begin();
QVector2D winvec1, winvec2, winvecend1, winvecend2, winvecend3, winvecend4;
QVector3D vecZ = m.row(2).toVector3D();
segmentname = "";
while ( segit != segmentlist.end() )
{
if(showallsegments ? true : (*segit)->segmentshow)
{
for(int i = 0; i < (*segit)->winvectorfirst.length()-1; i++)
{
winvecend1.setX((*segit)->winvectorfirst.at(i).x()); winvecend1.setY((*segit)->winvectorfirst.at(i).y());
winvecend2.setX((*segit)->winvectorfirst.at(i+1).x()); winvecend2.setY((*segit)->winvectorfirst.at(i+1).y());
winvecend3.setX((*segit)->winvectorlast.at(i).x()); winvecend3.setY((*segit)->winvectorlast.at(i).y());
winvecend4.setX((*segit)->winvectorlast.at(i+1).x()); winvecend4.setY((*segit)->winvectorlast.at(i+1).y());
// first last
// winvecend1 ------------------------------------ winvecend3
// | p01 | p03
// | |
// | |
// | |
// | |
// | |
// | p02 | p04
// winvecend2 ------------------------------------ winvecend4
//
qreal angle = ArcCos(QVector3D::dotProduct( vecZ, (*segit)->vecvector.at(i)));
//qDebug() << QString("angle = %1").arg(angle * 180.0 / PI);
if (angle < PI/2 + (asin(1/distance)))
{
struct point p01;
p01.x = (int)winvecend1.x();
p01.y = (int)winvecend1.y();
struct point p02;
p02.x = (int)winvecend2.x();
p02.y = (int)winvecend2.y();
struct point p03;
p03.x = (int)winvecend3.x();
p03.y = (int)winvecend3.y();
struct point p04;
p04.x = (int)winvecend4.x();
p04.y = (int)winvecend4.y();
QPoint points[4] = {
QPoint(p01.x, p01.y),
QPoint(p02.x, p02.y),
QPoint(p04.x, p04.y),
QPoint(p03.x, p03.y)
};
int result = pnpoly( 4, points, x, realy);
if (result)
{
if((*segit)->ToggleSelected())
{
qDebug() << QString("segment selected is = %1").arg((*segit)->fileInfo.fileName());
isselected = true;
segmentname = (*segit)->fileInfo.fileName();
qApp->processEvents();
}
break;
}
}
}
}
++segit;
}
return isselected;
}
int main()
{
int i,j,k;
double x,y,a1,a2,a3,a;
while(1) {
for(i=0;i<2;i++) {
scanf("%d",&n[i]);
if(n[0]==0) goto done;
for(j=0;j<n[i];j++) scanf("%d %d",&px[i][j],&py[i][j]);
px[i][j]=px[i][0]; py[i][j]=py[i][0];
}
np=0;
for(i=0;i<n[0];i++)
for(j=0;j<n[1];j++) {
/* check all lines in polygon 1 against all lines in polygon 2 for
* intersections */
k=linesintersect(px[0][i],py[0][i],px[0][i+1],py[0][i+1],
px[1][j],py[1][j],px[1][j+1],py[1][j+1],&x,&y);
if(k==1) {
p[np].x=x; p[np++].y=y;
} else if(k==2) {
s[0].x=px[0][i]; s[0].y=py[0][i];
s[1].x=px[0][i+1]; s[1].y=py[0][i+1];
s[2].x=px[1][j]; s[2].y=py[1][j];
s[3].x=px[1][j+1]; s[3].y=py[1][j+1];
qsort(s,2,sizeof(s[0]),comppi);
qsort(s+2,2,sizeof(s[0]),comppi);
if(comppi(&s[1],&s[2])==1) {
if(comppi(&s[0],&s[2])==1) {
p[np].x=s[0].x; p[np++].y=s[0].y;
} else {
p[np].x=s[2].x; p[np++].y=s[2].y;
}
if(comppi(&s[1],&s[2])!=0) {
if(comppi(&s[1],&s[3])==-1) {
p[np].x=s[1].x; p[np++].y=s[1].y;
} else {
p[np].x=s[3].x; p[np++].y=s[3].y;
}
}
}
}
}
/* check if point from polygon 1 is within polygon 2 */
for(i=0;i<n[0];i++)
if(pnpoly(n[1],px[1],py[1],px[0][i],py[0][i])) {
p[np].x=px[0][i]; p[np++].y=py[0][i];
}
/* check if point from polygon 2 is within polygon 1 */
for(j=0;j<n[1];j++)
if(pnpoly(n[0],px[0],py[0],px[1][j],py[1][j])) {
p[np].x=px[1][j]; p[np++].y=py[1][j];
}
/* printf("%d: ",np);
for(i=0;i<np;i++) printf("(%.1f %.1f) ",p[i].x,p[i].y);
printf("\n");*/
/* convex hull */
if(np>=3) {
nr=chainhull2d(p,np,r);
r[nr]=r[0];
} else
nr=0;
/* printf("%d: ",nr);
for(i=0;i<=nr;i++) printf("(%.1f %.1f) ",r[i].x,r[i].y);
printf("\n");*/
a1=fabs(areap(n[0],px[0],py[0]));
a2=fabs(areap(n[1],px[1],py[1]));
if(nr>=3) a3=fabs(aread(nr,r));
else a3=0;
a=(a1+a2)/2-a3;
printf("%8.2f",a);
}
done:
printf("\n");
}
//---------------------------
bool ofxCvCoordWarping::bInQuad(ofPoint pt){
return pnpoly(4, srcQuad, pt.x, pt.y);
}
// vertex-vertex or vertex-edge collisions
int MyPhysics::CheckForCollision(_RigidBody *body1, _RigidBody *body2){
Vector d, vList1[4], vList2[4], v1, v2, u, edge, p, proj;
float r, wd, lg, s, dist, dot, Vrn;
int i, j, retval = 0;
bool interpenetrating = false;
bool haveNodeEdge = false;
// First check to see if the bounding circles are colliding
r = body1->fLength/2 + body2->fLength/2;
d = body1->vPosition - body2->vPosition;
s = d.Magnitude() - r;
if(s <= ctol)
{ // We have a possible collision, check further
// build vertex lists for each hovercraft
wd = body1->fWidth;
lg = body1->fLength;
vList1[0].y = body1->vFirstpoint.y; vList1[0].x = body1->vFirstpoint.x;
vList1[1].y = body1->vSecondpoint.y; vList1[1].x = body1->vSecondpoint.x;
vList1[2].y = body1->vThirdpoint.y; vList1[2].x = body1->vThirdpoint.x;
vList1[3].y = body1->vFourthpoint.y; vList1[3].x = body1->vFirstpoint.x;
for(i=0; i<4; i++)
{
/*v1 = VRotate2D(body1->fOrientation, vList1[i]);*/
vList1[i] += body1->vPosition;
}
wd = body2->fWidth;
lg = body2->fLength;
vList2[0].y = body2->vFirstpoint.y; vList2[0].x = body2->vFirstpoint.x;
vList2[1].y = body2->vSecondpoint.y; vList2[1].x = body2->vSecondpoint.x;
vList2[2].y = body2->vThirdpoint.y; vList2[2].x = body2->vThirdpoint.x;
vList2[3].y = body2->vFourthpoint.y; vList2[3].x = body2->vFirstpoint.x;
for(i=0; i<4; i++)
{
/*v2 = VRotate2D(body2->fOrientation, vList2[i]);*/
vList2[i] += body2->vPosition;
}
// Check for vertex-edge collision
for(i=0; i<4 && !haveNodeEdge; i++)
{
for(j=0; j<4 && !haveNodeEdge; j++)
{
if(j==3)
edge = vList2[j/*0*/] - vList2[0/*j*/];
else
edge = vList2[j+1] - vList2[j];
u = edge;
u.Normalize();
p = vList1[i] - vList2[j];
proj = (p * u) * u;
d = p^u;
dist = d.Magnitude();
dot = p * edge;
if(dot > 0)
dist = -dist; // point is on inside
vCollisionPoint = vList1[i];
body1->vCollisionPoint = vCollisionPoint - body1->vPosition;
body2->vCollisionPoint = vCollisionPoint - body2->vPosition;
vCollisionNormal = ((u^p)^u);
vCollisionNormal.Normalize();
v1 = body1->vVelocity + (body1->vAngularVelocity^body1->vCollisionPoint);
v2 = body2->vVelocity + (body2->vAngularVelocity^body2->vCollisionPoint);
vRelativeVelocity = (v1 - v2);
Vrn = vRelativeVelocity * vCollisionNormal;
vCollisionTangent = (vCollisionNormal^vRelativeVelocity)^vCollisionNormal;
vCollisionTangent.Normalize();
if( (proj.Magnitude() > tol) &&
(proj.Magnitude() <= edge.Magnitude()) &&
(dist <= ctol) &&
(Vrn < 0.0f)
)
{
haveNodeEdge = true;
CollisionBody1 = body1;
CollisionBody2 = body2;
}
}
}
for(i=0; i<4 && !haveNodeEdge; i++)
{
for(j=0; j<4 && !haveNodeEdge; j++)
{
if(j==3)
edge = vList1[j/*0*/] - vList1[0/*j*/];
else
edge = vList1[j+1] - vList1[j];
u = edge;
u.Normalize();
p = vList2[i] - vList1[j];
proj = (p * u) * u;
d = p^u;
dist = d.Magnitude();
dot = p * edge;
if(dot > 0)
dist = -dist; // point is on inside
vCollisionPoint = vList2[i];
body1->vCollisionPoint = vCollisionPoint - body1->vPosition;
body2->vCollisionPoint = vCollisionPoint - body2->vPosition;
vCollisionNormal = ((u^p)^u);
vCollisionNormal.Normalize();
v1 = body1->vVelocity + (body1->vAngularVelocity^body1->vCollisionPoint);
v2 = body2->vVelocity + (body2->vAngularVelocity^body2->vCollisionPoint);
vRelativeVelocity = (v1 - v2);
Vrn = vRelativeVelocity * vCollisionNormal;
vCollisionTangent = (vCollisionNormal^vRelativeVelocity)^vCollisionNormal;
vCollisionTangent.Normalize();
if( (proj.Magnitude() > tol) &&
(proj.Magnitude() <= edge.Magnitude()) &&
(dist <= ctol) &&
(Vrn < 0.0f)
)
{
haveNodeEdge = true;
CollisionBody1 = body2;
CollisionBody2 = body1;
}
}
}
if(!haveNodeEdge)
{
for(i=0; i<4 && !interpenetrating; i++)
{
if(pnpoly(4, vList2, vList1[i]) == 1)
interpenetrating = true;
if(pnpoly(4, vList1, vList2[i]) == 1)
interpenetrating = true;
}
}
if(interpenetrating)
retval = PENETRATING;
else if(haveNodeEdge)
retval = COLLISION;
else
retval = NOCOLLISION;
} else
{
retval = NOCOLLISION;
}
return retval;
}
World::World(){
drawMowedLawn = true;
memset(lawnMowStatus, 0, sizeof lawnMowStatus);
//printf("%d\n", sizeof bfield);
memset(bfield, 0, sizeof bfield);
imgBfield = Mat(WORLD_SIZE_Y, WORLD_SIZE_X, CV_8UC3, Scalar(0,0,0));
imgWorld = Mat(WORLD_SIZE_Y, WORLD_SIZE_X, CV_8UC3, Scalar(0,0,0));
// perimeter lines coordinates (1/10 meter)
std::vector<point_t> list;
list.push_back( (point_t) {30, 35 } );
list.push_back( (point_t) {50, 15 } );
list.push_back( (point_t) {400, 40 } );
list.push_back( (point_t) {410, 50 } );
list.push_back( (point_t) {420, 90 } );
list.push_back( (point_t) {350, 160 } );
list.push_back( (point_t) {320, 190 } );
list.push_back( (point_t) {210, 250 } );
list.push_back( (point_t) {40, 300 } );
list.push_back( (point_t) {20, 290 } );
list.push_back( (point_t) {30, 230 } );
chgStationX = 35;
chgStationY = 150;
// compute magnetic field (compute distance to perimeter lines)
int x1 = list[list.size()-1].x;
int y1 = list[list.size()-1].y;
// for each perimeter line
for (int i=0; i < list.size(); i++){
int x2 = list[i].x;
int y2 = list[i].y;
int dx = (x2-x1);
int dy = (y2-y1);
int len=(sqrt( dx*dx + dy*dy )); // line length
float phi = atan2(dy,dx); // line angle
// compute magnetic field for points (x,y) around perimeter line
for (int y=-200; y < 200; y++){
for (int x=-100; x < len*2+100-1; x++){
int px= x1 + cos(phi)*x/2 - sin(phi)*y;
int py= y1 + sin(phi)*x/2 + cos(phi)*y;
int xend = max(0, min(len, x/2)); // restrict to line ends
int cx = x1 + cos(phi)*xend; // cx on line
int cy = y1 + sin(phi)*xend; // cy on line
if ((py >= 0) && (py < WORLD_SIZE_Y)
&& (px >=0) && (px < WORLD_SIZE_X)) {
float r = max(0.000001, sqrt( (cx-px)*(cx-px) + (cy-py)*(cy-py) ) ) / 10; // distance to line (meter)
float b=100.0/(2.0*M_PI*r); // field strength
int c = pnpoly(list, px, py);
//if ((y<=0) || (bfield[py][px] < 0)){
if (c == 0){
b=b*-1.0;
bfield[py][px] = min(bfield[py][px], b);
} else bfield[py][px] = max(bfield[py][px], b);
}
}
}
x1=x2;
y1=y2;
}
// draw magnetic field onto image
for (int y=0; y < WORLD_SIZE_Y; y++){
for (int x=0; x < WORLD_SIZE_X; x++) {
float b=30 + 30*sqrt( abs(getBfield(x,y)) );
//b:=10 + bfield[y][x];
int v = min(255, max(0, (int)b));
Vec3b intensity;
if (bfield[y][x] > 0){
intensity.val[0]=255-v;
intensity.val[1]=255-v;
intensity.val[2]=255;
} else {
intensity.val[0]=255;
intensity.val[1]=255-v;
intensity.val[2]=255-v;
}
imgBfield.at<Vec3b>(y, x) = intensity;
}
}
}
static int save_as_csv(ImageInfo *ii,
const char *out_file, int what_to_save,
int strict_boundary, int load)
{
int i,j;
assert (g_poly->n > 0);
assert (crosshair_line > 0 && crosshair_samp > 0);
meta_parameters *meta = ii->meta;
int line_min, line_max, samp_min, samp_max, nl, ns;
compute_extent(meta, &line_min, &line_max, &samp_min, &samp_max,
&nl, &ns);
if (nl>500 || ns>500) {
// too big for csv -- Excel etc. will choke
char errbuf[1024];
snprintf(errbuf, 1024,
"\nRegion is too large (%dx%d) to export as CSV (500x500 max)\n\n",
nl, ns);
message_box(errbuf);
printf("%s", errbuf);
return FALSE; // failure
}
FILE *outFp = fopen(out_file, "w");
if (!outFp) {
// failed to open the output file!
char errbuf[1024];
snprintf(errbuf, 1024, "Failed to open %s: %s", out_file,
strerror(errno));
message_box(errbuf);
strcat(errbuf, "\n");
printf("%s", errbuf);
return FALSE; // failure
}
printf("Generating %s...\n", out_file);
// define clipping region, if necessary
double xp[MAX_POLY_LEN+2], yp[MAX_POLY_LEN+2];
int n=0;
if (strict_boundary)
define_clipping_region(meta, &n, xp, yp);
// generate csv
fprintf(outFp, ",");
for (j=0; j<ns; ++j) {
if (what_to_save==LAT_LON_2_BAND)
fprintf(outFp, "%d,%s", samp_min+j, j==ns-1 ? "\n" : ",");
else
fprintf(outFp, "%d%s", samp_min+j, j==ns-1 ? "\n" : ",");
}
if (what_to_save==LAT_LON_2_BAND) {
fprintf(outFp, ",");
for (j=0; j<ns; ++j) {
fprintf(outFp, "Lat,Lon%s", j==ns-1 ? "\n" : ",");
}
}
for (i=0; i<nl; ++i) {
int l = line_min+i;
fprintf(outFp, "%d,", l);
for (j=0; j<ns; ++j) {
int s = samp_min+j;
if (what_to_save==LAT_LON_2_BAND) {
float lat, lon;
if (!strict_boundary || pnpoly(n, xp, yp, s, l)) {
double dlat, dlon;
meta_get_latLon(meta, l, s, 0, &dlat, &dlon);
lat = (float)dlat;
lon = (float)dlon;
} else {
lat = lon = 0.;
}
fprintf(outFp, "%f,%f%s", lat, lon, j==ns-1 ? "\n" : ",");
}
else {
float val;
if (!strict_boundary || pnpoly(n, xp, yp, s, l)) {
val = get_data(ii, what_to_save, l, s);
} else {
val = 0;
}
fprintf(outFp, "%f%s", val, j==ns-1 ? "\n" : ",");
}
}
asfLineMeter(i,nl);
}
fclose(outFp);
// if requested, open up the csv with an external viewer
if (load)
open_csv(out_file);
return TRUE;
}
//Ray and Polygon Intersection---------------------------------------------------------------------------------------------------
float rayPolygonIntersection(Ray ray, POLY4 poly, Point3dPtr n2, Point3dPtr interp2, float *kd2)
{
float t;
float temp1, temp2;
float projectPlane[4][2];
float projPoint[2];
float p0[2];
float p1[2];
float p2[2];
float p3[2];
float Array[4];
int c;
float D;
Point3d np;
D = -1 * (poly.N[0] * poly.v[0][0] + poly.N[1] * poly.v[0][1] + poly.N[2] * poly.v[0][2]); // Put Vertax #1 to calculate value of D
np = assign_values(poly.N);
temp1 = dotProduct(&np, ray.a) + D;
temp2 = dotProduct(&np, ray.b);
t = -1 * (temp1 / temp2);
*kd2 = poly.kd;
// The surface normal------------------------------------------------------------------------------------------------
n2->x = poly.N[0];
n2->y = poly.N[1];
n2->z = poly.N[2];
// The intersection point--------------------------------------------------------------------------------------------
interp2->x = ray.a->x + ray.b->x*t;
interp2->y = ray.a->y + ray.b->y*t;
interp2->z = ray.a->z + ray.b->z*t;
// Get Projection of intersection point--------------------------------------------------------------------------------------------
findProjPoint(poly.N, interp2, projPoint);
// Get a Projection Plane------------------------------------------------------------------------------------------------------
findProjPlane(poly.N, poly.v, projectPlane);
float S0[4];
float S1[4];
S0[0] = projectPlane[0][0];
S0[1] = projectPlane[1][0];
S0[2] = projectPlane[2][0];
S0[3] = projectPlane[3][0];
S1[0] = projectPlane[0][1];
S1[1] = projectPlane[1][1];
S1[2] = projectPlane[2][1];
S1[3] = projectPlane[3][1];
c = pnpoly(4, S0, S1, projPoint[0], projPoint[1]);
if ((temp2 > 0.0f) || (temp2 = 0.0f))
return 0.0f; // Ray and Polygon parallel, intersection rejection
else
{
if (c != 0)
return t; //The distance
else
return 0.0f;
}
}
static int save_as_asf(ImageInfo *ii,
const char *out_file, int what_to_save,
int strict_boundary, int load)
{
// See if we can open the output file up front
FILE *outFp = fopen(out_file, "wb");
if (!outFp) {
// failed to open the output file!
char errbuf[1024];
snprintf(errbuf, 1024, "Failed to open %s: %s", out_file,
strerror(errno));
message_box(errbuf);
strcat(errbuf, "\n");
printf("%s", errbuf);
return FALSE; // failure
}
assert (g_poly->n > 0);
assert (crosshair_line > 0 && crosshair_samp > 0);
meta_parameters *meta = ii->meta;
// figure out where to chop
int line_min, line_max, samp_min, samp_max, nl, ns;
compute_extent(meta, &line_min, &line_max, &samp_min, &samp_max,
&nl, &ns);
// generate metadata
char *out_metaname = appendExt(out_file, ".meta");
printf("Generating %s...\n", out_metaname);
// data will be saved as floating point, except scaled pixel values,
// which we can make bytes.
data_type_t data_type = REAL32;
if (what_to_save == SCALED_PIXEL_VALUE)
data_type = ASF_BYTE;
meta_parameters *out_meta =
build_metadata(meta, out_file, nl, ns, line_min, samp_min, data_type,
what_to_save);
// put_float_line() will always dump BYTE data if the optical block
// is present... we want to be in control of the data type, so we must
// wipe out this block
if (out_meta->optical) {
FREE(out_meta->optical);
out_meta->optical=NULL;
}
if (what_to_save == LAT_LON_2_BAND) {
out_meta->general->band_count = 2;
strcpy(out_meta->general->bands, "LAT,LON");
}
// define clipping region, if necessary
double xp[MAX_POLY_LEN+2], yp[MAX_POLY_LEN+2];
int i,j,n=0;
if (strict_boundary)
define_clipping_region(meta, &n, xp, yp);
float ndv = 0;
if (meta_is_valid_double(out_meta->general->no_data))
ndv = out_meta->general->no_data;
else if (strict_boundary) // need to set a no data value in this case
out_meta->general->no_data = 0.;
meta_write(out_meta, out_metaname);
// now actually write the data
printf("Generating %s...\n", out_file);
if (what_to_save == LAT_LON_2_BAND) {
// dump a 2-band image, lat & lon data
float *lats = MALLOC(sizeof(float)*ns);
float *lons = MALLOC(sizeof(float)*ns);
for (i=0; i<nl; ++i) {
int l = line_min+i;
for (j=0; j<ns; ++j) {
int s = samp_min+j;
if (!strict_boundary || pnpoly(n, xp, yp, s, l)) {
double lat, lon;
meta_get_latLon(meta, l, s, 0, &lat, &lon);
lats[j] = (float)lat;
lons[j] = (float)lon;
}
else {
lats[j] = ndv;
lons[j] = ndv;
}
}
put_band_float_line(outFp, out_meta, 0, i, lats);
put_band_float_line(outFp, out_meta, 1, i, lons);
asfLineMeter(i,nl);
}
free(lats);
free(lons);
}
else {
// normal case
float *buf = MALLOC(sizeof(float)*ns);
for (i=0; i<nl; ++i) {
int l = line_min+i;
for (j=0; j<ns; ++j) {
int s = samp_min+j;
float val;
if (!strict_boundary || pnpoly(n, xp, yp, s, l)) {
val = get_data(ii, what_to_save, l, s);
}
else {
val = ndv;
}
buf[j] = val;
}
put_float_line(outFp, out_meta, i, buf);
asfLineMeter(i,nl);
}
free(buf);
}
fclose(outFp);
meta_free(out_meta);
// load the generated file if we were told to
if (load)
load_file(out_file);
return TRUE;
}
// return TRUE if there is any overlap between the two scenes
static int test_overlap(meta_parameters *meta1, meta_parameters *meta2)
{
if (!meta_is_valid_double(meta1->general->center_longitude)) {
int nl = meta1->general->line_count;
int ns = meta1->general->sample_count;
meta_get_latLon(meta1, nl/2, ns/2, 0,
&meta1->general->center_latitude,
&meta1->general->center_longitude);
}
if (!meta_is_valid_double(meta2->general->center_longitude)) {
int nl = meta2->general->line_count;
int ns = meta2->general->sample_count;
meta_get_latLon(meta2, nl/2, ns/2, 0,
&meta2->general->center_latitude,
&meta2->general->center_longitude);
}
int zone1 = utm_zone(meta1->general->center_longitude);
int zone2 = utm_zone(meta2->general->center_longitude);
// if zone1 & zone2 differ by more than 1, we can stop now
if (iabs(zone1-zone2) > 1) {
return FALSE;
}
// The Plan:
// Generate polygons for each metadata, then test of any pair of
// line segments between the polygons intersect.
// Other possibility: meta1 is completely contained within meta2,
// or the reverse.
// corners of meta1.
double xp_1[5], yp_1[5];
if (meta1->location) {
// use the location block if available
latLon2UTM_zone(meta1->location->lat_start_near_range,
meta1->location->lon_start_near_range, 0, zone1, &xp_1[0], &yp_1[0]);
latLon2UTM_zone(meta1->location->lat_start_far_range,
meta1->location->lon_start_far_range, 0, zone1, &xp_1[1], &yp_1[1]);
latLon2UTM_zone(meta1->location->lat_end_far_range,
meta1->location->lon_end_far_range, 0, zone1, &xp_1[2], &yp_1[2]);
latLon2UTM_zone(meta1->location->lat_end_near_range,
meta1->location->lon_end_near_range, 0, zone1, &xp_1[3], &yp_1[3]);
} else {
double lat, lon;
int nl1 = meta1->general->line_count;
int ns1 = meta1->general->sample_count;
// must call meta_get_latLon for each corner
meta_get_latLon(meta1, 0, 0, 0, &lat, &lon);
latLon2UTM_zone(lat, lon, 0, zone1, &xp_1[0], &yp_1[0]);
meta_get_latLon(meta1, nl1-1, 0, 0, &lat, &lon);
latLon2UTM_zone(lat, lon, 0, zone1, &xp_1[1], &yp_1[1]);
meta_get_latLon(meta1, nl1-1, ns1-1, 0, &lat, &lon);
latLon2UTM_zone(lat, lon, 0, zone1, &xp_1[2], &yp_1[2]);
meta_get_latLon(meta1, 0, ns1-1, 0, &lat, &lon);
latLon2UTM_zone(lat, lon, 0, zone1, &xp_1[3], &yp_1[3]);
}
// close the polygon
xp_1[4] = xp_1[0];
yp_1[4] = yp_1[0];
// corners of meta2.
double xp_2[5], yp_2[5];
if (meta2->location) {
// use the location block if available
latLon2UTM_zone(meta2->location->lat_start_near_range,
meta2->location->lon_start_near_range, 0, zone1, &xp_2[0], &yp_2[0]);
latLon2UTM_zone(meta2->location->lat_start_far_range,
meta2->location->lon_start_far_range, 0, zone1, &xp_2[1], &yp_2[1]);
latLon2UTM_zone(meta2->location->lat_end_far_range,
meta2->location->lon_end_far_range, 0, zone1, &xp_2[2], &yp_2[2]);
latLon2UTM_zone(meta2->location->lat_end_near_range,
meta2->location->lon_end_near_range, 0, zone1, &xp_2[3], &yp_2[3]);
} else {
double lat, lon;
int nl2 = meta2->general->line_count;
int ns2 = meta2->general->sample_count;
// must call meta_get_latLon for each corner
meta_get_latLon(meta2, 0, 0, 0, &lat, &lon);
latLon2UTM_zone(lat, lon, 0, zone1, &xp_2[0], &yp_2[0]);
meta_get_latLon(meta2, nl2-1, 0, 0, &lat, &lon);
latLon2UTM_zone(lat, lon, 0, zone1, &xp_2[1], &yp_2[1]);
meta_get_latLon(meta2, nl2-1, ns2-1, 0, &lat, &lon);
latLon2UTM_zone(lat, lon, 0, zone1, &xp_2[2], &yp_2[2]);
meta_get_latLon(meta2, 0, ns2-1, 0, &lat, &lon);
latLon2UTM_zone(lat, lon, 0, zone1, &xp_2[3], &yp_2[3]);
}
// close the polygon
xp_2[4] = xp_2[0];
yp_2[4] = yp_2[0];
// loop over each pair of line segments, testing for intersection
int i, j;
for (i = 0; i < 4; ++i) {
for (j = 0; j < 4; ++j) {
if (lineSegmentsIntersect(
xp_1[i], yp_1[i], xp_1[i+1], yp_1[i+1],
xp_2[j], yp_2[j], xp_2[j+1], yp_2[j+1]))
{
return TRUE;
}
}
}
// test for containment: meta2 in meta1
int all_in=TRUE;
for (i=0; i<4; ++i) {
if (!pnpoly(5, xp_1, yp_1, xp_2[i], yp_2[i])) {
all_in=FALSE;
break;
}
}
if (all_in)
return TRUE;
// test for containment: meta1 in meta2
all_in = TRUE;
for (i=0; i<4; ++i) {
if (!pnpoly(5, xp_2, yp_2, xp_1[i], yp_1[i])) {
all_in=FALSE;
break;
}
}
if (all_in)
return TRUE;
// no overlap
return FALSE;
}
//---------------------------
bool coordWarping::bInQuad(ofVec2f pt){
return pnpoly(4, srcQuad, pt.x, pt.y);
}
static int run ()
{
int npol=20, count=0;
if (pnpoly(npol,xp,yp,0.5,0.5)) count++;
if (pnpoly(npol,xp,yp,0.5,1.5)) count++;
if (pnpoly(npol,xp,yp,-.5,1.5)) count++;
if (pnpoly(npol,xp,yp,0.75,2.25)) count++;
if (pnpoly(npol,xp,yp,0,2.01)) count++;
if (pnpoly(npol,xp,yp,-.5,2.5)) count++;
if (pnpoly(npol,xp,yp,-1.0,-.5)) count++;
if (pnpoly(npol,xp,yp,-1.5,.5)) count++;
if (pnpoly(npol,xp,yp,-2.25,-1.0)) count++;
if (pnpoly(npol,xp,yp,0.5,-.25)) count++;
if (pnpoly(npol,xp,yp,0.5,-1.25)) count++;
if (pnpoly(npol,xp,yp,-.5,-2.5)) count++;
return count;
}
