Getting Heuristic of 2 coordinates for A* algorithm - c#

For my school project im making a 2D tile map with A* algorithm to find shortest path through obstacles. I have used a formula to get heuristic score for the next tiles from http://www.growingwiththeweb.com/2012/06/a-pathfinding-algorithm.html
This is the function for getting heuristic
public static int geth(int cx, int cy, int ex, int ey)
{
//cx = current position x
//cy = current position y
//ex = end (goal) position x
//ey = end (goal) position y
int c = 14;
int d_min = Math.Min(cx - ex, cy - ey);
int d_max = Math.Max(cx - ex, cy - ey);
int h = c * d_min + (d_max - d_min);
if (h < 0) //make h positive in case it's negative
{
h = h * -1;
}
return h;
}
This works when start point is higher on y-axis than end point, but doesn't find the most efficient path when start is lower on y-axis.
I've added a console version of my problem. The most efficient should be diagonally going up, but it takes a wrong path.
(blue 'C' are the nodes checked, green 'P' path made, red 'N' still to be checked, others are not yet reached)

In the linked article, I see this algorithm:
The pipe symbols | around the differences are the absolute value function, and this is represented in C# as Math.Abs.
Distance does not have a sign attached to it. cx - ex will be negative when ex is larger than cx (and likewise with cy - ey) which can cause your Math.Min and Math.Max to choose the wrong dimension for d_min and d_max. You should use Math.Abs to remove the sign from the quantities being compared.
e.g.
int d_min = Math.Min(Math.Abs(cx - ex), Math.Abs(cy - ey));
(And likewise for d_max.)

Related

How to get a parabola shape according to a moved point and nearest points

Being not very good at math, I have a problem with my project.
The objective is boundary correction on 3D files.
In my application, the user moves a 3D point on X-axis in order to correct or modify the boundary of the object.
I want to move the nearest boundary points in the same direction but decreasingly. I mean no point should move more than the main point. The nearest points move most and, the farthest points should move less.
On the image, the red dots represent the initial status of points. And the user pulls the P0 in the x-direction. And the other points follow it. The last status of the points is represented by violet dots.
Here is what I tried.
//On point moved event
//Get nearest boundary Points (Uses Geometry3D to get boundary points).
(var clothDMesh, _) = Utilities3D.BuildDMesh(baseMesh);
CreateClothModel(clothDMesh);
var boundryVertices = nodes.Where(ro => ro.Value.isBorder).Select(ro => ro.Value.vertex).ToList();
var refPoint = CustomPoint.FromPoint3D(movedPoint);
//Gets the delta X.
var deltaX = p.X - initialX;
//Gets nearest country points, so 15 points above and 15 points below to move only a given number of points (I know maybe this should be calculated according to delta).
var nearestPoints = refPoint.GetNearesPoints(boundryVertices, 30);
foreach (var item in nearestPoints)
{
//This is only one of what I tried as a function. None of them worked correctly.
item.X += deltaX - (deltaX * 1/ Math.Pow(item.Distance, 2));
}
Any help will be appreciated.
Thanks in advance.
Here's the math part:
I call "a" your "deltaX".
We also need a second parameter: "b", the maximum height of the red dots. I assume it is symetrical and "-b" would be the minimum height of the red dots.
So, if you look for the value X, horizontal move, in fonction of the coordinate Y of the dot:
X = a - a * Y * Y / (b * b);
You can verify that for Y = 0, you obtain X = a and for Y = b (or -b) you get X = 0.
You have your parabola (X is function of Y^2).

How to find closest point on multisegment line

