Polygon splitted by 4 parts - algorithm

I have an arbitrary convex polygon. And it is splitted by 2 perpendicular lines (vectors of them are (0,1) and (1,0)). Is there any algorithm that can calculate area by smaller figures (S1, S2, S3, S4). All I can do is to calculate points where lines cross the polygon and then calculate areas, but is there something better optimized?
I store all vertexes in array double **v;
And then I calculate all points, where my polygon crosses X and Y axises:
void cross() { //calculates buf (crossing with Y)
act = 0;
for (int i = 0; i < n; ++i) {
buf[act][0]=v[i][0];
buf[act][1]=v[i][1];
act++;
if (v[i][0]*v[(i+1)%n][0] < 0) {
buf[act][0] = 0;
buf[act][1] = v[i][1] + std::abs(v[i][0])*(v[(i+1)%n][1]-v[i][1])/(std::abs(v[i][0])+std::abs(v[(i+1)%n][0]));
act++;
}
}
}
void vert() { /calculates buf2 (crossing with X)
act2 =0;
for (int i = 0; i < act; ++i) {
buf2[act2][0]=buf[i][0];
buf2[act2][1]=buf[i][1];
act2++;
if (buf[i][1]*buf[(i+1)%act][1] < 0) {
buf2[act2][1] = 0;
buf2[act2][0] = buf[i][0] + std::abs(buf[i][1])*(buf[(i+1)%act][0] - buf[i][0])/ (std::abs(buf[i][1])+std::abs(buf[(i+1)%act][1]));
act2++;
}
}
}
After calling cross(); vert(); I get an array buf2 and number of elements there is act2;
After this I'am triangilating polygon and detect in what squad does traingle lay.
double s_trian (double a, double b, double c, double d) {
//area of triangle
double s =0;
s=0.5*std::abs((a)*(d)-(c)*(b));
return s;
}
void triang() { //calculate areas of s1,s2,s3,s4 by
//triangulating
bool rotation;
double temror;
s1=0, s2 =0, s3 =0, s4 =0;
int a,b;
for (int i =0; i < act2; ++i) {
a=i%act2;
b=(i+1)%act2;
temror = s_trian(buf2[a][0], buf2[a][1], buf2[b][0], buf2[b][1]);
if ((buf2[a][0]+buf2[b][0]) > 0) {
if((buf2[a][1]+buf2[b][1] > 0))
s1+=temror;
else
s4+=temror;
} else {
if ((buf2[a][1]+buf2[b][1] > 0))
s2+=temror;
else
s3+=temror;
}
}
}
Can I optimize something here?

Given that your polygon is convex, just select an arbitrary point P inside the polygon and split it up in triangles with one corner in P.
Then calculate the area of each triangle: http://www.mathopenref.com/heronsformula.html and sum them up.

You can do slightly better.
Ignore X to begin with.
Project horizontally every vertex on Y. This way, you define trapezoids. The sum of the algebraic areas of these trapezoids gives the total surface. Add the positive and negative areas in separate accumulators, this will give you the areas on both sides of Y. But some trapezoids will cross Y and be skewed: compute the areas of the two triangles and accumulate where appropriate.
Now to deal with the horizontal axis, similarly you will add the contributions to a positive/negative accumulator, or to both.
In total there will be four accumulators, for all sign combinations, giving you the four requested areas.
This procedure will cost you a little more than one accumulation per side, instead of four. It can be done in a single loop, avoiding the need to compute and store the four subpolygons.

