Finding shortest way between 3d points (x,y,z) in C# - c#

Im having a lot of data stored as
public class Position{
public double X{get;set;}
public double Y{get;set;}
public double Z{get;set;}
}
Now I would like to find the shortest path between two of these Position objects by going through the array of all Position
Like finding a path in a star map with known star positions, and I want to go from Star A to Star B, which path must I take...
My Position can have doubles with negative numbers
Constraint should be something like, max distance to next position (jumprange), and of course trying to find the path that generates minimum number of Positions i need to go through...

There is a simple mathematical formula for this ,
Let's say u have :
var A = new Position();
var B = new Position();
//Assign value
Using the formula: √(A.X - B.X)^2 + (A.Y - B.Y)^2 + (A.Z - B.Z)^2) we have the following:
public static double Distance(Position A, Position B)
{
var xDifferenceSquared = Math.Pow(A.X - B.X, 2);
var yDifferenceSquared = Math.Pow(A.Y - B.Y, 2);
var zDifferenceSquared = Math.Pow(A.Z - B.Z, 2);
return Math.Sqrt(xDifferenceSquared + yDifferenceSquared + zDifferenceSquared);
}

Related

Distance between two grid cells, with no diagonal

I've been working on a small project for some days, everything was working fine until I changed my "map" implementation to be the same as in the game (Dofus) I'm based on (it's a little helper for the community).
Basically, I've a grid layout rotated at 45° (see image below), contructed from top left to bottom right. Every cell as an xIndex and zIndex to represent where it is (xIndex ; zIndex) on the image, and I just want to get the distance between two cells, without traveling diagonally.
As I tried to explain on the picture:
GetDistanceBetweenTiles(A, B) should be 3
GetDistanceBetweenTiles(A, C) should be 5
GetDistanceBetweenTiles(B, C) should be 2
I found the "Manhattan distance" which looks like it is what I want, but it's not giving me the values above.
Here is the code:
private int GetDistanceBetweenTiles(MovableObject a, MovableObject b)
{
//int dist = Mathf.Abs(a.xIndex - b.xIndex) + Mathf.Abs(a.zIndex - b.zIndex);
int minX = a.xIndex < b.xIndex ? a.xIndex : b.xIndex;
int maxX = a.xIndex > b.xIndex ? a.xIndex : b.xIndex;
int minZ = a.zIndex < b.zIndex ? a.zIndex : b.zIndex;
int maxZ = a.zIndex > b.zIndex ? a.zIndex : b.zIndex;
int distX = (maxX - minX);
int distZ = (maxZ - minZ);
int dist = Mathf.Abs(maxX - minX) + Mathf.Abs(maxZ - minZ);
print($"Distance between {a.name} and {b.name} is {dist}");
return dist;
}
Any help would be gladly appreciated.
If it can help, here is the project working with the first map implementation I did (but not translated yet).
Let make new coordinates in inclined rows with simple formulae:
row = z/2 - x ("/" for **integer division**)
col = z - row
Now we can just calculate Manhattan distance as
abs(row2 - row1) + abs(col2 - col1)
For your example
x z r c
4, 2 => -3, 5
1, 4 => 1, 4
distance = (1-(-3)) + (5-4) = 4 + 1 = 5
To explain: your grid rotated by 45 degrees:
0 1 2 3 4 5 6 7 8 \column
40|41 row -4
30|31|42|43 row -3
20|21|32|33|44|45 row -2
10|11|22|23|34|35|46|47 row -1
00|01|12|13|24|15|36|37|48 row 0
02|03|14|15|26|27|38 row 1
04|05|16|17|28 row 2
06|07|18 row 3
The "No-Maths" solution
I maybe have a workaround solution for you. I'm kind of a lazy person and very bad in maths ... so I usually let Unity do the maths for me in situations like yours ;)
For that you would need one dedicated GameObject that is rotated in the way that it represents the grid "rotation" so 0,45,0.
Then - since your tiles move always in steps of exactly 1 just in the rotated coordinate system - you could inetad of using an index based distance rather directly compare the absolute positions using Transform.InverseTransformPoint in order to get the positions relative to that rotated object.
InverseTransformPoint retuns as said the given world position in the local space of the used transform so that if the object was originally placed at e.g. x=1, z=1 in our rotated local space it will have the position z=1.1414..., x=0.
I simply attached this component to my rotated object .. actually I totate in Awake just to be sure ;)
public class PositionsManager : MonoBehaviour
{
// I know .. singleton pattern .. buuu
// but that's the fastest way to prototype ;)
public static PositionsManager Singleton;
private void Awake()
{
// just for making sure this object is at world origin
transform.position = Vector3.zero;
// rotate the object liek you need it
// possible that in your case you rather wanted -45°
transform.eulerAngles = new Vector3(0, 45, 0);
// since InverseTransformPoint is affacted by scale
// just make sure this object has the default scale
transform.localScale = Vector3.one;
// set the singleton so we can easily access this reference
Singleton = this;
}
public Vector2Int GetDistance(Transform from, Transform to)
{
var localPosFrom = transform.InverseTransformPoint(from.position);
var localPosTo = transform.InverseTransformPoint(to.position);
// Now you can simply get the actual position distance and return
// them as vector2 so you can even still see the components
// seperately
var difference = localPosTo - localPosFrom;
// since you are using X-Z not X-Y you have to convert the vector "manually"
return new Vector2Int(Mathf.RoundToInt(difference.x), Mathf.RoundToInt(difference.z));
}
public int GetAbsoluteDistance(Transform from, Trasnform to)
{
var difference = GetDistance(from, to);
return Mathf.Abs(difference.x) + Mathf.Abs(difference.y);
}
}
Now when you need to get the absolute distance you could simply do
var difference = PositionsManager.Singleton.GetDistance(objectA.transform, objectB.transform);
var absoluteDistance = PositionsManager.Singleton.GetAbsoluteDistance(objectA.transform, objectB.transform);
Little Demo (used a chess board drawer since I had that ^^)
The maths solution
It just came to me while writing the upper explenation:
You already know your steps between the tiles: It is allways Mathf.Sqrt(2)!
So again you could simply use the absolute positions in your world and compare them like
private float Sqrt2;
private void Awake()
{
Sqrt2 = Mathf.Sqrt(2);
}
...
// devide the actual difference by Sqrt(2)
var difference = (objectA.position - objectB.position) / Mathf.Sqrt(2);
// again set the Vector2 manually since we use Z not Y
// This step is optional if you anyway aren't interrested in the Vector2
// distance .. jsut added it for completeness
// You might need the rounding part though
var fixedDifference = new Vector2Int(Mathf.RoundToInt(difference.x), Mathf.RoundToInt(difference.z));
// get the absolute difference
var absoluteDistance = Mathf.Abs(fixedDifference.x) + Mathf.Abs(fixedDifference.y);
...
still completely without having to deal with the indexes at all.