I'm trying to find a solution for best performance.
I need to find closet point on multisegment line (List points) to given point.
My line have thousands of points and I need to check distance to this line few times per second. So solution need to be very fast.
Right now I have something like below. It works but it is going to be slow when line have 10000+ points.
Maybe someone have idea how to make it faster?
public static float GetSqrDistXZ(Vector3 a, Vector3 b)
{
Vector3 vector = new Vector3(a.x - b.x, 0, a.z - b.z);
return vector.sqrMagnitude;
}
public static Vector3 NearestPointOnFiniteLine(Vector3 start, Vector3 end, Vector3 pnt)
{
Vector3 line = (end - start);
float len = Mathf.Sqrt(line.sqrMagnitude);
line.Normalize();
Vector3 v = pnt - start;
float d = (v.x * line.x) + (v.z * line.z);
d = Mathf.Clamp(d, 0f, len);
return start + line * d;
}
int pointsCount = line.points3.Count - 1; // line - List<Vector3> points.
float[] distances = new float[pointsCount];
for (int i = 0; i < pointsCount+1; i++) {
if (i >= 1) {
distances [i - 1] = GetSqrDistXZ (point, NearestPointOnFiniteLine (line.points3 [i - 1], line.points3 [i], point));
}
}
int minListIndexLeft = System.Array.IndexOf (distances, Mathf.Min (distances));
float minimalDistance = distances[minListIndexLeft];
Vector3 closestPoint = NearestPointOnFiniteLine (line.points3[minListIndexLeft], line.points3[minListIndexLeft+1], point);
You'll want to think about space partitioning. In this example I'm going to assume a 2D space, but this works in 3D just as well. Also there are much better solutions like BSP trees and stuff, but we'll keep it simple here.
Imagine putting a grid over your 2D space. Every segment (distance between 2 points) of your line intersects with one or more cells of that grid. What you have to do is to store the intersecting segments for every cell. If your line does not change, you can do that in one single pass on startup, or even store that information statically in an Asset.
But once you have that information, all you have to do is calculate the cell that your point is inside and then only check the line segments that intersect with that specific cell or a number of direct neighbours (see below). This makes finding the closest point lightning fast in comparison.
If you play with this idea on a piece of paper you may come across cases where this solution does not yield the closest point, because it did not consider a neighboring cell that contained a closer point. The easiest way to solve this is the following approach:
1. Find cell C, which is the cell your point is in
2. Let cellRange = 0
3. Let point B be undefined
4. Find closest point P among all segments that intersect cell C and its neighboring cells of range cellRange*
5. If B is the same as newly found point P then P is the solution. You are done.
6. Increase cellRange by 1
7. Let B = P
8. Repeat from step 4
* "neighboring cells of range cellRange" means:
cellRange 0: only cell C, no neighbours
cellRange 1: cell C and direct neighbours
cellRange 2: cell C, direct neighbours and their direct neighbours
...
This solution basically checks if increasing the search range improves the solution. As soon as increasing the range did not improve the solution, you found the closest point.

What's the best way to create a line that starts at a point on another line and extends at a given angle for a given length?