[Following up on my comment yesterday; has much in common with Dan Bystrom's answer.]
Loop over all sides, and compute the area of the triangle made of the side and the origin. Add to the appropriate quad area. Where a side crosses the axis, compute the intercept and split the triangle. Compute both triangle areas parts and add each to the appropriate quad.
Using the origin as a point for a triangle vertex makes the cross product based formula for a triangle area very fast and simple. You don't even need the call to fabs() if you take care to pass the parameters in the right order.
This code has not handled the problem where a vertex lies on an axis or cases where no point lies in a given quadrant.
struct Point
{
double x;
double y;
};
double areaOfTriangle(double ax, double ay, double bx, double by)
{
return fabs(by*ax - bx *ay)/2;
}
unsigned getQuad(double x, double y)
{
int xPos = (x > 0) ? 0 : 1;
int yPos = (y > 0) ? 0 : 1 ;
int quad = xPos + yPos;
if (!xPos && yPos)
quad = 3;
return quad;
}
Point getIntercept(const Point& a, const Point& b)
{
Point intercept;
if ( (a.x * b.x) < 0)
{
// Crosses y axis.
intercept.x = 0;
intercept.y = a.y - (b.y - a.y) / (b.x - a.x)*a.x;
}
else
{
// Crosses x axis.
intercept.y = 0;
intercept.x = a.x - (b.x - a.x) / (b.y - a.y)*a.y;
}
return intercept;
}
void getAreaOfQuads(double* retQuadArea, const Point* points, unsigned numPts)
{
for (unsigned i = 0; i != 4; ++i)
retQuadArea[i] = 0;
const Point* a = &points[numPts - 1];
unsigned quadA = getQuad(a->x, a->y);
for (unsigned i = 0; i != numPts; ++i)
{
const Point* b = &points[i];
unsigned quadB = getQuad(b->x, b->y);
if (quadA == quadB)
{
retQuadArea[quadA] += areaOfTriangle(a->x, a->y, b->x, b->y);
}
else
{
// The side a->b crosses an axis.
// First, find out where.
Point c = getIntercept(*a, *b);
retQuadArea[quadA] += areaOfTriangle(a->x, a->y, c.x, c.y);
retQuadArea[quadB] += areaOfTriangle(c.x, c.y, b->x, b->y);
}
a = b;
quadA = quadB;
}
}
void test(Point* polygon, unsigned n)
{
double areas[4] = {};
getAreaOfQuads(areas, polygon, n);
for (unsigned i = 0; i != 4; ++i)
std::cout << areas[i] << ", ";
std::cout << std::endl;
}
Point polygon[]
{
{0.6, 0.2},
{ 0.2, 0.8 },
{ -0.2, 0.7 },
{ -0.6, 0.6 },
{ -1.0, 0.1 },
{ -0.6, -0.5 },
{ 0.1, -0.5 },
{ 0.9, -0.1 }
};
Point square[]
{
{1, 1},
{ -1, 1 },
{ -1, -1 },
{ 1, -1 }
};
int main()
{
test(square, 4);
test(polygon, 8);
return 0;
}

Related

Grid - Obstacle / How to Calculate "Field of view"

My english is not my native language, I have no idea how title it, how to explain it clearly and not sure if it's the right term. I've tried to search on google first but for the reason quoted above, I couldn't find anything related.
Could you guys first please check the imgur album : https://imgur.com/a/4mMuCil
So...
Black square is an "obstacle"
Red Square is a "player"
Grey square are "area where player is not able to see"
Depending on the distance of the player from the obstacle, the player can see more or less "things"
Is there a general formula to determine the area that he can see or can't see ?
Or I have to write a unique formula depending on the player position relative to the obstacle
I'm sorry If what I wrote doesn't make sense
Thanks for your help
EDIT :
point player(5, 0);
point obstacle(4, 2);
.....o...
.........
....#....
.........
.....#...
.....##..
.....###.
.....####
......###
This needs a little work. I'll say the steps to make it work:
(1) Let's define the player position p and the obstacle position o.
We know we want to paint a "triangle" after the obstacle.
(2) Let's define the angle according to proximity.
The nearer, the bigger the angle is, so I set the angle to 45 if the distance is 1 and it decreases as the player gets further from the obstacle. The angle is 5 + (max(0, 50 - distance * 10)). You can tune this angle.
(3) Let's build a big triangle. The first vertex is the obstacle. Then, throw a big line from the player through the obstacle. Rotate this line around the obstacle half angle clockwise (to get the second vertex) and half angle anti-clockwise (to get the third vertex), as shown in the image:
(4) Lastly, iterate to the matrix and for each position, ask if that coordinates are inside the triangle.
#include <bits/stdc++.h>
using namespace std;
struct point{
float x, y;
point(){}
point(float x, float y){this->x = x; this->y = y;}
//point(int x, int y){this->x = x; this->y = y;}
};
point rotate(point pivot, float angle, point p, bool clockwise){
float s = sin(angle);
float c = cos(angle);
p.x -= pivot.x;
p.y -= pivot.y;
if(clockwise){
return point(p.x * c + p.y * s + pivot.x, -p.x * s + p.y * c + pivot.y);
}
else{
return point(p.x * c - p.y * s + pivot.x, p.x * s + p.y * c + pivot.y);
}
}
float triangleArea(point p1, point p2, point p3) { //find area of triangle formed by p1, p2 and p3
return abs((p1.x*(p2.y-p3.y) + p2.x*(p3.y-p1.y)+ p3.x*(p1.y-p2.y))/2.0);
}
bool inside(point p1, point p2, point p3, point p) { //check whether p is inside or outside
float area = triangleArea (p1, p2, p3); //area of triangle ABC
float area1 = triangleArea (p, p2, p3); //area of PBC
float area2 = triangleArea (p1, p, p3); //area of APC
float area3 = triangleArea (p1, p2, p); //area of ABP
return abs(area - area1 + area2 + area3) < 1; //when three triangles are forming the whole triangle
}
char m[9][9];
point player(4, 0);
point obstacle(4, 2);
float angle(){
float dist = sqrt(pow(player.x - obstacle.x, 2) + pow(player.y - obstacle.y, 2));
cout<<"dist: "<<(5.0 + max(0.0, 50.0 - 10.0 * dist))<<endl;
return (5.0 + max(0.0, 50.0 - 10.0 * dist)) * 0.0174533;
}
void print(){
for(int i = 0; i < 9; i++){
for(int j = 0; j < 9; j++){
cout<<m[i][j];
}
cout<<endl;
}
}
int main(){
for(int i = 0; i < 9; i++){
for(int j = 0; j < 9; j++){
m[i][j] = '.';
}
}
m[(int)player.y][(int)player.x] = 'o';
m[(int)obstacle.y][(int)obstacle.x] = '#';
float rad = angle();
point end(20.0 * (obstacle.x - player.x) + obstacle.x, 20.0 * (player.y - obstacle.y) + obstacle.y);
point p2 = rotate(obstacle, rad / 2.0, end, true);
point p3 = rotate(obstacle, rad / 2.0, end, false);
for(int i = 0; i < 9; i++){
for(int j = 0; j < 9; j++){
if(i == (int) player.y && j == (int) player.x) continue;
if(i == (int) obstacle.y && j == (int) obstacle.x) continue;
if(inside(obstacle, p2, p3, point(j, i))) m[i][j] = '#';
}
}
print();
return 0;
}
OUTPUT:
....o....
.........
....#....
....#....
....#....
....#....
...###...
...###...
...###...
Suppose the player is at (0,0), and the obstacle is at (j,k), with j>0 and k>=0.
Then a square at (x, y) will be visible if either (2j-1)y >= (2k+1)x or (2k-1)x >= (2j+1)y.
Applying this rule to the other three quadrants is straightforward.

Obtaining orientation map of fingerprint image using OpenCV

I'm trying to implement the method of improving fingerprint images by Anil Jain. As a starter, I encountered some difficulties while extracting the orientation image, and am strictly following those steps described in Section 2.4 of that paper.
So, this is the input image:
And this is after normalization using exactly the same method as in that paper:
I'm expecting to see something like this (an example from the internet):
However, this is what I got for displaying obtained orientation matrix:
Obviously this is wrong, and it also gives non-zero values for those zero points in the original input image.
This is the code I wrote:
cv::Mat orientation(cv::Mat inputImage)
{
cv::Mat orientationMat = cv::Mat::zeros(inputImage.size(), CV_8UC1);
// compute gradients at each pixel
cv::Mat grad_x, grad_y;
cv::Sobel(inputImage, grad_x, CV_16SC1, 1, 0, 3, 1, 0, cv::BORDER_DEFAULT);
cv::Sobel(inputImage, grad_y, CV_16SC1, 0, 1, 3, 1, 0, cv::BORDER_DEFAULT);
cv::Mat Vx, Vy, theta, lowPassX, lowPassY;
cv::Mat lowPassX2, lowPassY2;
Vx = cv::Mat::zeros(inputImage.size(), inputImage.type());
Vx.copyTo(Vy);
Vx.copyTo(theta);
Vx.copyTo(lowPassX);
Vx.copyTo(lowPassY);
Vx.copyTo(lowPassX2);
Vx.copyTo(lowPassY2);
// estimate the local orientation of each block
int blockSize = 16;
for(int i = blockSize/2; i < inputImage.rows - blockSize/2; i+=blockSize)
{
for(int j = blockSize / 2; j < inputImage.cols - blockSize/2; j+= blockSize)
{
float sum1 = 0.0;
float sum2 = 0.0;
for ( int u = i - blockSize/2; u < i + blockSize/2; u++)
{
for( int v = j - blockSize/2; v < j+blockSize/2; v++)
{
sum1 += grad_x.at<float>(u,v) * grad_y.at<float>(u,v);
sum2 += (grad_x.at<float>(u,v)*grad_x.at<float>(u,v)) * (grad_y.at<float>(u,v)*grad_y.at<float>(u,v));
}
}
Vx.at<float>(i,j) = sum1;
Vy.at<float>(i,j) = sum2;
double calc = 0.0;
if(sum1 != 0 && sum2 != 0)
{
calc = 0.5 * atan(Vy.at<float>(i,j) / Vx.at<float>(i,j));
}
theta.at<float>(i,j) = calc;
// Perform low-pass filtering
float angle = 2 * calc;
lowPassX.at<float>(i,j) = cos(angle * pi / 180);
lowPassY.at<float>(i,j) = sin(angle * pi / 180);
float sum3 = 0.0;
float sum4 = 0.0;
for(int u = -lowPassSize / 2; u < lowPassSize / 2; u++)
{
for(int v = -lowPassSize / 2; v < lowPassSize / 2; v++)
{
sum3 += inputImage.at<float>(u,v) * lowPassX.at<float>(i - u*lowPassSize, j - v * lowPassSize);
sum4 += inputImage.at<float>(u, v) * lowPassY.at<float>(i - u*lowPassSize, j - v * lowPassSize);
}
}
lowPassX2.at<float>(i,j) = sum3;
lowPassY2.at<float>(i,j) = sum4;
float calc2 = 0.0;
if(sum3 != 0 && sum4 != 0)
{
calc2 = 0.5 * atan(lowPassY2.at<float>(i, j) / lowPassX2.at<float>(i, j)) * 180 / pi;
}
orientationMat.at<float>(i,j) = calc2;
}
}
return orientationMat;
}
I've already searched a lot on the web, but almost all of them are in Matlab. And there exist very few ones using OpenCV, but they didn't help me either. I sincerely hope someone could go through my code and point out any error to help. Thank you in advance.
Update
Here are the steps that I followed according to the paper:
Obtain normalized image G.
Divide G into blocks of size wxw (16x16).
Compute the x and y gradients at each pixel (i,j).
Estimate the local orientation of each block centered at pixel (i,j) using equations:
Perform low-pass filtering to remove noise. For that, convert the orientation image into a continuous vector field defined as:
where W is a two-dimensional low-pass filter, and w(phi) x w(phi) is its size, which equals to 5.
Finally, compute the local ridge orientation at (i,j) using:
Update2
This is the output of orientationMat after changing the mat type to CV_16SC1 in Sobel operation as Micka suggested:
Maybe it's too late for me to answer, but anyway somebody could read this later and solve the same problem.
I've been working for a while in the same algorithm, same method you posted... But there's some writting errors when the papper was redacted (I guess). After fighting a lot with the equations I found this errors by looking other similar works.
Here is what worked for me...
Vy(i, j) = 2*dx(u,v)*dy(u,v)
Vx(i,j) = dx(u,v)^2 - dy(u,v)^2
O(i,j) = 0.5*arctan(Vy(i,j)/Vx(i,j)
(Excuse me I wasn't able to post images, so I wrote the modified ecuations. Remeber "u" and "v" are positions of the summation across the BlockSize by BlockSize window)
The first thing and most important (obviously) are the equations, I saw that in different works this expressions were really different and in every one they talked about the same algorithm of Hong et al.
The Key is finding the Least Mean Square (First 3 equations) of the gradients (Vx and Vy), I provided the corrected formulas above for this ation. Then you can compute angle theta for the non overlapping window (16x16 size recommended in the papper), after that the algorithm says you must calculate the magnitud of the doubled angle in "x" and "y" directions (Phi_x and Phi_y).
Phi_x(i,j) = V(i,j) * cos(2*O(i,j))
Phi_y(i,j) = V(i,j) * sin(2*O(i,j))
Magnitud is just:
V = sqrt(Vx(i,j)^2 + Vy(i,j)^2)
Note that in the related work doesn't mention that you have to use the gradient magnitud, but it make sense (for me) in doing it. After all this corrections you can apply the low pass filter to Phi_x and Phi_y, I used a simple Mask of size 5x5 to average this magnitudes (something like medianblur() of opencv).
Last thing is to calculate new angle, that is the average of the 25ith neighbors in the O(i,j) image, for this you just have to:
O'(i,j) = 0.5*arctan(Phi_y/Phi_x)
We're just there... All this just for calculating the angle of the NORMAL VECTOR TO THE RIDGES DIRECTIONS (O'(i,j)) in the BlockSize by BlockSize non overlapping window, what does it mean? it means that the angle we just calculated is perpendicular to the ridges, in simple words we just calculated the angle of the riges plus 90 degrees... To get the angle we need, we just have to substract to the obtained angle 90°.
To draw the lines we need to have an initial point (X0, Y0) and a final point(X1, Y1). For that imagine a circle centered on (X0, Y0) with a radious of "r":
x0 = i + blocksize/2
y0 = j + blocksize/2
r = blocksize/2
Note we add i and j to the first coordinates becouse the window is moving and we are gonna draw the line starting from the center of the non overlaping window, so we can't use just the center of the non overlaping window.
Then to calculate the end coordinates to draw a line we can just have to use a right triangle so...
X1 = r*cos(O'(i,j)-90°)+X0
Y1 = r*sin(O'(i,j)-90°)+Y0
X2 = X0-r*cos(O'(i,j)-90°)
Y2 = Y0-r*cos(O'(i,j)-90°)
Then just use opencv line function, where initial Point is (X0,Y0) and final Point is (X1, Y1). Additional to it, I drawed the windows of 16x16 and computed the oposite points of X1 and Y1 (X2 and Y2) to draw a line of the entire window.
Hope this help somebody.
My results...
Main function:
Mat mat = imread("nwmPa.png",0);
mat.convertTo(mat, CV_32F, 1.0/255, 0);
Normalize(mat);
int blockSize = 6;
int height = mat.rows;
int width = mat.cols;
Mat orientationMap;
orientation(mat, orientationMap, blockSize);
Normalize:
void Normalize(Mat & image)
{
Scalar mean, dev;
meanStdDev(image, mean, dev);
double M = mean.val[0];
double D = dev.val[0];
for(int i(0) ; i<image.rows ; i++)
{
for(int j(0) ; j<image.cols ; j++)
{
if(image.at<float>(i,j) > M)
image.at<float>(i,j) = 100.0/255 + sqrt( 100.0/255*pow(image.at<float>(i,j)-M,2)/D );
else
image.at<float>(i,j) = 100.0/255 - sqrt( 100.0/255*pow(image.at<float>(i,j)-M,2)/D );
}
}
}
Orientation map:
void orientation(const Mat &inputImage, Mat &orientationMap, int blockSize)
{
Mat fprintWithDirectionsSmoo = inputImage.clone();
Mat tmp(inputImage.size(), inputImage.type());
Mat coherence(inputImage.size(), inputImage.type());
orientationMap = tmp.clone();
//Gradiants x and y
Mat grad_x, grad_y;
// Sobel(inputImage, grad_x, CV_32F, 1, 0, 3, 1, 0, BORDER_DEFAULT);
// Sobel(inputImage, grad_y, CV_32F, 0, 1, 3, 1, 0, BORDER_DEFAULT);
Scharr(inputImage, grad_x, CV_32F, 1, 0, 1, 0);
Scharr(inputImage, grad_y, CV_32F, 0, 1, 1, 0);
//Vector vield
Mat Fx(inputImage.size(), inputImage.type()),
Fy(inputImage.size(), inputImage.type()),
Fx_gauss,
Fy_gauss;
Mat smoothed(inputImage.size(), inputImage.type());
// Local orientation for each block
int width = inputImage.cols;
int height = inputImage.rows;
int blockH;
int blockW;
//select block
for(int i = 0; i < height; i+=blockSize)
{
for(int j = 0; j < width; j+=blockSize)
{
float Gsx = 0.0;
float Gsy = 0.0;
float Gxx = 0.0;
float Gyy = 0.0;
//for check bounds of img
blockH = ((height-i)<blockSize)?(height-i):blockSize;
blockW = ((width-j)<blockSize)?(width-j):blockSize;
//average at block WхW
for ( int u = i ; u < i + blockH; u++)
{
for( int v = j ; v < j + blockW ; v++)
{
Gsx += (grad_x.at<float>(u,v)*grad_x.at<float>(u,v)) - (grad_y.at<float>(u,v)*grad_y.at<float>(u,v));
Gsy += 2*grad_x.at<float>(u,v) * grad_y.at<float>(u,v);
Gxx += grad_x.at<float>(u,v)*grad_x.at<float>(u,v);
Gyy += grad_y.at<float>(u,v)*grad_y.at<float>(u,v);
}
}
float coh = sqrt(pow(Gsx,2) + pow(Gsy,2)) / (Gxx + Gyy);
//smoothed
float fi = 0.5*fastAtan2(Gsy, Gsx)*CV_PI/180;
Fx.at<float>(i,j) = cos(2*fi);
Fy.at<float>(i,j) = sin(2*fi);
//fill blocks
for ( int u = i ; u < i + blockH; u++)
{
for( int v = j ; v < j + blockW ; v++)
{
orientationMap.at<float>(u,v) = fi;
Fx.at<float>(u,v) = Fx.at<float>(i,j);
Fy.at<float>(u,v) = Fy.at<float>(i,j);
coherence.at<float>(u,v) = (coh<0.85)?1:0;
}
}
}
} ///for
GaussConvolveWithStep(Fx, Fx_gauss, 5, blockSize);
GaussConvolveWithStep(Fy, Fy_gauss, 5, blockSize);
for(int m = 0; m < height; m++)
{
for(int n = 0; n < width; n++)
{
smoothed.at<float>(m,n) = 0.5*fastAtan2(Fy_gauss.at<float>(m,n), Fx_gauss.at<float>(m,n))*CV_PI/180;
if((m%blockSize)==0 && (n%blockSize)==0){
int x = n;
int y = m;
int ln = sqrt(2*pow(blockSize,2))/2;
float dx = ln*cos( smoothed.at<float>(m,n) - CV_PI/2);
float dy = ln*sin( smoothed.at<float>(m,n) - CV_PI/2);
arrowedLine(fprintWithDirectionsSmoo, Point(x, y+blockH), Point(x + dx, y + blockW + dy), Scalar::all(255), 1, CV_AA, 0, 0.06*blockSize);
// qDebug () << Fx_gauss.at<float>(m,n) << Fy_gauss.at<float>(m,n) << smoothed.at<float>(m,n);
// imshow("Orientation", fprintWithDirectionsSmoo);
// waitKey(0);
}
}
}///for2
normalize(orientationMap, orientationMap,0,1,NORM_MINMAX);
imshow("Orientation field", orientationMap);
orientationMap = smoothed.clone();
normalize(smoothed, smoothed, 0, 1, NORM_MINMAX);
imshow("Smoothed orientation field", smoothed);
imshow("Coherence", coherence);
imshow("Orientation", fprintWithDirectionsSmoo);
}
seems nothing forgot )
I have read your code thoroughly and found that you have made a mistake while calculating sum3 and sum4:
sum3 += inputImage.at<float>(u,v) * lowPassX.at<float>(i - u*lowPassSize, j - v * lowPassSize);
sum4 += inputImage.at<float>(u, v) * lowPassY.at<float>(i - u*lowPassSize, j - v * lowPassSize);
instead of inputImage you should use a low pass filter.

My Implementation of Midpoint Displacement to generate islands does not create the desired pattern?

I wanted to create an effect such as the one in this article, starting from the code found at the processing port of 'realistic terrain in 130 lines'. I modified the code so it only produces a grey scale height map in Processing rather than a 3D terrain. However, my code does not produce island heightmaps and also leaves an unusual square pattern, as pictured here.
My Code is as follows:
int size = 512;
Terrain t;
float rough = 0.7;
void setup() {
t = new Terrain(8, size);
doit();
}
void doit() {
t.generate(rough);
size(513, 513);
loadPixels();
for (int a=0;a<t.tMap.length;a++) {
float val = ((t.tMap[a]+24)/48)*255;
pixels[a] = color(floor(val));
}
updatePixels();
}
void mousePressed() {
doit();
}
class Terrain {
public final int tSize, tMax;
public float[] tMap;
public float tRoughness;
Terrain(int detail, int size) {
tSize = size+1;
tMax = tSize-1;
tMap = new float[tSize*tSize];
}
private float getp(int x, int y) {
if (x < 0) x= tMax + x;
if (x > tMax) x= x % tMax;
if (y < 0) y= tMax + y;
if (y > tMax) y= y % tMax;
return tMap[x + tSize * y];
}
private void setp(int x, int y, float val) {
tMap[x + tSize * y] = val;
}
public void generate(float roughness) {
tRoughness = roughness;
//set edges and corners 0
for (int x=0;x<tSize;x++) {
setp(x, 0, 0);
setp(x, tMax, 0);
}
for (int y=0;y<tSize;y++) {
setp(y, 0, 0);
setp(y, tMax, 0);
}
//seed random numbers every 16
for (int x=0;x<tSize;x+=16) {
for (int y=0;y<tSize;y+=16) {
setp(x, y, random(-(roughness*16), roughness*16));
}
}
//divide each 16x16 square
for (int x=0;x<tSize;x+=16) {
for (int y=0;y<tSize;y+=16) {
divide(16, x, y);
}
}
}
private void divide(int size, int smallestX, int smallestY) {
int x, y, half = size/2;
float scale = tRoughness * size;
if (half < 1) return;
for (y = smallestY + half; y < tMax; y += size) {
for (x = smallestX + half; x < tMax; x += size) {
square(x, y, half, random(-scale, scale));
}
}
for (y = smallestY; y <= tMax; y += half) {
for (x = (y + half + smallestY) % size; x <= tMax; x += size) {
diamond(x, y, half, random(-scale, scale));
}
}
divide(size/2, smallestX, smallestY);
}
private float average(float[] values) {
int valid = 0;
float total = 0;
for (int i=0; i<values.length; i++) {
if (values[i] != -1) {
valid++;
total += values[i];
}
}
return valid == 0 ? 0 : total / valid;
}
private void square(int x, int y, int size, float offset) {
float ave = average(new float[] {
getp(x - size, y - size), // upper left
getp(x + size, y - size), // upper right
getp(x + size, y + size), // lower right
getp(x - size, y + size) // lower left
}
);
setp(x, y, ave + offset);
}
private void diamond(int x, int y, int size, float offset) {
float ave = average(new float[] {
getp(x, y - size), // top
getp(x + size, y), // right
getp(x, y + size), // bottom
getp(x - size, y) // left
}
);
setp(x, y, ave + offset);
}
}
Thanks for any help!
You break down the terrain in 16x16 blocks first. In the output, these borders are quite visible. Why is that? Well, because the values at the border differs a lot more than the values within each block differ.
The differences between blocks are determined by the initial generate values, the differences with a block are caused by divide. You need more variation in generate or less in divide.
Even then it might be worthwhile to apply a smoothing operator over the whole image after all blocks have been generated. This decreases roughness a bit, but it will further decrease the visibility of specific borders (especially to the human eye; a statistical test is still likely to prove that the image was generated by an axis-aligned process)

Finding best path given a distance transform

Given a distance transform of a map with obstacles in it, how do I get the least cost path from a start pixel to a goal pixel? The distance transform image has the distance(euclidean) to the nearest obstacle of the original map, in each pixel i.e. if in the original map pixel (i,j) is 3 pixels away to the left and 2 pixels away to the down of an obstacle, then in the distance transform the pixel will have sqrt(4+9) = sqrt(13) as the pixel value. Thus darker pixels signify proximity to obstacles and lighter values signify that they are far from obstacles.
I want to plan a path from a given start to goal pixel using the information provided by this distance transform and optimize the cost of the path and also there is another constraint that the path should never reach a pixel which is less than 'x' pixels away from an obstacle.
How do I go about this?
P.S. A bit of description on the algorithm (or a bit of code) would be helpful as I am new to planning algorithms.
I found an algorithm in the appendix I of the chapter titled
JARVIS, Ray. Distance transform based path planning for robot navigation. Recent trends in mobile robots, 1993, 11: 3-31.
That chapter is fully visible to me in Google books and the book is
ZHENG, Yuang F. (ed.). Recent trends in mobile robots. World Scientific, 1993.
A C++ implementation of the algorithm follows:
#include <vector>
#include <iostream>
#include <cmath>
#include <algorithm>
#include <cassert>
#include <sstream>
/**
Algorithm in the appendix I of the chapter titled
JARVIS, Ray. Distance transform based path planning for robot navigation. *Recent trends in mobile robots*, 1993, 11: 3-31.
in the book
ZHENG, Yuang F. (ed.). *Recent trends in mobile robots*. World Scientific, 1993.
See also http://stackoverflow.com/questions/21215244/least-cost-path-using-a-given-distance-transform
*/
template < class T >
class Matrix
{
private:
int m_width;
int m_height;
std::vector<T> m_data;
public:
Matrix(int width, int height) :
m_width(width), m_height(height), m_data(width *height) {}
int width() const
{
return m_width;
}
int height() const
{
return m_height;
}
void set(int x, int y, const T &value)
{
m_data[x + y * m_width] = value;
}
const T &get(int x, int y) const
{
return m_data[x + y * m_width];
}
};
float distance( const Matrix< float > &a, const Matrix< float > &b )
{
assert(a.width() == b.width());
assert(a.height() == b.height());
float r = 0;
for ( int y = 0; y < a.height(); y++ )
{
for ( int x = 0; x < a.width(); x++ )
{
r += fabs(a.get(x, y) - b.get(x, y));
}
}
return r;
}
int PPMGammaEncode(float radiance, float d)
{
//return int(std::pow(std::min(1.0f, std::max(0.0f, radiance * d)),1.0f / 2.2f) * 255.0f);
return radiance;
}
void PPM_image_save(const Matrix<float> &img, const std::string &filename, float d = 15.0f)
{
FILE *file = fopen(filename.c_str(), "wt");
const int m_width = img.width();
const int m_height = img.height();
fprintf(file, "P3 %d %d 255\n", m_width, m_height);
for (int y = 0; y < m_height; ++y)
{
fprintf(file, "\n# y = %d\n", y);
for (int x = 0; x < m_width; ++x)
{
const float &c(img.get(x, y));
fprintf(file, "%d %d %d\n",
PPMGammaEncode(c, d),
PPMGammaEncode(c, d),
PPMGammaEncode(c, d));
}
}
fclose(file);
}
void PPM_image_save(const Matrix<bool> &img, const std::string &filename, float d = 15.0f)
{
FILE *file = fopen(filename.c_str(), "wt");
const int m_width = img.width();
const int m_height = img.height();
fprintf(file, "P3 %d %d 255\n", m_width, m_height);
for (int y = 0; y < m_height; ++y)
{
fprintf(file, "\n# y = %d\n", y);
for (int x = 0; x < m_width; ++x)
{
float v = img.get(x, y) ? 255 : 0;
fprintf(file, "%d %d %d\n",
PPMGammaEncode(v, d),
PPMGammaEncode(v, d),
PPMGammaEncode(v, d));
}
}
fclose(file);
}
void add_obstacles(Matrix<bool> &m, int n, int avg_lenght, int sd_lenght)
{
int side = std::max(3, std::min(m.width(), m.height()) / 10);
for ( int y = m.height() / 2 - side / 2; y < m.height() / 2 + side / 2; y++ )
{
for ( int x = m.width() / 2 - side / 2; x < m.width() / 2 + side / 2; x++ )
{
m.set(x, y, true);
}
}
/*
for ( int y = m.height()/2-side/2; y < m.height()/2+side/2; y++ ) {
for ( int x = 0; x < m.width()/2+side; x++ ) {
m.set(x,y,true);
}
}
*/
for ( int y = 0; y < m.height(); y++ )
{
m.set(0, y, true);
m.set(m.width() - 1, y, true);
}
for ( int x = 0; x < m.width(); x++ )
{
m.set(x, 0, true);
m.set(x, m.height() - 1, true);
}
}
class Info
{
public:
Info() {}
Info(float v, int x_o, int y_o): value(v), x_offset(x_o), y_offset(y_o) {}
float value;
int x_offset;
int y_offset;
bool operator<(const Info &rhs) const
{
return value < rhs.value;
}
};
void next(const Matrix<float> &m, const int &x, const int &y, int &x_n, int &y_n)
{
//todo: choose the diagonal adiacent in case of ties.
x_n = x;
y_n = y;
Info neighbours[8];
neighbours[0] = Info(m.get(x - 1, y - 1), -1, -1);
neighbours[1] = Info(m.get(x , y - 1), 0, -1);
neighbours[2] = Info(m.get(x + 1, y - 1), +1, -1);
neighbours[3] = Info(m.get(x - 1, y ), -1, 0);
neighbours[4] = Info(m.get(x + 1, y ), +1, 0);
neighbours[5] = Info(m.get(x - 1, y + 1), -1, +1);
neighbours[6] = Info(m.get(x , y + 1), 0, +1);
neighbours[7] = Info(m.get(x + 1, y + 1), +1, +1);
auto the_min = *std::min_element(neighbours, neighbours + 8);
x_n += the_min.x_offset;
y_n += the_min.y_offset;
}
int main(int, char **)
{
std::size_t xMax = 200;
std::size_t yMax = 150;
Matrix<float> cell(xMax + 2, yMax + 2);
Matrix<bool> start(xMax + 2, yMax + 2);
start.set(0.1 * xMax, 0.1 * yMax, true);
Matrix<bool> goal(xMax + 2, yMax + 2);
goal.set(0.9 * xMax, 0.9 * yMax, true);
Matrix<bool> blocked(xMax + 2, yMax + 2);
add_obstacles(blocked, 1, 1, 1);
PPM_image_save(blocked, "blocked.ppm");
PPM_image_save(start, "start.ppm");
PPM_image_save(goal, "goal.ppm");
for ( int y = 0; y <= yMax + 1; y++ )
{
for ( int x = 0; x <= xMax + 1; x++ )
{
if ( goal.get(x, y) )
{
cell.set(x, y, 0.);
}
else
{
cell.set(x, y, xMax * yMax);
}
}
}
Matrix<float> previous_cell = cell;
float values[5];
int cnt = 0;
do
{
std::ostringstream oss;
oss << "cell_" << cnt++ << ".ppm";
PPM_image_save(cell, oss.str());
previous_cell = cell;
for ( int y = 2; y <= yMax; y++ )
{
for ( int x = 2; x <= xMax; x++ )
{
if (!blocked.get(x, y))
{
values[0] = cell.get(x - 1, y ) + 1;
values[1] = cell.get(x - 1, y - 1) + 1;
values[2] = cell.get(x , y - 1) + 1;
values[3] = cell.get(x + 1, y - 1) + 1;
values[4] = cell.get(x , y );
cell.set(x, y, *std::min_element(values, values + 5));
}
}
}
for ( int y = yMax - 1; y >= 1; y-- )
{
for ( int x = xMax - 1; x >= 1; x-- )
{
if (!blocked.get(x, y))
{
values[0] = cell.get(x + 1, y ) + 1;
values[1] = cell.get(x + 1, y + 1) + 1;
values[2] = cell.get(x , y + 1) + 1;
values[3] = cell.get(x - 1, y + 1) + 1;
values[4] = cell.get(x , y );
cell.set(x, y, *std::min_element(values, values + 5));
}
}
}
}
while (distance(previous_cell, cell) > 0.);
PPM_image_save(cell, "cell.ppm");
Matrix<bool> path(xMax + 2, yMax + 2);
for ( int y_s = 1; y_s <= yMax; y_s++ )
{
for ( int x_s = 1; x_s <= xMax; x_s++ )
{
if ( start.get(x_s, y_s) )
{
int x = x_s;
int y = y_s;
while (!goal.get(x, y))
{
path.set(x, y, true);
int x_n, y_n;
next(cell, x, y, x_n, y_n);
x = x_n;
y = y_n;
}
}
}
}
PPM_image_save(path, "path.ppm");
return 0;
}
The algorithm uses the simple PPM image format explained for example in the Chapter 15 from the book Computer Graphics: Principles and Practice - Third Edition by Hughes et al. in order to save the images.
The algorithm starts from the image of the obstacles (blocked) and computes from it the distance transform (cell); then, starting from the distance transform, it computes the optimal path with a steepest descent method: it walks downhill in the transform distance potential field. So you can start from your own distance transform image.
Please note that it seems to me that the algorithm does not fulfill your additional constraint that:
the path should never reach a pixel which is less than 'x' pixels away
from an obstacle.
The following png image is the image of the obstacles, the program generated blocked.ppm image was exported as png via Gimp:
The following png image is the image of the start point, the program generated start.ppm image was exported as png via Gimp:
The following png image is the image of the end point, the program generated goal.ppm image was exported as png via Gimp:
The following png image is the image of the computed path, the program generated path.ppm image was exported as png via Gimp:
The following png image is the image of the distance transform, the program generated cell.ppm image was exported as png via Gimp:
I found the Jarvis' article after having a look at
CHIN, Yew Tuck, et al. Vision guided agv using distance transform. In: Proceedings of the 32nd ISR (International Symposium on Robotics). 2001. p. 21.
Update:
The Jarvis' algorithm is reviewed in the following paper where the authors state that:
Since the path is found by choosing locally only between neighbour
cells, the obtained path can be sub optimal
ELIZONDO-LEAL, Juan Carlos; PARRA-GONZÁLEZ, Ezra Federico; RAMÍREZ-TORRES, José Gabriel. The Exact Euclidean Distance Transform: A New Algorithm for Universal Path Planning. Int J Adv Robotic Sy, 2013, 10.266.
For a graph-based solution you can check for example chapter 15 of the book
DE BERG, Mark, et al. Computational geometry. Springer Berlin Heidelberg, 2008.
which has title "Visibility Graphs - Finding the Shortest Route" and it is freely available at the publisher site.
The chapter explains how to compute the Euclidean shortest path starting from the so-called visibility graph. The visibility graph is computed starting from the set of obstacles, each obstacle is described as a polygon.
The Euclidean shortest path is then found applying a shortest path algorithm such as Dijkstra's algorithm to the visibility graph.
In your distance transform image the obstacles are represented by pixels with zero value and so you can try to approximate them as polygons and after that apply the method described in the cited book.
In the distance transform map of pixels you choose your starting puxel and then select its neighbor with a lower value than your starting puxel - repeat the process until goal puxel is reached (a pixel with value zero).
Normally the goal pixel has value zero, the lowest number of any passable mode.
The problem of nor passing close to barriers is silver by generate the distance transform map so that barriers are enlarged. For example if you want a dustance if two pixels to any barrier - just add two pixels of barrier value. Normally the barriers wich could mot be passed is given a value of minus one.
Same value i used for the edges. An alternative approach ua to surround barriers with a very hifh starting value - the path is not guaranteed to get close, but the algorithm will try to avoid paths un the proximity of barriers.

How to get the nearest point outside a polygon from a point inside a polygon?

I have a map with a lot of polygons and a point inside in one of them, like this:
The x and y coordinates of the edges of the polygons are save in a database like this(for example):
Polygon(Point(11824, 10756), Point(11822, 10618), Point(11912, 10517), Point(12060, 10529), Point(12158, 10604), Point(12133, 10713), Point(12027, 10812), Point(11902, 10902)),
Polygon(Point(11077, 13610), Point(10949, 13642), Point(10828, 13584), Point(10772, 13480), Point(10756, 13353), Point(10849, 13256), Point(10976, 13224), Point(11103, 13294), Point(11171, 13414), Point(11135, 13558)),
Polygon(Point(11051.801757813, 11373.985351563), Point(11165.717773438, 11275.469726563), Point(11281.733398438, 11255.646484375), Point(11381.07421875, 11333.15625), Point(11440.202148438, 11467.706054688), Point(11404.73046875, 11584.534179688), Point(11301.662109375, 11643.852539063), Point(11169.486328125, 11644.079101563), Point(11067.555664063, 11579.676757813), Point(11018.21484375, 11454.750976563)),
Polygon(Point(12145, 13013), Point(12069.065429688, 13014.67578125), Point(12012.672851563, 12953.833984375), Point(11973.942382813, 12910.14453125), Point(11958.610351563, 12853.736328125), Point(11988.58203125, 12780.668945313), Point(12046.806640625, 12735.046875), Point(12117.080078125, 12729.838867188), Point(12185.567382813, 12743.389648438), Point(12225.575195313, 12803.530273438), Point(12255.934570313, 12859.2109375), Point(12263.861328125, 12914.166992188), Point(12221.2578125, 12978.983398438)),
They are drawn later, I don't have access to that, only to this coordinates.
So I have the x/y of the edges of the polygons and the x/y of the red dot. Now i have to know in which polygon the red dot is.
Then the most important I need is this:
I have the x and y coordinates of the red point and the x y coordinates of the edges.
I need the x and y coordinates of the green dot.(The nearest location outside the polygon)
I need it in lua, but feel free to answer in any language, I can translate it.
To know in which polygon the point is in you can use the Ray Casting algorithm.
For finding the closest edge you could use a naive approach computing the distance to each edge and then take the minimum. Just remember to check if the intersection with edge is outside the edge (check this). If it is outside take the distance to the closest extreme of the edge.
Ok better explaining the intuition with some pseudo-code:
dot(u,v) --> ((u).x * (v).x + (u).y * (v).y)
norm(v) --> sqrt(dot(v,v)) // norm = length of vector
dist(u,v)--> norm(u-v) // distance = norm of difference
// Vector contains x and y
// Point contains x and y
// Segment contains P0 and P1 of type Point
// Point = Point ± Vector
// Vector = Point - Point
// Vector = Scalar * Vector
Point closest_Point_in_Segment_to(Point P, Segment S)
{
Vector v = S.P1 - S.P0;
Vector w = P - S.P0;
double c1 = dot(w,v);
if ( c1 <= 0 ) // the closest point is outside the segment and nearer to P0
return S.P0;
double c2 = dot(v,v);
if ( c2 <= c1 ) // the closest point is outside the segment and nearer to P1
return S.P1;
double b = c1 / c2;
Point Pb = S.P0 + b * v;
return Pb;
}
[Point, Segment] get_closest_border_point_to(Point point, Polygon poly) {
double bestDistance = MAX_DOUBLE;
Segment bestSegment;
Point bestPoint;
foreach s in poly.segments {
Point closestInS = closest_Point_in_Segment_to(point, s);
double d = dist(point, closestInS);
if (d < bestDistance) {
bestDistance = d;
bestSegment = s;
bestPoint = closestInS;
}
}
return [bestPoint, bestSegment];
}
I think this pseudo-code should get you going, of course once you have the polygon your point is in!.
My PolyCollisions class:
public class PolyCollisions {
// Call this function...
public static Vector2 doCollisions (Vector2[] polygon, Vector2 point) {
if(!pointIsInPoly(polygon, point)) {
// The point is not colliding with the polygon, so it does not need to change location
return point;
}
// Get the closest point of the polygon
return closestPointOutsidePolygon(polygon, point);
}
// Check if the given point is within the given polygon (Vertexes)
//
// If so, call on collision if required, and move the point to the
// closest point outside of the polygon
public static boolean pointIsInPoly(Vector2[] verts, Vector2 p) {
int nvert = verts.length;
double[] vertx = new double[nvert];
double[] verty = new double[nvert];
for(int i = 0; i < nvert; i++) {
Vector2 vert = verts[i];
vertx[i] = vert.x;
verty[i] = vert.y;
}
double testx = p.x;
double testy = p.y;
int i, j;
boolean c = false;
for (i = 0, j = nvert-1; i < nvert; j = i++) {
if ( ((verty[i]>testy) != (verty[j]>testy)) &&
(testx < (vertx[j]-vertx[i]) * (testy-verty[i]) / (verty[j]-verty[i]) + vertx[i]) )
c = !c;
}
return c;
}
// Gets the closed point that isn't inside the polygon...
public static Vector2 closestPointOutsidePolygon (Vector2[] poly, Vector2 point) {
return getClosestPointInSegment(closestSegment(poly, point), point);
}
public static Vector2 getClosestPointInSegment (Vector2[] segment, Vector2 point) {
return newPointFromCollision(segment[0], segment[1], point);
}
public static Vector2 newPointFromCollision (Vector2 aLine, Vector2 bLine, Vector2 p) {
return nearestPointOnLine(aLine.x, aLine.y, bLine.x, bLine.y, p.x, p.y);
}
public static Vector2 nearestPointOnLine(double ax, double ay, double bx, double by, double px, double py) {
// https://stackoverflow.com/questions/1459368/snap-point-to-a-line-java
double apx = px - ax;
double apy = py - ay;
double abx = bx - ax;
double aby = by - ay;
double ab2 = abx * abx + aby * aby;
double ap_ab = apx * abx + apy * aby;
double t = ap_ab / ab2;
if (t < 0) {
t = 0;
} else if (t > 1) {
t = 1;
}
return new Vector2(ax + abx * t, ay + aby * t);
}
public static Vector2[] closestSegment (Vector2[] points, Vector2 point) {
Vector2[] returns = new Vector2[2];
int index = closestPointIndex(points, point);
returns[0] = points[index];
Vector2[] neighbors = new Vector2[] {
points[(index+1+points.length)%points.length],
points[(index-1+points.length)%points.length]
};
double[] neighborAngles = new double[] {
getAngle(new Vector2[] {point, returns[0], neighbors[0]}),
getAngle(new Vector2[] {point, returns[0], neighbors[1]})
};
// The neighbor with the lower angle is the one to use
if(neighborAngles[0] < neighborAngles[1]) {
returns[1] = neighbors[0];
} else {
returns[1] = neighbors[1];
}
return returns;
}
public static double getAngle (Vector2[] abc) {
// https://stackoverflow.com/questions/1211212/how-to-calculate-an-angle-from-three-points
// atan2(P2.y - P1.y, P2.x - P1.x) - atan2(P3.y - P1.y, P3.x - P1.x)
return Math.atan2(abc[2].y - abc[0].y, abc[2].x - abc[0].x) - Math.atan2(abc[1].y - abc[0].y, abc[1].x - abc[0].x);
}
public static int closestPointIndex (Vector2[] points, Vector2 point) {
int leastDistanceIndex = 0;
double leastDistance = Double.MAX_VALUE;
for(int i = 0; i < points.length; i++) {
double dist = distance(points[i], point);
if(dist < leastDistance) {
leastDistanceIndex = i;
leastDistance = dist;
}
}
return leastDistanceIndex;
}
public static double distance (Vector2 a, Vector2 b) {
return Math.sqrt(Math.pow(Math.abs(a.x-b.x), 2)+Math.pow(Math.abs(a.y-b.y), 2));
}
}
Here's a small explanation (Fun Fact: This is the first image I posted to stack overflow!)
Sorry that it's so messy...
Step-by-Step of the class:
Check if the given point is within a polygon
If it isn't, return the current point as no changes are needed.
Find the closest VERTEX of the polygon
This is not the closest POINT, as a point can be between vertexes
Grab the two neighbors of the vertex, keeping the one with a lower angle.
The lower angle has lower distance than the higher angle because the higher angle "goes away" faster
Get the closest point of the line segment, using the answer to this question on StackOverflow.
Congrats! You survived the bad tutorial! Hopefully it helped :)
+ Thanks to all of the commented links to answers that helped me help you!
Snap Point to Line
How to calculate an angle from 3 points
Here is C# version of the answer #nathanfranke
using System;
using Windows.Foundation;
using Windows.UI.Xaml.Shapes;
using System.Numerics;
public class PolyCollisions
{
// Call this function...
public static Vector2 DoCollisions(Vector2[] polygon, Vector2 point)
{
if (!PointIsInPoly(polygon, point))
{
// The point is not colliding with the polygon, so it does not need to change location
return point;
}
// Get the closest point of the polygon
return ClosestPointOutsidePolygon(polygon, point);
}
// Check if the given point is within the given polygon (Vertexes)
//
// If so, call on collision if required, and move the point to the
// closest point outside of the polygon
public static bool PointIsInPoly(Vector2[] verts, Vector2 p)
{
int nvert = verts.Length;
double[] vertx = new double[nvert];
double[] verty = new double[nvert];
for (int d = 0; d < nvert; d++)
{
Vector2 vert = verts[d];
vertx[d] = vert.X;
verty[d] = vert.Y;
}
double testx = p.X;
double testy = p.Y;
int i, j;
bool c = false;
for (i = 0, j = nvert - 1; i < nvert; j = i++)
{
if (((verty[i] > testy) != (verty[j] > testy)) && (testx < (vertx[j] - vertx[i]) * (testy - verty[i]) / (verty[j] - verty[i]) + vertx[i]))
c = !c;
}
return c;
}
// Gets the closed point that isn't inside the polygon...
public static Vector2 ClosestPointOutsidePolygon(Vector2[] poly, Vector2 point)
{
return GetClosestPointInSegment(ClosestSegment(poly, point), point);
}
public static Vector2 GetClosestPointInSegment(Vector2[] segment, Vector2 point)
{
return NewPointFromCollision(segment[0], segment[1], point);
}
public static Vector2 NewPointFromCollision(Vector2 aLine, Vector2 bLine, Vector2 p)
{
return NearestPointOnLine(aLine.X, aLine.Y, bLine.X, bLine.Y, p.X, p.Y);
}
public static Vector2 NearestPointOnLine(double ax, double ay, double bx, double by, double px, double py)
{
// https://stackoverflow.com/questions/1459368/snap-point-to-a-line-java
double apx = px - ax;
double apy = py - ay;
double abx = bx - ax;
double aby = by - ay;
double ab2 = abx * abx + aby * aby;
double ap_ab = apx * abx + apy * aby;
double t = ap_ab / ab2;
if (t < 0)
{
t = 0;
}
else if (t > 1)
{
t = 1;
}
return new Vector2((float)(ax + (abx * t)), (float)(ay + aby * t));
}
public static Vector2[] ClosestSegment(Vector2[] points, Vector2 point)
{
Vector2[] returns = new Vector2[2];
int index = ClosestPointIndex(points, point);
returns[0] = points[index];
Vector2[] neighbors = new Vector2[] {
points[(index+1+points.Length)%points.Length],
points[(index-1+points.Length)%points.Length]
};
double[] neighborAngles = new double[] {
GetAngle(new Vector2[] {point, returns[0], neighbors[0]}),
GetAngle(new Vector2[] {point, returns[0], neighbors[1]})
};
// The neighbor with the lower angle is the one to use
if (neighborAngles[0] < neighborAngles[1])
{
returns[1] = neighbors[0];
}
else
{
returns[1] = neighbors[1];
}
return returns;
}
public static double GetAngle(Vector2[] abc)
{
// https://stackoverflow.com/questions/1211212/how-to-calculate-an-angle-from-three-points
// atan2(P2.y - P1.y, P2.x - P1.x) - atan2(P3.y - P1.y, P3.x - P1.x)
return Math.Atan2(abc[2].X - abc[0].Y, abc[2].X - abc[0].X) - Math.Atan2(abc[1].Y - abc[0].Y, abc[1].X - abc[0].X);
}
public static int ClosestPointIndex(Vector2[] points, Vector2 point)
{
int leastDistanceIndex = 0;
double leastDistance = Double.MaxValue;
for (int i = 0; i < points.Length; i++)
{
double dist = Distance(points[i], point);
if (dist < leastDistance)
{
leastDistanceIndex = i;
leastDistance = dist;
}
}
return leastDistanceIndex;
}
public static double Distance(Vector2 a, Vector2 b)
{
return Math.Sqrt(Math.Pow(Math.Abs(a.X - b.X), 2) + Math.Pow(Math.Abs(a.Y - b.Y), 2));
}
}

Resources