Formula for Calculating a Final Score

I'm stuck on one final piece of a calculation puzzle below. I know how to generate a percentage score of correct parts from total correct possible parts ((correctNumPartsOnBoard / totalPossibleCorrectParts)*100) but I want to the final percentage score to factor in the number the incorrect parts on the board as well. (even if all the right parts are on the board you still won't get 100% if there are also incorrect parts). Right now my current formula percentCorrectParts = ((correctNumPartsOnBoard / totalPossibleCorrectParts) / totalNumPartsOnBoard) * 100); is wrong and I'm having trouble pinpointing the correct calculation.
So, the way the calc would need to work is: a user needs to match one of the six possible animals, each animal has around 15 correct parts, but users can also drag incorrect parts onto the board (parts from the other animals are still visible so they could drag a different set of legs or horns on a lizard head, they could make frankenstein type creatures as well this way). So the total number of parts available would be 6*15. But seeing as how they're not all correct they would influence the score as well by bringing the overall score average of pieces on the board down.
What's the correct formula for this?
// Scoring System
using UnityEngine;
using System.Linq;
using System.Collections.Generic;
public class ScoreManager : MonoBehaviour
{
public List<string> totalBuildBoardParts; // Running list of all parts on board (by Tag)
public int numCorrectPartsOnBoard;
public int numIncorrectPartsOnBoard;
public int totalPossibleCorrectParts;
public float percentCorrectParts;
void Start()
{
GameObject gameController = GameObject.FindGameObjectWithTag("gc");
GameSetup gameSetup = gameController.GetComponent<GameSetup>();
totalPossibleCorrectParts = gameSetup.totalPossibleCorrectParts;
Debug.Log("TOTAL POSSIBLE CORRECT PARTS ARE: " + totalPossibleCorrectParts);
}
public void AddAnimalPartByTag(string tag)
{
// Add object tag to List
totalBuildBoardParts.Add(tag);
Debug.Log ("Added an object tagged as: " + tag);
GameObject gameController = GameObject.FindGameObjectWithTag("gc");
GameSetup gameSetup = gameController.GetComponent<GameSetup>();
if (tag == gameSetup.activeTag)
{
numCorrectPartsOnBoard ++;
Debug.Log ("There are " + numCorrectPartsOnBoard + " correct parts on the board");
} else {
numIncorrectPartsOnBoard ++;
}
CalculateScore();
}
public void RemoveAnimalPartByTag(string tag)
{
// Add object tag to List
totalBuildBoardParts.Remove(tag);
Debug.Log ("Removed an object tagged as: " + tag);
GameObject gameController = GameObject.FindGameObjectWithTag("gc");
GameSetup gameSetup = gameController.GetComponent<GameSetup>();
if (tag == gameSetup.activeTag)
{
numCorrectPartsOnBoard --;
Debug.Log ("There are " + numCorrectPartsOnBoard + " correct parts on the board");
} else {
numIncorrectPartsOnBoard --;
}
CalculateScore();
}
public void CalculateScore()
{
float totalNumPartsOnBoard = totalBuildBoardParts.Count();
float correctNumPartsOnBoard = numCorrectPartsOnBoard;
percentCorrectParts = ((correctNumPartsOnBoard / totalPossibleCorrectParts) / totalNumPartsOnBoard) * 100);
Debug.Log ("Your current score is: " + percentCorrectParts);
}
}
Your formula is probably correct. However, your datatypes are not.
You are currently doing an integer division, which results in an int too. So let's say that correctNumPartsOnBoard is 3 and totalPossibleCorrectParts is 5, 3/5 gives 0 because an int does not have any decimals.
You need to cast one of the two operands in the division as a datatype with decimals ( float, double or decimal for example):
percentCorrectParts = ((correctNumPartsOnBoard / (float)totalPossibleCorrectParts) / totalNumPartsOnBoard) * 100);
By setting denominator totalPossibleCorrectParts as a float, the first division will return a float. That float is then used in the second division, also returning correctly a float.
I think your formula should look like this:
int correctParts;
int possibleCorrect;
int incorrectParts;
int parts;
float percentFinished =
Mathf.Max((((float)correctParts/possibleCorrect) // Correct percent
- ((float)incorrectParts/parts)) // Minus incorrect percent
* 100f, // Normalized to 100
0f); // Always a minimum of 0
Also with this formula unlike other answers, you don't have to use all of the parts to get 100%, just get the total possible correct parts which doesn't necessarily have to use up all of your parts ;)
Scenario
Lets say you have 100 parts, with 3 right and 3 wrong. Total right we are aiming for here is 20.
int correctParts = 3;
int possibleCorrect = 20;
int incorrectParts = 3;
int parts = 100;
float percentFinished =
Mathf.Max((((float)correctParts/possibleCorrect) // Correct percent is 0.15 or 15%
- ((float)incorrectParts/parts)) // Minus incorrect percent which is .03 or 3%
* 100f, // Normalized to 100 which gives us 15% - 3% = 12%
0f); // Always a minimum of 0
I think your final (%age) score should be:
correctNumPartsOnBoard / totalNumPartsOnBoard * 100
If you have 80 correctparts and 20 incorrect then the total parts is 100 and you've got 80 of them correct so you should score 80% like this:
80 / (80+20) * 100

