I constructed a small function to check if a group of points are coplanar:
public static bool IsCoplanar(Point[] points)
{
// Ensure there are greater than three points (otherwise always coplanar)
if (points.Length < 4)
{
return true;
}
Point pointA = points[0];
Point pointB = points[1];
Point pointC = points[2];
// Calculate the scalar triple product using vectors formed from
// the first three points and each successive point to check that
// the point is on the same plane as the first three.
Vector vectorBA = pointB - pointA;
Vector vectorCA = pointC - pointA;
for (int i = 3; i < points.Length; i++)
{
Point pointD = points[i];
Vector vectorDA = pointD - pointA;
if (!(System.Math.Abs(vectorBA.Dot(vectorCA.Cross(vectorDA))) < Epsilon))
{
return false;
}
}
return true;
}
Unfortunately, it seems to be returning true in the case, for example, starting with 3 coplanar points:
(-50, 50, -50)
(-50, -50, -50)
(-50, -50, 50)
Which are fine. But if you add:
(50, -50, 50)
(50, -50, -50)
To the list and run again, it still returns true.
I've been looking at this for ages but haven't been able to spot the problem, does anyone have any idea?
Thanks.
Here is code in C#. Note, operations like Cross and Equal should be class operators, I did not do that. Also, I included edge case testing for things like coincidental points. I.E. what happens if input are non-unique points, like (50,50,50) followed by (50,50,50), your current code fails!
public static bool IsCoplanar(MyPoint[] points)
{
if (points.Length <= 3)
return true;
//input points may be the coincidental/same (edge case),
//so we first need to loop to find three unique points.
//the first unique point is by default at position 0,
//so we will start looking for second at position 1:
int unique_point2_index = 0;
int unique_point3_index = 0;
bool found_point2 = false;
bool found_point3 = false;
for (int i = 1; i < points.Length; ++i )
{
if (!found_point2)
{
if (!Equals(points[0], points[i]))
{
found_point2 = true;
unique_point2_index = i;
}
}
else if (!found_point3)
{
if (!Equals(points[0], points[i]) && !Equals(points[unique_point2_index], points[i]))
{
found_point3 = true;
unique_point3_index = i;
}
}
else
break;
}
//if we did not find three unique points, then all of the points are coplanar!
if (!found_point3)
return true;
//we found three unique points lets loop through the rest and check if those
//are also coplanar. We do that as following:
//First compute the plane normal:
MyPoint P1 = points[0];
MyPoint P2 = points[unique_point2_index];
MyPoint P3 = points[unique_point3_index];
MyPoint vecP1P2 = Minus(P2, P1); //Should be class operator, P2 - P1
MyPoint vecP1P3 = Minus(P3, P1);
MyPoint normal = Cross(vecP1P2, vecP1P3);
//Secondly, for the remainder of points, we compute
//a vector from P1 to each point,
//and take the dot product with the normal.
//This should be zero (+- epsilon) for coplanar points
for (int i = unique_point3_index + 1; i < points.Length; ++i)
{
MyPoint testVec = Minus(points[i], P1);
double dot = Dot(testVec, normal);
//include error boundary for double precision
if (Math.Abs(dot) > 0.000001)
return false;
}
return true;
}
Nothing obvious about the code jumps out at me, but you might try a slightly different approach.
Given the formula for a plane:
Ax + By + Cz + D = 0
take your first three points, which define a plane, and generate the coefficients A, B, C and D.
For the rest of the points, then check:
Point v;
float d = A * v.x + B * v.y + C * v.d;
d is now the distance from that point to the plane, along the plane's normal.
If d is less than D (the distance of the plane to the origin along its normal), the point is behind the plane (ie, opposite side of the plane from which the normal is pointing). If d is greater than D, the point is in front of the plane.
if abs(d - D) < float.Epsilon), then the point may safely be assumed to lie in the plane.
Example (from this site)
Given points P, Q, R in space, find the equation of the plane through the 3 points.
If P = (1, 1, 1), Q = (1, 2, 0), R = (-1, 2, 1).
We seek the coefficients of an equation ax + by + cz = d, where P, Q and R satisfy the equations, thus:
a + b + c = d
a + 2b + 0c = d
-a + 2b + c = d
Subtracting the first equation from the second and then adding the first equation to the third, we eliminate a to get
b - c = 0
4b + c = 2d
Adding the equations gives 5b = 2d, or b = (2/5)d, then solving for c = b = (2/5)d and then a = d - b - c = (1/5)d.
So the equation (with a nonzero constant left in to choose) is d(1/5)x + d(2/5)y + d(2/5)z = d, so one choice of constant gives
x + 2y + 2z = 5
or, A = 1, B = 2, C = 2, and D = -5.
Once you have those, checking the rest of the points is simply substituting the point's x,y,z into the plane equation, and comparing the output to the D distance of the plane to the origin.
Note that the coefficients can also be found using a matrix to solve a system of three equations with three unknowns. If you already have a matrix class available, it should be pretty straightforward to use it to find the coefficients.
Related
I'm quite new to coding in general. I have found some answers for this question but the answers seem advanced for me.
I'm trying to write my own Finite Element Project. For this I would like to write a method that checks if random 4 nodes given as input form a convex quadrilateral.
My method is supposed to look like this:
private bool IsConvex(Node[4] corners)
{
bool isConvex;
//CODE//
return isConvex;
}
the Node class is defined by three public properties referring to their coordinates (.coordX, .coordY, .coordZ)
In order to know if a quadrilateral is convex or not, you can make a triangle of three points and see if the fourth point is located inside that triangle or not. If you manage finding one triangle, which contains the fourth point, then you don't have a convex quadrilateral.
Ok, and how can you know if a point is located inside a triangle?
Well, you start by determining at which side a point is located compared to a vector.
Come again?
Well, for each vector, you can find out if a point is located at the left side or at the right side: you just rotate the vector back to the Y-axis, you do the same with the point and if the X coordinate of the point is negative your point is located at the left side, otherwise it's at the right side, like in these three cases (left, left and right):
Once you have figured that out, you define a point being inside a triangle if, after having described the triangle as a triangle of vectors, your point is at the same side of all vectors, like in this example (be aware that your triangle consists of the vectors AB, BC and CA: the points must follow up each other):
Good luck
First a little helper class to handle things related to triangles made up of three nodes.
using System.Numerics;
public readonly struct Triangle
{
public const float DistanceTolerance = 1e-6f;
public Triangle(Vector3 a, Vector3 b, Vector3 c)
{
A = a;
B = b;
C = c;
}
public Vector3 A { get; }
public Vector3 B { get; }
public Vector3 C { get; }
private Vector3 AreaVector { get => (Vector3.Cross(A, B) + Vector3.Cross(B, C) + Vector3.Cross(C, A)) / 2; }
public float Area { get => AreaVector.Length(); }
public Vector3 Normal { get => Vector3.Normalize(AreaVector); }
public float DistanceTo(Vector3 point) => Vector3.Dot(Normal, point - A);
public Vector3 Project(Vector3 point)
{
// A projected point lies on the plane defined by the three veertices A,B,C
Vector3 n = Normal;
float d = Vector3.Dot(n, point - A);
return point - n * d;
}
public void Barycentric(Vector3 P, out (float w_A, float w_B, float w_C) coordinates)
{
Vector3 n = Vector3.Cross(A, B) + Vector3.Cross(B, C) + Vector3.Cross(C, A);
float w_A = Vector3.Dot(n, Vector3.Cross(P, B) + Vector3.Cross(B, C) + Vector3.Cross(C, P));
float w_B = Vector3.Dot(n, Vector3.Cross(A, P) + Vector3.Cross(P, C) + Vector3.Cross(C, A));
float w_C = Vector3.Dot(n, Vector3.Cross(A, B) + Vector3.Cross(B, P) + Vector3.Cross(P, A));
float sum = w_A + w_B + w_C;
coordinates = (w_A / sum, w_B / sum, w_C / sum);
}
public bool Contains(Vector3 P)
{
if (Math.Abs(DistanceTo(P)) <= DistanceTolerance)
{
Barycentric(P, out var coordinates);
return coordinates.w_A >= 0 && coordinates.w_A <= 1
&& coordinates.w_B >= 0 && coordinates.w_B <= 1
&& coordinates.w_C >= 0 && coordinates.w_C <= 1;
}
return false;
}
}
If you are not familiar with barycentric coordinates, they are the linear combinations of the vertices that make up an interior (or exterior) point.
For example if a point is defined as P = 0.3*A + 0.5*B + 0.2*C then the barycentric coordinates of P are (0.3,0.5,0.2). The only restriction here is that the sum of the barycentric coordinates must equal to 1.
A point P is interior to the triangle ABC if all the barycentric coordinates of P are between 0 and 1.
This is the rule that I am using to write the Triangle.Contains(point) function. I also check to see if the point is on the same plane as the triangle.
Now to get to the algorithm to check if an n-gon is convex, all I have to do is take 3 vertices at a time, and check that all remaining other vertices are exterior to those three.
public static bool IsConvex(Vector3[] nodes)
{
for (int i = 0; i < nodes.Length; i++)
{
// pick three nodes at a time i,j,k
var j = (i + 1) % nodes.Length;
var k = (i + 2) % nodes.Length;
var A = nodes[i];
var B = nodes[j];
var C = nodes[k];
// deefine triangle ABC from three nodes
var trig = new Triangle(A, B, C);
// check nodes after the three and wrap around to grab first nodes also
for (int r = 3; r < nodes.Length; r++)
{
var P = nodes[(r + i) % nodes.Length];
// if _any_ node is interior to ABC then non-convex
if (trig.Contains(P))
{
return false;
}
}
}
return true;
}
and some test code to make sure it all works as intended.
static readonly Random rng = new Random();
static void Main(string[] args)
{
// Generate a random 3D triangle
var trig = new Triangle(
new Vector3(10 * (float)rng.NextDouble(), 0, 0),
new Vector3(0, 10 * (float)rng.NextDouble(), 0),
new Vector3(0, 0, 10 * (float)rng.NextDouble()));
// Generate an interior point (in the plane)
var point1 = 0.3f * trig.A + 0.5f * trig.B + 0.2f * trig.C;
// Check that it is contained inside the triangle
Debug.Assert(trig.Contains(point1));
// Generate an exterior point (on the plane)
var point2 = -0.3f * trig.A + 0.5f * trig.B + 0.8f * trig.C;
// Check that it is not contained inside the triangle
Debug.Assert(!trig.Contains(point2));
// Generate a point out of plane
var point3 = point1 + 2.5f * trig.Normal;
// Check that it is not contained inside the triangle
Debug.Assert(!trig.Contains(point3));
// Generate a convex quadrilateral
var poly1 = new Vector3[] {
new Vector3(0f,0f,0f),
new Vector3(5f,0f,0f),
new Vector3(5f,3f,0f),
new Vector3(1f,7f,0f),
};
// Generate a non-convex quadrilateral
var poly2 = new Vector3[] {
new Vector3(0f,0f,0f),
new Vector3(5f,0f,0f),
new Vector3(2f,2f,0f),
new Vector3(1f,7f,0f),
};
// Check that it is convex
Debug.Assert(IsConvex(poly1));
// Check that it is not convex
Debug.Assert(!IsConvex(poly2));
}
I'm attempting to convert from state vectors (position and speed) into Kepler elements, however I'm running into problems where a negative velocity or position will give me wrong results when trying to calculate true anomaly.
Here are the different ways I'm trying to calculate the True Anomaly:
/// <summary>
/// https://en.wikipedia.org/wiki/True_anomaly#From_state_vectors
/// </summary>
public static double TrueAnomaly(Vector4 eccentVector, Vector4 position, Vector4 velocity)
{
var dotEccPos = Vector4.Dot(eccentVector, position);
var talen = eccentVector.Length() * position.Length();
talen = dotEccPos / talen;
talen = GMath.Clamp(talen, -1, 1);
var trueAnomoly = Math.Acos(talen);
if (Vector4.Dot(position, velocity) < 0)
trueAnomoly = Math.PI * 2 - trueAnomoly;
return trueAnomoly;
}
//sgp = standard gravitational parameter
public static double TrueAnomaly(double sgp, Vector4 position, Vector4 velocity)
{
var H = Vector4.Cross(position, velocity).Length();
var R = position.Length();
var q = Vector4.Dot(position, velocity); // dot product of r*v
var TAx = H * H / (R * sgp) - 1;
var TAy = H * q / (R * sgp);
var TA = Math.Atan2(TAy, TAx);
return TA;
}
public static double TrueAnomalyFromEccentricAnomaly(double eccentricity, double eccentricAnomaly)
{
var x = Math.Sqrt(1 - Math.Pow(eccentricity, 2)) * Math.Sin(eccentricAnomaly);
var y = Math.Cos(eccentricAnomaly) - eccentricity;
return Math.Atan2(x, y);
}
public static double TrueAnomalyFromEccentricAnomaly2(double eccentricity, double eccentricAnomaly)
{
var x = Math.Cos(eccentricAnomaly) - eccentricity;
var y = 1 - eccentricity * Math.Cos(eccentricAnomaly);
return Math.Acos(x / y);
}
Edit: another way of doing it which Spectre pointed out:
public static double TrueAnomaly(Vector4 position, double loP)
{
return Math.Atan2(position.Y, position.X) - loP;
}
Positions are all relative to the parent body.
These functions all agree if position.x, position.y and velocity.y are all positive.
How do I fix these so that I get a consistent results when position and velocity are negitive?
Just to clarify: My angles appear to be sort of correct, just pointing in the wrong quadrant depending on the position and or velocity vectors.
Yeah so I was wrong, the above all do return the correct values after all.
So I found an edge case where most of the above calculations fail.
Given position and velocity:
pos = new Vector4() { X = -0.208994076275941, Y = 0.955838328099748 };
vel = new Vector4() { X = -2.1678187689294E-07, Y = -7.93096769486992E-08 };
I get some odd results, ie ~ -31.1 degrees, when I think it should return ` 31.1 (non negative). one of them returns ~ 328.8.
However testing with this position and velocity the results apear to be ok:
pos = new Vector4() { X = -0.25, Y = 0.25 };
vel = new Vector4() { X = Distance.KmToAU(-25), Y = Distance.KmToAU(-25) };
See my answer for extra code on how I'm testing and the math I'm using for some of the other variables.
I'm going around in circles on this one. this is a result of a bug in my existing code that shows up under some conditions but not others.
I guess the real question now is WHY am I getting different results with position/velocity above that don't match to my expectations or each other?
Assuming 2D case... I am doing this differently:
compute radius of semi axises and rotation
so you need to remember whole orbit and find 2 most distant points on it that is major axis a. The minor axis b usually is 90 deg from major axis but to be sure just fins 2 perpendicularly most distant points on your orbit to major axis. So now you got both semi axises. The initial rotation is computed from the major axis by atan2.
compute true anomaly E
so if center is x0,y0 (intersection of a,b or center point of both) initial rotation is ang0 (angle of a) and your point on orbit is x,y then:
E = atan2(y-y0,x-x0) - ang0
However in order to match Newton/D'Alembert physics to Kepler orbital parameters you need to boost the integration precision like I did here:
Is it possible to make realistic n-body solar system simulation in matter of size and mass?
see the [Edit3] Improving Newton D'ALembert integration precision even more in there.
For more info and equations see:
Solving Kepler's equation
[Edit1] so you want to compute V I see it like this:
As you got your coordinates relative to parent you can assume they are already in focal point centered so no need for x0,y0 anymore. Of coarse if you want high precision and have more than 2 bodies (focal mass + object + proximity object(s) like moons) then the parent mass will no longer be in focal point of orbit but close to it ... and to remedy you need to use real focal point position so x0,y0 again... So how to do it:
compute center point (cx,cy) and a,b semi axises
so its the same as in previous text.
compute focal point (x0,y0) in orbit axis aligned coordinates
simple:
x0 = cx + sqrt( a^2 + b^2 );
y0 = cy;
initial angle ang0 of a
let xa,ya be the intersection of orbit and major axis a on the side with bigger speeds (near parent object focus). Then:
ang0 = atan2( ya-cy , xa-cx );
and finally the V fore any of yours x,y
V = atan2( y-y0 , x-x0 ) - ang0;
Ok so on further testing it appears my original calcs do all return the correct values, however when I was looking at the outputs I was not taking the LoP into account and basically not recognizing that 180 is essentially the same angle as -180.
(I was also looking at the output in radians and just didn't see what should have been obvious)
Long story short, I have a bug I thought was in this area of the code and got lost in the weeds.
Seems I was wrong above. see OP for edge case.
Here's some code I used to test these,
I used variations of the following inputs:
pos = new Vector4() { X = 0.25, Y = 0.25 };
vel = new Vector4() { X = Distance.KmToAU(-25), Y = Distance.KmToAU(25) };
And tested them with the following
double parentMass = 1.989e30;
double objMass = 2.2e+15;
double sgp = GameConstants.Science.GravitationalConstant * (parentMass + objMass) / 3.347928976e33;
Vector4 ev = OrbitMath.EccentricityVector(sgp, pos, vel);
double e = ev.Length();
double specificOrbitalEnergy = Math.Pow(vel.Length(), 2) * 0.5 - sgp / pos.Length();
double a = -sgp / (2 * specificOrbitalEnergy);
double ae = e * a;
double aop = Math.Atan2(ev.Y, ev.X);
double eccentricAnomaly = OrbitMath.GetEccentricAnomalyFromStateVectors(pos, a, ae, aop);
double aopD = Angle.ToDegrees(aop);
double directAngle = Math.Atan2(pos.Y, pos.X);
var θ1 = OrbitMath.TrueAnomaly(sgp, pos, vel);
var θ2 = OrbitMath.TrueAnomaly(ev, pos, vel);
var θ3 = OrbitMath.TrueAnomalyFromEccentricAnomaly(e, eccentricAnomaly);
var θ4 = OrbitMath.TrueAnomalyFromEccentricAnomaly2(e, eccentricAnomaly);
var θ5 = OrbitMath.TrueAnomaly(pos, aop);
double angleΔ = 0.0000001; //this is the "acceptable" amount of error, really only the TrueAnomalyFromEccentricAnomaly() calcs needed this.
Assert.AreEqual(0, Angle.DifferenceBetweenRadians(directAngle, aop - θ1), angleΔ);
Assert.AreEqual(0, Angle.DifferenceBetweenRadians(directAngle, aop - θ2), angleΔ);
Assert.AreEqual(0, Angle.DifferenceBetweenRadians(directAngle, aop - θ3), angleΔ);
Assert.AreEqual(0, Angle.DifferenceBetweenRadians(directAngle, aop - θ4), angleΔ);
Assert.AreEqual(0, Angle.DifferenceBetweenRadians(directAngle, aop - θ5), angleΔ);
and the following to compare the angles:
public static double DifferenceBetweenRadians(double a1, double a2)
{
return Math.PI - Math.Abs(Math.Abs(a1 - a2) - Math.PI);
}
And eccentricity Vector found thus:
public static Vector4 EccentricityVector(double sgp, Vector4 position, Vector4 velocity)
{
Vector4 angularMomentum = Vector4.Cross(position, velocity);
Vector4 foo1 = Vector4.Cross(velocity, angularMomentum) / sgp;
var foo2 = position / position.Length();
return foo1 - foo2;
}
And EccentricAnomaly:
public static double GetEccentricAnomalyFromStateVectors(Vector4 position, double a, double linierEccentricity, double aop)
{
var x = (position.X * Math.Cos(-aop)) - (position.Y * Math.Sin(-aop));
x = linierEccentricity + x;
double foo = GMath.Clamp(x / a, -1, 1); //because sometimes we were getting a floating point error that resulted in numbers infinatly smaller than -1
return Math.Acos(foo);
}
Thanks to Futurogogist and Spektre for their help.
I am assuming you are working in two dimensions?
Two dimensional vectors of position p and velocity v. The constant K is the the product of the gravitational constant and the mass of the gravity generating body. Calculate the eccentricity vector
eccVector = (dot(v, v)*p - dot(v, p)*v) / K - p / sqrt(dot(p, p));
eccentricity = sqrt(dot(eccVector, eccVector));
eccVector = eccVector / eccentricity;
b = { - eccVector.y, eccVector.x}; //unit vector perpendicular to eccVector
r = sqrt(dot(p, p));
cos_TA = dot(p, eccVector) / r; \\ cosine of true anomaly
sin_TA = dot(p, b) / r; \\ sine of true anomaly
if (sin_TA >= 0) {
trueAnomaly = arccos(cos_TA);
}
else if (sin_TA < 0){
trueAnomaly = 2*pi - arccos(cos_TA);
}
Let's say we define two vectors by subtracting the end points of two lines.
V1 = Pa - Pb;
V2 = Pc - Pd;
and we define the X axis as the following vector.
var V = new System.Windows.Vector(1, 0);
How can we know which one of the two two vectors V1 and V2 has the least angle to X axis.
You have two options: You can calculate the angles between V1 and V and V2 and V with the AngleBetween Function:
var angle1 = Vector.AngleBetween(V1,V);
var angle2 = Vector.AngleBetween(V2,V);
if (angle1 < angle2) {
//V1 is closer to V
}else{
//V2 is closer to V
}
or you could also normalize your vectors and compare their y values afterwards:
V1.Normalize();
V2.Normalize();
if(Math.Abs(V1.Y) < Math.Abs(V2.Y)){
//V1 is closer
}else{
//V2 is closer
}
I would prefer the first method, since the Normalize() function actually changes the original Vector so you would need to make a copy if you wanted to use them afterwards. Also you can use the first version in order to compare them to any other vector V without needing to adjust the code.
Edit: Actually the first version only chooses the vector with the smallest angle to the V vector and not to the x-axis. So if you want to compare to the axis instead of just the vector you should do the angle calculation with copies of V1 and V2 with absolute x and y values:
var ref1 = new System.Windows.Vector(Math.Abs(V1.X),Math.Abs(V1.Y));
var ref2 = new System.Windows.Vector(Math.Abs(V2.X),Math.Abs(V2.Y));
var angle1 = Vector.AngleBetween(ref1,V);
var angle2 = Vector.AngleBetween(ref2,V);
if (angle1 < angle2) {
//V1 is closer to V
}else{
//V2 is closer to V
}
Looking at it like this, the normalization method might be the better choice after all. (But be sure to call Normalize() on copies)
Normalize both and find one with higher absolute value of x.
Alternatively, angle between two vectors a and b is arccos((a.x*b.x+a.y*b.y)/(a.Length*b.Length)) (undefined if one of vectors is zero)
Check the following solution. May fulfill your query.Add reference Windowsbase to project. Very Important.
using System;
using System.Windows;
namespace Vectors
{
class Program
{
static void Main(string[] args)
{
// Define Points
Point Pa = new Point(5.0,1.0);
Point Pb = new Point(10.0,3.0);
Point Pc = new Point(7.0,10.0);
Point Pd = new Point(1.0,3.0);
Vector V1 = Pa - Pb;
Vector V2 = Pc - Pd;
Vector V = new Vector(1, 0);
double Phi1 = Math.Atan2(V1.Y, V1.X)*180/Math.PI;
double Phi2 = Math.Atan2(V2.Y, V2.X)*180/Math.PI;
// Check for -ve angle and take 180 degree complement.
Phi1 = (Phi1 >= 0) ? Phi1 : 180 + Phi1;
Phi2 = (Phi2 >= 0) ? Phi2 : 180 + Phi2;
if(Phi1<=Phi2)
{
Console.WriteLine("Vector V1 has a least angle");
}
else
{
Console.WriteLine("Vector V2 has a least angle");
}
Console.ReadLine();
}
}
}
double compare() {
Point p0 = new Point(0, 0);
Point p1 = new Point(100, 100);
Point p2 = new Point(100, -100);
Vector v0 = new System.Windows.Vector(1, 0);
var v1 = p1 - p0;
var v2 = p2 - p0;
var value1 = Vector.AngleBetween(v0, v1); //value = 45
var value2 = Vector.AngleBetween(v0, v2); //value = -45
value1 = Math.Abs(value1);
value2 = Math.Abs(value2);
/* Or if you want to compare the value of the angle
if (value1 < 0)
value1 += 360;
if (value2 < 0)
value2 += 360;
*/
return Math.Min(value1,value2);
}
I have two rays. Each ray has a start location vector (Vector3D) and a direction vector (Vector3D), but continue on to infinity. They are both on the same plane, but in a 3D environment. The Rays are interdependent, which means that they might not mirror each other perfectly. From this i need to calculate the location at which these rays intersect in the 3D environment and output it as a vector. In essence: a rangefinder.
How should i go about doing this? Is there a better way of doing it than using the C# Ray structure, is it even possible?
I am a pretty new coder (read: bad) but any answer is appreciated, I would enjoy it if an explanation was included.
Crude image of the rays
Two lines in 3D space only intersect if they are on the same plane. The probability that two random lines in space intersect is really small.
When you want to find out if two rays intersect, if you are looking for an exact intersection point, the chance is that you are not going to be able to calculate it due to floating point errors.
The next best thing is to find the shortest distance between two rays. Then if that distance is smaller than a certain threshold (defined by you) we could say the rays are intersecting.
Finding shortest distance
Here are two rays in 3D space, with the blue vector representing the shortest distance.
Let's take a frame from that gif:
Legend:
p1 is ray1.Position
p2 is ray2.Position
d1 is ray1.Direction
d2 is ray2.Direction
d3 is the cross product d1 x d2
The cross product of the rays directions will be perpendicular to both rays, so it's the shortest direction from ray to ray. If lines are parallel, the cross product will be zero, but for now lets only deal with non-parallel lines.
From the photo, we get the equation:
p1 + a*d1 + c*d3 = p2 + b*d2
Rearranged so the variables are on the left:
a*d1 - b*d2 + c*d3 = p2 - p1
Since each of the know values (d1, d2, d3, p1 and p2) has three components (x,y,z), this is a system of three linear equations with 3 variables.
a*d1.X - b*d2.X + c*d3.X = p2.X - p1.X
a*d1.Y - b*d2.Y + c*d3.Y = p2.Y - p1.Y
a*d1.Z - b*d2.Z + c*d3.Z = p2.Z - p1.Z
Using Gaussian elimination, we get the values of a, b and c.
If both a and b are positive, the position of the intersection will be
Vector3 position = ray1.Position + a*ray1.Direction;
Vector3 direction = c * d3; //direction.Length() is the distance
You could return these values as a Ray for convenience.
If either a or b are negative, this means that the calculated shortest distance would be behind one (or both) of the rays, so a different method should be used to find the shortest distance. This method is the same for lines that are parallel (cross product d1 x d2 is zero).
Now the calculation becomes finding which ray (positive direction) is closest to the position (p1 or p2) of the other ray. For this we use dot product (projection of a vector onto another vector)
Legend:
dP = p2 - p1
Before calculating dot product of d1 dot dP, make sure that d1 (or d2) are normalized (Vector3.Normalize()) - dot product is meant to work on unit vectors.
Now it's a matter of finding which is the shortest distance, based on the projection factor (result of dot) on ray1 (lets call it a2) and projection factor on ray2 (lets call it b2).
If both a2 and b2 are negative (negative side of the ray), then the shortest distance is from position to position. If one is in the negative direction then the other one is the shortest. Else it's the shorter of the two.
Working code:
public Ray FindShortestDistance(Ray ray1, Ray ray2)
{
if (ray1.Position == ray2.Position) // same position - that is the point of intersection
return new Ray(ray1.Position, Vector3.Zero);
var d3 = Vector3.Cross(ray1.Direction, ray2.Direction);
if (d3 != Vector3.Zero) // lines askew (non - parallel)
{
//d3 is a cross product of ray1.Direction (d1) and ray2.Direction(d2)
// that means d3 is perpendicular to both d1 and d2 (since it's not zero - we checked that)
//
//If we would look at our lines from the direction where they seem parallel
// (such projection must always exist for lines that are askew)
// we would see something like this
//
// p1 a*d1
// +----------->x------
// |
// | c*d3
// p2 b*d2 v
// +------->x----
//
//p1 and p2 are positions ray1.Position and ray2.Position - x marks the points of intersection.
// a, b and c are factors we multiply the direction vectors with (d1, d2, d3)
//
//From the illustration we can the shortest distance equation
// p1 + a*d1 + c*d3 = p2 + b*d2
//
//If we rearrange it so we have a b and c on the left:
// a*d1 - b*d2 + c*d3 = p2 - p1
//
//And since all of the know variables (d1, d2, d3, p2 and p1) have 3 coordinates (x,y,z)
// now we have a set of 3 linear equations with 3 variables.
//
// a * d1.X - b * d2.X + c * d3.X = p2.X - p1.X
// a * d1.Y - b * d2.Y + c * d3.Y = p2.Y - p1.Y
// a * d1.Z - b * d2.Z + c * d3.Z = p2.Z - p1.Z
//
//If we use matrices, it would be
// [d1.X -d2.X d3.X ] [ a ] [p2.X - p1.X]
// [d1.Y -d2.Y d3.Y ] * [ a ] = [p2.Y - p1.Y]
// [d1.Z -d2.Z d3.Z ] [ a ] [p2.Z - p1.Z]
//
//Or in short notation
//
// [d1.X -d2.X d3.X | p2.X - p1.X]
// [d1.Y -d2.Y d3.Y | p2.Y - p1.Y]
// [d1.Z -d2.Z d3.Z | p2.Z - p1.Z]
//
//After Gaussian elimination, the last column will contain values a b and c
float[] matrix = new float[12];
matrix[0] = ray1.Direction.X;
matrix[1] = -ray2.Direction.X;
matrix[2] = d3.X;
matrix[3] = ray2.Position.X - ray1.Position.X;
matrix[4] = ray1.Direction.Y;
matrix[5] = -ray2.Direction.Y;
matrix[6] = d3.Y;
matrix[7] = ray2.Position.Y - ray1.Position.Y;
matrix[8] = ray1.Direction.Z;
matrix[9] = -ray2.Direction.Z;
matrix[10] = d3.Z;
matrix[11] = ray2.Position.Z - ray1.Position.Z;
var result = Solve(matrix, 3, 4);
float a = result[3];
float b = result[7];
float c = result[11];
if (a >= 0 && b >= 0) // normal shortest distance (between positive parts of the ray)
{
Vector3 position = ray1.Position + a * ray1.Direction;
Vector3 direction = d3 * c;
return new Ray(position, direction);
}
//else will fall through below:
// the shortest distance was between a negative part of a ray (or both rays)
// this means the shortest distance is between one of the ray positions and another ray
// (or between the two positions)
}
//We're looking for the distance between a point and a ray, so we use dot products now
//Projecting the difference between positions (dP) onto the direction vectors will
// give us the position of the shortest distance ray.
//The magnitude of the shortest distance ray is the the difference between its
// position and the other rays position
ray1.Direction.Normalize(); //needed for dot product - it works with unit vectors
ray2.Direction.Normalize();
Vector3 dP = ray2.Position - ray1.Position;
//shortest distance ray position would be ray1.Position + a2 * ray1.Direction
// or ray2.Position + b2 * ray2.Direction (if b2 < a2)
// or just distance between points if both (a and b) < 0
//if either a or b (but not both) are negative, then the shortest is with the other one
float a2 = Vector3.Dot(ray1.Direction, dP);
float b2 = Vector3.Dot(ray2.Direction, -dP);
if (a2 < 0 && b2 < 0)
return new Ray(ray1.Position, dP);
Vector3 p3a = ray1.Position + a2 * ray1.Direction;
Vector3 d3a = ray2.Position - p3a;
Vector3 p3b = ray1.Position;
Vector3 d3b = ray2.Position + b2 * ray2.Direction - p3b;
if (b2 < 0)
return new Ray(p3a, d3a);
if (a2 < 0)
return new Ray(p3b, d3b);
if (d3a.Length() <= d3b.Length())
return new Ray(p3a, d3a);
return new Ray(p3b, d3b);
}
//Solves a set of linear equations using Gaussian elimination
float[] Solve(float[] matrix, int rows, int cols)
{
for (int i = 0; i < cols - 1; i++)
for (int j = i; j < rows; j++)
if (matrix[i + j * cols] != 0)
{
if (i != j)
for (int k = i; k < cols; k++)
{
float temp = matrix[k + j * cols];
matrix[k + j * cols] = matrix[k + i * cols];
matrix[k + i * cols] = temp;
}
j = i;
for (int v = 0; v < rows; v++)
if (v == j)
continue;
else
{
float factor = matrix[i + v * cols] / matrix[i + j * cols];
matrix[i + v * cols] = 0;
for (int u = i + 1; u < cols; u++)
{
matrix[u + v * cols] -= factor * matrix[u + j * cols];
matrix[u + j * cols] /= matrix[i + j * cols];
}
matrix[i + j * cols] = 1;
}
break;
}
return matrix;
}
It is so simple. you need to apply AAS Triangle formula.
Consider the above figure as triangle,
Ray1 location as A and Ray2 Location as B and you need to find point c. Using dot product find angle between Ray AB and AC (alpha), BA and BC (theta).
Find the distance between location A and B (D). So finally you have alpha, beta and D, i.e (2 angles and 1 side of the triangle.) Apply AAS method and find C location.
https://www.mathsisfun.com/algebra/trig-solving-aas-triangles.html.
I have a list of Point types in C#. I want to run Dijkstra's algorithm on this list of points where the first entry in the list is the starting location.
Is there a way of doing this using an existing library?
If such library doesn't exist, is there a way of calculating the distance between two points with x and y coordinates. For example, calculate the distance between point A (x coordinate =2, y coordinate = 4) and point B ((x coordinate =9, y coordinate = 7).
I have used the ZedGraph library to build the graph.
I think you misunderstood, what the Dijkstra algorithm stands for.
For a given source vertex (node) in the graph, the algorithm finds the path with lowest cost (i.e. the shortest path) between that vertex and every other vertex.
What you need (i think) the lowest distance between two points based on their coordinates.
And the distance between two points can be counted using basic math:
Point p = new Point(4, 5);
Point r = new Point(10, 2);
double distance = Math.Sqrt(Math.Pow(p.X - r.X, 2) + Math.Pow(p.Y - r.Y, 2));
using this knowledge the problem could be solved with two functions like this:
Returns the distance between p and r:
static double distance(Point p, Point r)
{
return Math.Sqrt(Math.Pow(p.X - r.X, 2) + Math.Pow(p.Y - r.Y, 2));
}
Returns the index of the closest point to the fromIndex th element of the points list:
static int closestPoint(List<Point> points, int fromIndex)
{
Point start = points[fromIndex];
int resultIndex = 0;
for (int i = 1; i < points.Count; i++)
{
if (fromIndex == i)
continue;
Point current = points[i];
if (distance(start, current) < distance(start, points[resultIndex]))
resultIndex = i;
}
return resultIndex;
}
I'm really sorry, if i misunderstood you!