I have a program where I need to recursively generate new lines off of parent lines so that it ends up looking like a tree. The problem I'm having is that I don't know how to make the child line at angle A, where A is between 10 and 80 or 100 and 170 degrees, relative to its parent line.
My current algorithm steps are the following:
choose a random length (call it newBranchDist) from the parent's (x1, y1) point that is less than the total parent line's length (using distance formula)
Find the new line's x-coordinate using the Math.Cos(A) times newBranchDist
Find the new line's y-coordinate using Math.Sin(A) times newBranchDist
Now we have the (x1, y1) coordinate for one point of the line.
At this point, I need to calculate (x2, y2) that is at angle A relative to the parent line. Any suggestions?
EDIT: Also, my program will randomly choose which side of the parent line to draw the new line. So, sometimes it will be angle A, other times it will be A + 90.
Making some assumption on the result, as I'm not clear exactly what you're aiming for, I'd say that the calculation is as follows.
int x2 = x1 + newBranchDist * Math.Cos(a);
int y2 = y1 + newBranchDist * Math.Sin(a);
Then, you can verify the length by Pythagorean theorem.
double lengthSquared = Math.Pow(x2 - x1, 2) + Math.Pow(y2 - y1, 2);
double lengthRooted = Math.Pow(lengthSquared, 0.5);
you would need to know the angle of the parent line as well as the angle of the child line. Your terms are confusing so I am going to use a little different. And it needs to be noted that the angles of all the branches should be stored as angles relative to horizontal, although you will need to calculate them relative to the parent branch to do your 10-80,100-170 thing. But the calculation for the angle from horizontal is easy enough and given below:
1. figure out origin of the new branch<p>
a. BreakOffDistance = a random number less than the parent length (random distance from the start of the parent branch)
b. NewBranchOriginX = ParentBranchOriginX + BreakOffDistance * cos(ParentBranchAngle);
c. NewBranchOriginY = ParentBranchOriginY + BreakOffDistance * sin(ParentBranchAngle);
2. figure out a random angle to the new, child line;
a. figure out random angle between 10 and 80 or 100 and 170.
b. NewBranchAngle = ParentBranchAngle - 90 + RandomAngle.
(all branch angles relative to horizontal, right?)
3. figure out random length of new branch - less than parent?
4. The previous steps determine the new branch - origin point, angle and length. But to figure out its endpoint so you can draw it:
a. NewBranchEndX = NewBranchOriginX + NewBranchLength * cos(NewBranchAngle);
b. NewBranchEndY = NewBranchOriginY + NewBranchLength * sin(NewBranchAngle);
Because screen coordinates are turned upside down, you might need to replace the plus signs in steps 1b, 1c, and 4a, and 4b with minus signs.
Also, if you are trying to simulate a tree, I don't think it is right to choose the angle of a new branch as 10-80 or 100-170. Branches in a tree prefer to grow out and up, and it is not hard to do that in figuring out the angle for each new branch. Finally, your tree would be much more realistic if you thought of it in three dimensions. Have a tree that grows branches towards and away from you as well as to the sides. That too would be fairly simple, but more than you asked for.
With a class like this you could do the calculations first and draw the whole thing in a third step.
This code is completely untestet. There will be some errors, but you might get an idea how one could do this...
public class MyLine {
private Random random;
public MyLine(int Level, PointF Start, PointF End, int Angle) {
this.random = new Random();
this.Level = Level;
this.Start = Start;
this.End = End;
this.Angle = Angle;
}
public int Level{get;set;}
public PointF Start { get; set; }
public PointF End { get; set; }
public float MyLength {
get {
return (float)Math.Sqrt(Math.Pow(End.X - Start.X, 2) + Math.Pow(End.Y - Start.Y, 2));
}
}
public int Angle { get; set; }
public MyLine MySideLine { get; set; }
public void CalculateSideLine() {
float middleX = Start.X + (End.X - Start.X) / 2f;
float k = (End.Y - Start.Y) / (End.X - Start.X);
float d = (End.X * Start.Y - Start.X * End.Y) / (End.X - Start.X);
float middleY = k * middleX + d;
PointF newStart = new PointF(middleX, middleY);
int angle = random.Next(10, 80);
if (random.Next(0, 1) == 0)
angle = angle + 90;
float LengthPercentage = (float)random.NextDouble();
if (LengthPercentage < 0.5)
LengthPercentage = 0.5f;
float newLength = MyLength * LengthPercentage;
//Now we know the starting point of the new line, its angle and the length
//I do not have enough time to write the complete calculation down but it's result would be a new endPoint
//You think of a circle with its middle on "newStart" and its radius = "newLength".
//This circle you'll have to intersect with the line through "newStart" with the given angle.
//There are two results, you have to choose the one in the right direction
PointF newEnd = new PointF(0, 0); //This you'll have to find yourself...
this.MySideLine = new MyLine(this.Level++, newStart, newEnd, angle);
//this will calculate a new nested side line and - kind of recursively - go deeper and deeper.
//you'll have to find a break condition on a certain level.
this.MySideLine.CalculateSideLine();
//Be aware of randomly angle of 90 or 0. you might want to calculate two side lines on each twig (otherwise it will not look like a tree)
}
//This will draw the line to a Graphics (e.g. look at Form.CreateGraphics() )
//it will kind of recursively draw down the full tree.
public void DrawMeAndMySideLine(Graphics g){
g.DrawLine(Pens.Black,this.Start,this.End);
this.MySideLine.DrawMeAndMySideLine(g);
}
}