How to find the interval of a constant moviment in a bezier path

I get the bezier value from a path like this (note: If there is another/better way to do that let me know, please):
public static Vector3 PathCubic (float t, Vector3[] path)
{
if (t >= 1f)
return path[path.Length - 1];
// projects t in the path
var projT = path.Length / 3 * t;
// what interval t is (between P1 and P2 or between P2 and P3, etc)
var range = (int)projT;
// get the interval index
var i = range * 3;
var p0 = path[i + 0]; // first point
var c0 = path[i + 1]; // control 1
var c1 = path[i + 2]; // control 2
var p1 = path[i + 3]; // second point
// calculate bezier in the current interval
return Cubic(projT - range, p0, c0, c1, p1);
}
So, supposing points P1, P2, P3, P4, t = 0.0 is the first point P1 and t = 1.0 is the last point P4.
But, that doesn't give me a constant moviment over the bezier. Image bellow depicts what I mean. A t = 0.5 give me different positions depending on points location.
I found I have to calculate the length of the path in order to achieve that. And here comes the problem. Once calculated this length, how can I calculate the interval index, as I did before? Or I will need to walk through each interval (for instruction) in order to find that?
// get the ***interval index***
var i = range * 3;
var p0 = path[i + 0]; // first point
var c0 = path[i + 1]; // control 1
var c1 = path[i + 2]; // control 2
var p1 = path[i + 3]; // second point
There is no true solution but here is a usefull approximation:
First get the length of each segment. Let's say it's:
// |a b| is the operator of length between two points a and b
A=|P1 P2|=3.0
B=|P2 P3|=1.2
C=|P3 P4|=0.8
L=A+B+C=5.0 // length of the whole spline
Your t=0.5 and is between points P1 and P2 - we know that because t*L = 2.5 and length A=3 (and A is first, so the immaginary length before was 0).
We also know that the value of t that we want should be on 2.5/3.0 =~ 0.833 of the length between points P1 and P2.
Note that P1was calculated with t = 0 and P2 with t =~ 0.333.
We can now find the fixed value of t: Tfix=Mathf.Lerp(0, 0.333, 0.833) =~ 0.277, where:
0 - t of point P1
0.333 - t of point P2
0.833 - the approximated position of searched t relative to closest approximation points.
Finally to find the point you were searching for substitute Tfix for t: PathCubic(Tfix)
Also, think about using more approximation points. For more demanding uses I needed dividing the segment in 16 pieces.
Mathematical answer: in general you can't. There is no symbolic way to determine the length of your cubic curve (it'd require solving a 6th order polynomial, which cannot be with regular formula manipulations), so there's no way to straight-up compute which t value you need for distance d along your curve.
The fastest solution is typically to index your curve: at fixed t intervals, build a lookup table with the length of the curve up to that point. Then use that LUT to do estimation. If your curve is length 42, and your LUT is distance = [0,5,12,25,37,42] for corresponding t = [0,1/5,2/5,3/5,4/5,1] then now you can use that data at moment to quickly estimate which t values you need to highlight which fixed distances, using simple linear interpolation.

