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.
Related
I wrote an application in C# which controls a camera through a 3D environment.
At the moment the camera follows a path which is defined as an array of points, where each point is defined as: x, y, z & rotation for each axis (everything is a float)
The camera position between those points are computed by linear interpolation every frame.
Because this leads to very clunky movement, I want to implement cubic spline interpolation instead.
There are many different algorithms, but so far I set my eyes on the following, since those feel "more correct", which means they don't overshoot:
Piecewise Cubic Hermite Interpolating Polynomial (PCHIP)
Modified Akima piecewise cubic Hermite interpolation (MAKIMA)
My biggest problem is, that I am absolutely not a mathematician and I can't read and understand the gibberish found online for their definitions, so I need help translating it into code.
Constraints for the implementation
Algorithm should use either PCHIP or MAKIMA (I am open for other "correct" approaches though)
Interpolated values must be a function of time (t from 0 to {points.Length})
Must run for 3D points, not only 2D
Should not contain any external libraries if possible
Libraries
I have checked the following, but they either don't support 3D points, or interpolation by time (and aren't intuitive to use either... not even talking about the license here):
Math.NET Numerics
ALGLIB
Here is a small code stump which I came up with to demonstrate what I am looking for:
static float[,] points = new float[,] {
{10, 10, 10},
{35, 5, 25},
{15, 30, 15}
};
static int numberOfPoints = points.Length / 3; // dividing for each stored float value per point: only x, y, z for simplification
static void Main()
{
List<float[]> pointsWithInterpolation = new(); // holding both known and interpolated points
float stepSize = 0.1f; // step size for t
for (int i = 0; i < numberOfPoints - 1; i++)
{ // loop for each spline segment
pointsWithInterpolation.Add(new float[] { points[i, 0], points[i, 1], points[i, 2] }); // add an allready known point to the list
for (float t = stepSize; t < 1f; t += stepSize)
{ // should run (1 / {stepSize} - 2) times
float[] interpolatedPoint = Interpolate(i + t);
pointsWithInterpolation.Add(interpolatedPoint); // add an interpolated point to the list
}
if (i == numberOfPoints - 1)
{ // add the very last point if we have finished the last segment
pointsWithInterpolation.Add(new float[] { points[i + 1, 0], points[i + 1, 1], points[i + 1, 2] }); // add an allready known point to the list
}
}
}
private static float[] Interpolate(float t)
{
// point indexes
int p1 = (int)t;
int p2 = p1 + 1;
int p0 = p1 == 0 ? 0 : p1 - 1; // if there are no previous point, set it to the first one
int p3 = p2 == numberOfPoints - 1 ? p2 : p2 + 1; // if there are no following points, set it to the last one
float x0 = points[p0, 0];
float x1 = points[p1, 0];
float x2 = points[p2, 0];
float x3 = points[p3, 0];
float y0 = points[p0, 1];
float y1 = points[p1, 1];
float y2 = points[p2, 1];
float y3 = points[p3, 1];
float z0 = points[p0, 2];
float z1 = points[p1, 2];
float z2 = points[p2, 2];
float z3 = points[p3, 2];
float x, y, z;
/* black magic hocus pocus happening here
x = ???;
y = ???;
z = ???;
*/
float[] point = new float[] { x, y, z };
return point;
}
So the question is: what is the formula for solving for x/y/z (the formula should be the same for each variable, but they use different variables).
Yes I know, this results in a constant number of interpolated points between the "real" ones, even if the distance of segments widely varies - I will tackle the issue of constant speed separately.
If I am not mistaken, every interpolation between p1 and p2 needs 4 values, so I set p0 equal to p1 for the very first segment and p3 equal to p2 for the last one to make up for non-existant points.
I haven't performance optimized the code, so I (hopefully) make it more clear/easy to understand what I try to do.
Of course I would also be thankful for code in other languages, as long as it fulfils the implementation constraints and doesn't use functions which aren't available in C# (and also no assembler please lmao)
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);
}
I have a line going bettween two Vector3 points and I want to find when the line is intersected at a height along the Z axis.
I am trying to write a function to calculate the intersection point.
void Main()
{
Vector3 A = new Vector3(2.0f, 2.0f, 2.0f);
Vector3 B = new Vector3(7.0f, 10.0f, 6.0f);
float Z = 3.0f;
Vector3 C = getIntersectingPoint(A, B, Z);
//C should be X=3.25, Y=4.0, Z=3.0
}
But trying to figure out how to do the math to handle possible negative numbers correctly is really starting to confuse me.
This is what I have and the moment, but this isn't correct.
public static Vector3 getIntersectingPoint(Vector3 A, Vector3 B, float Z)
{
// Assume z is bettween A and B and we don't need to validate
// Get ratio of triangle hight, height Z divided by (Za to Zb)
("absolute value: " + Math.Abs(A.Z-B.Z)).Dump();
("z offset: " + (Math.Abs(Z-B.Z)<Math.Abs(A.Z-Z)?Math.Abs(Z-B.Z):Math.Abs(A.Z-Z))).Dump();
float ratio = (Math.Abs(Z-B.Z)<Math.Abs(A.Z-Z)?Math.Abs(Z-B.Z):Math.Abs(A.Z-Z))/Math.Abs(A.Z-B.Z);
("ratio: " + ratio.ToString()).Dump();
float difX = ratio*Math.Abs(A.X-B.X);//this still needs to be added to or taken from the zero point offset
("difX: " + difX.ToString()).Dump();
float difY = ratio*Math.Abs(A.Y-B.Y);//this still needs to be added to or taken from the zero point offset
("difY: " + difY.ToString()).Dump();
float X = difX + (A.X<B.X?A.X:B.X);
("X: " + X).Dump();
float Y = difY + (A.Y<B.Y?A.Y:B.Y);
("Y: " + Y).Dump();
return new Vector3(X,Y,Z);
}
Does anyone know if there are any Math libraries that will already do this or examples that show how to do this that I can follow?
You have the starting (2.0f) and ending (6.0f) Z coordinates. The Z distance between the two points is 4.0f. You want to know the X and Y coordinates at the point where Z is 3.0f.
Remember that Z changes linearly along the segment. The segment is 4 units long, The point you're interested in is 1 unit from the start, or 1/4 of the length of the segment.
The X distance of the entire segment is 7.0 - 2.0, or 5 units. 1/4 of 5 is 1.25, so the X coordinate at the intersection is 3.25.
The Y distance of the entire segment is 8. 1/4 of 8 is 2. So the Y coordinate of the intersection point is 6.0.
The intersection point is (3.25f, 6.0f, 3.0f).
How to compute:
// start is the starting point
// end is the ending point
// target is the point you're looking for.
// It's pre-filled with the Z coordinate.
zDist = Math.Abs(end.z - start.z);
zDiff = Math.Abs(target.z - start.z);
ratio = zDiff / zDist;
xDist = Math.Abs(end.x - start.x);
xDiff = xDist * ratio;
xSign = (start.x < end.x) ? 1 : -1;
target.x = start.x + (xDiff * xSign);
yDist = Math.Abs(end.y - start.y);
yDiff = yDist * ratio;
ySign = (start.y < end.y) ? 1 : -1;
target.y = start.y + (yDiff * ySign);
Come to think of it, the whole sign thing shouldn't be necessary. Consider this, when end.x = 10 and start.x = 18:
xDist = end.x - start.x; // xDist = -8
xDiff = xDist * ratio; // xDiff = -2
target.x = start.x + xDiff; // target.x = 18 + (-2) = 16
Yeah, no need for sign silliness.
Also no need for the calls to Math.Abs when computing the ratio. We know that zDist and zDiff will both have the same sign, so ratio will always be positive.
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.
Using a bit of code like this:
static bool Intersects(Vector2 a1, Vector2 a2, Vector2 b1, Vector2 b2, out Vector2 intersection)
{
intersection = Vector2.Zero;
Vector2 b = a2 - a1;
Vector2 d = b2 - b1;
float bDotDPerp = b.X * d.Y - b.Y * d.X;
if (bDotDPerp == 0)
return false;
Vector2 c = b1 - a1;
float t = (c.X * d.Y - c.Y * d.X) / bDotDPerp;
if (t < 0 || t > 1)
return false;
float u = (c.X * b.Y - c.Y * b.X) / bDotDPerp;
if (u < 0 || u > 1)
return false;
intersection = a1 + t * b;
return true;
}
I can determine where two lines collide. However, the coordinates are given relative to the window, not each other. For example, say the two lines are perpendicular to one another. One of them overlaps the second one by one pixel at 0,0. This would mean that to resolve this collision, I would need to move back 1 pixel. However, if I performed this same test at say, 400,400, it would tell me that it intersects at 399, 399. I obviously do not have to resolve the collision by 399, 399.
I'm looking to use this information to find out how much the lines are overlapping so I can move them back appropriately. What must I do to do this?
This answer was pretty obvious, and I figured it out.
Just subtract a1 and a2 from X and Y of outVect:
(outVect.X-a1.X), (outVect.Y-a1.Y)
This negates its location.