Finding the point corresponding to an arc length on the ellipse iteratively

Given a distance (arc length) anticlockwise away from a known point (P_0) on an ellipse, I am trying to find the point at that distance (P_1).
Since I cannot evaluate the t corresponding to a specific arc length analytically, I am forced to iterate through each discrete point until I arrive at an answer.
My initial code is something like this:
// t_0 is the parametric t on the ellipse corresponding to P_0
Point GetPos(double distance, double t_0, double res = 5000, double epsilon = 0.1)
{
for(int i = 0; i < res; ++i)
{
// The minus is to make the point move in an clockwise direction
t = t_0 - (double)(i)/(double)res * t_0;
// Find the integral from t to t_0 to get the arc length
// If arc length is within epsilon, return the corresponding point
}
}
Unfortunately, this code may not converge if the arc length given by the t value just nicely overshoots the epsilon value. And since this is a loop that decreases t, the overshoot will not be corrected.
I was thinking of modelling this as a control problem, using something like a PID controller. However, I realised that since the set point (which is my desired arc length), and my output (which is essentially the parametric t), are referring to different variables, I do not know how to proceed.
Is there a better method of solving this kind of problem or am I missing something from my current approach?
After some thought I used a binary search method instead, since a PID controller is difficult to tune and usually does not converge fast enough for all cases of the ellipses under consideration.
double max = t_0; double min = 0; double result = 0; double mid = 0;
mid = (max - min) / 2.0;
while ((Math.Abs(distance - result) > epsilon))
{
result = // Arc Length from t_0 to mid
if (result > distance)
{
min = mid;
mid = ((max - mid) / 2.0) + min;
}
else
{
max = mid;
mid = (mid - min) / 2.0;
}
}
// Return the point at t = max
The binary search works as the search is always over an ordered range (from t_0 to 0).

Randomly pick two numbers in a range so that the sum of their squares is constant

I'm currently developing a 2-player Ping-Pong game (in 2D - real simple) from scratch, and it's going good. However Theres a problem I just can't seem to solve - I'm not sure if this should be located here or on MathExchange - anyway here goes.
Initially the ball should be located in the center of the canvas. When pressing a button the ball should be fired off in a completely random direction - but always with the same velocity.
The Ball object has (simplified) 4 fields - The position in X and Y, and the velocity in X and Y. This makes it simple to bounce the ball off the walls when it hits, simple by inverting the velocities.
public void Move()
{
if (X - Radius < 0 || X + Radius > GameWidth)
{
XVelocity = -XVelocity;
}
if (Y - Radius < 0 || Y + Radius > GameHeight)
{
YVelocity = -YVelocity;
}
X+= XVelocity;
Y+= YVelocity;
}
I figured the velocity should be the same in each game, so I figures I would use Pythagoras - the square of the two velocities should always be the same.
SO for the question:
Is there a way to randomly select two numbers (doubles) such that the sum of their squares is always a specific number - more formally:
double x = RandomDouble();
double y = RandomDouble();
if (x^2 + y^2 = 16) {/* should always be true */ }
Any help appreciated :)
Randomly pick an angle theta and multiply that by the magnitude of the distance d you want. Something like:
double theta = rand.NextDouble() * 2.0 * Math.PI;
double x = d * Math.Cos(theta);
double y = d * Math.Sin(theta);
If the constant is C, pick a number x between 0 and sqrt(C).
Solve for the other number y using simple algebra.
why not try this:
double x = RandomDouble();
double y = square(16-x^2);
as your application allow double type.
does this solve your problem?
if not, please let me know

Categories