XNA CatmullRom Curves

I'm in need of some clarification over a technique I'm trying. I'm trying to move an entity from point A to point B, but I don't want the entity to travel in a straight line.
For example if the entity is positioned at x: 0, y:0 and I want to get to point x:50, y: 0, I want the entity to travel in a curve to the target, I would imagine the maximum distance it would be away is x:25 y: 25 so it's travelled on the X towards the target but has moved away from the target on the y.
I've investigated a couple of options including splines, curves but what I thought would do the job is the CatmullRom curve. I'm a bit confused how to use it? I want to know where to move my entity each frame rather than what the function returns which is the interpolation. I would appreciate some gudiance as to how to use it.
If there's any alternative methods which might be easier that I've missed, I'd appreciate hearing them as well.
Edit:
To show how I'm getting a curve:
Vector2 blah = Vector2.CatmullRom(
StartPosition,
new Vector2(StartPosition.X + 5, StartPosition.Y + 5),
new Vector2(StartPosition.X + 10, StartPosition.Y + 5),
/*This is the end position*/
new Vector2(StartPosition.X + 15, StartPosition.Y), 0.25f);
The idea eventually is I generate these points on the fly but I'm just trying to work it out at the moment.
As you've noticed, splines produce line segments of different lengths. The tighter the curve, the shorter the segments. This is fine for display purposes, not so useful for path generation for mobiles.
To get a reasonable approximation of a constant-speed traversal of a spline path, you need to do some interpolation along the segments of the curve. Since you already have a set of line segments (between pairs of points returned by Vector2.CatmullRom()) you need an method of walking those segments in constant speed.
Given a set of points and a total distance to move along the path defined as lines between those points, the following (more-or-less pseudo-)code will find a point that lies a specific distance along the path:
Point2D WalkPath(Point2D[] path, double distance)
{
Point curr = path[0];
for (int i = 1; i < path.Length; ++i)
{
double dist = Distance(curr, path[i]);
if (dist < distance)
return Interpolate(curr, path[i], distance / dist;
distance -= dist;
curr = path[i];
}
return curr;
}
There are various optimizations you can do to speed this up, such as storing the path distance with each point in the path to make it easier to lookup during a walk operation. This becomes more important as your paths get more complex, but is probable overkill for a path with only a few segments.
Edit: Here's an example that I did with this method in JavaScript a while back. It's a proof-of-concept, so don't look too critically at the code :P
Edit: more information on spline generation
Given a set of 'knot' points - being points that a curve must pass through in sequence - the most obvious fit for a curve algorithm is Catmull-Rom. The downside is that C-R needs two additional control points that can be awkward to generate automatically.
A while back I found a fairly useful article online (which I can't locate anymore to give correct attribution) that calculated a set of control points based on the locations of sets of points within your path. Here's my C# code for the method that calculates the control points:
// Calculate control points for Point 'p1' using neighbour points
public static Point2D[] GetControlsPoints(Point2D p0, Point2D p1, Point2D p2, double tension = 0.5)
{
// get length of lines [p0-p1] and [p1-p2]
double d01 = Distance(p0, p1);
double d12 = Distance(p1, p2);
// calculate scaling factors as fractions of total
double sa = tension * d01 / (d01 + d12);
double sb = tension * d12 / (d01 + d12);
// left control point
double c1x = p1.X - sa * (p2.X - p0.X);
double c1y = p1.Y - sa * (p2.Y - p0.Y);
// right control point
double c2x = p1.X + sb * (p2.X - p0.X);
double c2y = p1.Y + sb * (p2.Y - p0.Y);
// return control points
return new Point2D[] { new Point2D(c1x, c1y), new Point2D(c2x, c2y) };
}
The tension parameter adjusts the control point generation to change the tightness of the curve. Higher values result in broader curves, lower values in tighter curves. Play with it and see what value works best for you.
Given a set of 'n' knots (points on the curve), we can generate a set of control points that will be used to generate the curves between pairs of knots:
// Generate all control points for a set of knots
public static List<Point2D> GenerateControlPoints(List<Point2D> knots)
{
if (knots == null || knots.Count < 3)
return null;
List<Point2D> res = new List<Point2D>();
// First control point is same as first knot
res.Add(knots.First());
// generate control point pairs for each non-end knot
for (int i = 1; i < knots.Count - 1; ++i)
{
Point2D[] cps = GetControlsPoints(knots[i - 1], knots[i], knots[i+1]);
res.AddRange(cps);
}
// Last control points is same as last knot
res.Add(knots.Last());
return res;
}
So now you have an array of 2*(n-1) control points, which you can then use to generate the actual curve segments between the knot points.
public static Point2D LinearInterp(Point2D p0, Point2D p1, double fraction)
{
double ix = p0.X + (p1.X - p0.X) * fraction;
double iy = p0.Y + (p1.Y - p0.Y) * fraction;
return new Point2D(ix, iy);
}
public static Point2D BezierInterp(Point2D p0, Point2D p1, Point2D c0, Point2D c1, double fraction)
{
// calculate first-derivative, lines containing end-points for 2nd derivative
var t00 = LinearInterp(p0, c0, fraction);
var t01 = LinearInterp(c0, c1, fraction);
var t02 = LinearInterp(c1, p1, fraction);
// calculate second-derivate, line tangent to curve
var t10 = LinearInterp(t00, t01, fraction);
var t11 = LinearInterp(t01, t02, fraction);
// return third-derivate, point on curve
return LinearInterp(t10, t11, fraction);
}
// generate multiple points per curve segment for entire path
public static List<Point2D> GenerateCurvePoints(List<Point2D> knots, List<Point2D> controls)
{
List<Point2D> res = new List<Point2D>();
// start curve at first knot
res.Add(knots[0]);
// process each curve segment
for (int i = 0; i < knots.Count - 1; ++i)
{
// get knot points for this curve segment
Point2D p0 = knots[i];
Point2D p1 = knots[i + 1];
// get control points for this curve segment
Point2D c0 = controls[i * 2];
Point2D c1 = controls[i * 2 + 1];
// calculate 20 points along curve segment
int steps = 20;
for (int s = 1; s < steps; ++s)
{
double fraction = (double)s / steps;
res.Add(BezierInterp(p0, p1, c0, c1, fraction));
}
}
return res;
}
Once you have run this over your knots you now have a set of interpolated points that are a variable distance apart, distance depending on the curvature of the line. From this you run the original WalkPath method iteratively to generate a set of points that are a constant distance apart, which define your mobile's progression along the curve at constant speed.
The heading of your mobile at any point in the path is (roughly) the angle between the points on either side. For any point n in the path, the angle between p[n-1] and p[n+1] is the heading angle.
// get angle (in Radians) from p0 to p1
public static double AngleBetween(Point2D p0, Point2D p1)
{
return Math.Atan2(p1.X - p0.X, p1.Y - p0.Y);
}
I've adapted the above from my code, since I use a Point2D class I wrote ages ago that has a lot of the functionality - point arithmetic, interpolation, etc - built in. I might have added some bugs during translation, but hopefully they'll be easy to spot when you're playing with it.
Let me know how it goes. If you run into any particular difficulties I'll see what I can do to help.

Find limiting point between 2 points

I have the coordinate of point A and another point (like B, C or D).
I also have the distance between A and the other point.
I know the maximum allowed distance between A and the other point (illustrated with the purple line and the imaginary circle).
Question: How do I find the coordinates of the red points (B1 or C1 or D1).
Example: A=(-1,1), E=(3,-8), Max allowed distance = 4. What is the coordinate of point E1?
Here is an image of the problem:
Note:
I found 2 other questions that are pretty similar or equal but I'm not able to work it out with those:
Finding coordinates of a point between two points?
How can I find a point placed between 2 points forming a segment using only the partial length of the segment?
P.S. This is not homework I need this for a programming problem but I forgot my Maths...
Assuming A is a position vector, B is a position vector, and maxLength is the max length you're allowing for.
A and B are Vector2's (as you tagged this question xna).
// Create a vector that describes going from A to B
var AtoB = (B - A);
// Make a vector going from A to B, but only one unit in length
var AtoBUnitLength = Vector2.Normalize(AtoB);
// Make a vector in the direction of B from A, of length maxLength
var AtoB1 = AtoBUnitLength * maxLength;
// B1 is the starting point (A) + the direction vector of the
// correct length we just created.
var B1 = A + AtoB1;
// One liner:
var B1 = A + Vector2.Normalize(B - A) * maxLength;

Categories

Resources