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.
Related
I have three vertices of a triangle i.e. (x1,y1,z1); (x2,y2,z2) and (x3,y3,z3).
The bounding box of my triangle is a cube of length = Maximum(xmax-xmin, ymax-ymin,zmax-zmin). xmax,xmin,....,zmin can be calculated by looping through all the vertices of the triangle.
Now,considering a particular resolution size(h), I divide my bounding box into grids.For example,if my bounding box is of length 10 and resolution is 1,No. of grids will be 1000(10*10*10).
Now,I want to find out all those grids(cubes) which are intersected by my Triangle as well as those cubes,which lie inside my triangle.
I am following the below approach:
-> Storing all the grids of my bounding box in a list (using 3 "for" loops for x,y,z.This involves huge memory wastage)
-> I am then looping through each grid coordinate in my list,checking out if the cube lies on my plane,if the cube lines on my plane,then I am checking if my grid is intersected by three edges of my triangle (or) if my grid is located inside my triangle.
I am using the following algorithms for checking the intersection and inside criteria:
bool IfLineIntersectsPoint(float x, float y, float z)
{
bool checkifIntersects = false;
FindProjectedPointonPlane(x, y, z);
//Px,Py and Pz are projected points on the plane
if ((((Px - vertex1x) / a) == ((Py - vertex1y) / b)) && (((Px - vertex1x) / a) == ((Pz - vertex1z) / c)))
{
checkifIntersects = true;
}
return checkifIntersects;
}
bool PointInTriangle(Vector3[] TriangleVectors, Vector3 P)
{
Vector3 A = TriangleVectors[0], B = TriangleVectors[1], C = TriangleVectors[2];
if (SameSide(P, A, B, C) && SameSide(P, B, A, C) && SameSide(P, C, A, B))
{
Vector3 AB = Vector3.Subtract(A, B);
Vector3 AC = Vector3.Subtract(A, C);
Vector3 AP = Vector3.Subtract(A, P);
Vector3 vc1 = Vector3.Cross(AB, AC);
float magnitude1 = AB.Length();
float magnitude2 = vc1.Length();
Vector3 NormAP = new Vector3(AP.X / magnitude1, AP.Y / magnitude1, AP.Z / magnitude1);
Vector3 NormVC1 = new Vector3(vc1.X / magnitude2, vc1.Y / magnitude2, vc1.Z / magnitude2);
float ftw = Math.Abs(Vector3.Dot(NormAP, NormVC1));
if (ftw <= 0.1f)
return true;
}
return false;
}
I would be really glad,if someone can suggest any changes in my algorithm and the above two functions,so as to minimize my computation time. Also,if I have some 8-9 triangles,Everytime,I am checking all these criteria for all 1000 grids.Can I get some subset(some 100-150 grids) of these 1000 grids and proceed?
Thanks in Advance.
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'm trying to implement a method to check if a circle and a line intersect. I took most of this code (fixed based on the answer), and also modified the code a bit to use Point's instead of Vector2f's`.
This is currently what I have:
private bool CircleLineIntersect(int x, int y, int radius, Point linePoint1, Point linePoint2) {
Point p1 = new Point(linePoint1.X,linePoint1.Y);
Point p2 = new Point(linePoint2.X,linePoint2.Y);
p1.X -= x;
p1.Y -= y;
p2.X -= x;
p2.Y -= y;
float dx = p2.X - p1.X;
float dy = p2.Y - p1.Y;
float dr = (float)Math.Sqrt((double)(dx * dx) + (double)(dy * dy));
float D = (p1.X * p2.Y) - (p2.X * p1.Y);
float di = (radius * radius) * (dr * dr) - (D * D);
if (di < 0) return false;
else return true;
}
It looks consistent with this algorithm, so I'm not sure what the problem is.
If anyone could provide guidance it would be much appreciated.
EDIT:
It doesn't seem to be calculating correctly. For example with input x=1272, y=1809, radius=80, linePoint1={X=1272,Y=2332}, linePoint2={X=1272,Y=2544} there shouldn't be an intersection (y+radius is less than both y values of the line segment), but the function is returning true.
Error exists in your test case. Not only does the it intersect, but your line goes through the center of the circle. The line is a vertical line (X =1272). Your circle is centred about (1272, 1809). ERGO it goes through the centre.
Perhaps you have a misunderstanding between the terms line and line-segment, within mathematics.
I'm trying to calculate the smallest difference between two angles.
This is my current code (a slight variation of something I found online):
float a1 = MathHelper.ToDegrees(Rot);
float a2 = MathHelper.ToDegrees(m_fTargetRot);
float dif = (float)(Math.Abs(a1 - a2);
if (dif > 180)
dif = 360 - dif;
dif = MathHelper.ToRadians(dif);
It works fine except for in cases at the edge of a circle. For example if the current angle is 355 and the target angle is 5 it calculates the difference is -350 rather than 10 since 365 degrees is equal to 5 degrees.
Any ideas on what I can do to make this work?
You basically had it. Just take the dif modulus 360 before checking to see if greater than 180:
float a1 = MathHelper.ToDegrees(Rot);
float a2 = MathHelper.ToDegrees(m_fTargetRot);
float dif = (float)Math.Abs(a1 - a2) % 360;
if (dif > 180)
dif = 360 - dif;
dif = MathHelper.ToRadians(dif);
Edit: #Andrew Russell made a great point in comments to your question and the solution below takes advantage of the MathHelper.WrapAngle method as he suggested:
diff = Math.Abs(MathHelper.WrapAngle(a2 - a1));
You would expand the check for out of bound angles:
if (dif < 0) dif = dif + 360;
if (dif > 180) dif = 360 - dif;
I never like handling the zero-wrapping with case statements. Instead, I use the definition of the dot product to compute the (unsigned) angle between two angles:
vec(a) . vec(b) = ||a|| ||b|| cos(theta)
We're just going to make a and b unit vectors, so ||a|| == ||b|| == 1.
Since vec(x) = [cos(x),sin(x)], we get:
unsigned_angle_theta(a,b) = acos(cos(a)cos(b) + sin(a)sin(b))
(n.b. all angles in radians)
You can normalize the result to be 0 <= theta < 360:
while(theta < 0) { theta += 360; }
If you want to keep the answer in radians (recommended):
const Double TwoPi = 2 * Math.Pi;
while(theta < 0) { theta += TwoPi; }
We can use Euler's formula: exp(iA) = cos A + i sin A.
In the case of the difference between two angles this becomes:
exp(i(A-B))
Using the laws of exponents:
= exp(iA).exp(-iB).
-iB is the conjugate of iB thus:
= exp(iA).exp(conjugate(iB)).
The complex exponent can be calcuated by Taylors series:
taylor_e(P={cplx,_,_}) ->
taylor_e(
#{sum => to_complex(1),
term => 0,
term_value => to_complex(1),
min_terms => 3,
quadrature => 1,
error_term => 1.0e-4,
domain => P}
);
taylor_e(P=#{sum := Sum,
term := Term,
term_value := TermValue0,
min_terms := MinTerms,
domain := Dom,
quadrature := Q,
error_term := ErrorTerm
})
when ((Term =< MinTerms) or (abs(1-Q) > ErrorTerm)) and
(Term < 20) ->
NewTerm = Term+1,
TermValue1 = scalar_divide(multiply(TermValue0,Dom),NewTerm),
PartialSum = add(Sum,TermValue1),
taylor_e(P#{sum := PartialSum,
term := Term+1,
term_value := TermValue1,
quadrature := quadrance(PartialSum)
});
taylor_e(#{sum := Result}) ->
Result.
The angle difference is the the argument (direction) of the resulting complex number and is retrieved by atan2.
Of course you will need some basic complex number routines. This method does not have a discontinuity around 0/360 degrees, and the sign of the result gives the direction of turning. Where this is a difference between some reference direction (say in an autopilot) it only needs calculating once and then storing until a new course is chosen. The deviations from the course would need to be calculated from every sample however.
I have a three lat-lon coordinates that make up two line segment A to B to C. I also found a function that can return north-bearing of a line segment A-B or B-C in -180 to 180 manner. However, I'm having trouble to determine when a car reaches from A to B, should it turn right or left to continue to C.
EDIT: Previous answer was wrong. now this is the correct
public Direction GetDirection(Point a, Point b, Point c)
{
double theta1 = GetAngle(a, b);
double theta2 = GetAngle(b, c);
double delta = NormalizeAngle(theta2 - theta1);
if ( delta == 0 )
return Direction.Straight;
else if ( delta == Math.PI )
return Direction.Backwards;
else if ( delta < Math.PI )
return Direction.Left;
else return Direction.Right;
}
private Double GetAngle(Point p1, Point p2)
{
Double angleFromXAxis = Math.Atan ((p2.Y - p1.Y ) / (p2.X - p1.X ) ); // where y = m * x + K
return p2.X - p1.X < 0 ? m + Math.PI : m ); // The will go to the correct Quadrant
}
private Double NormalizeAngle(Double angle)
{
return angle < 0 ? angle + 2 * Math.PI : angle; //This will make sure angle is [0..2PI]
}
Edited to fix the over 180 issue, also now supports U-Turns.
const int THRESHOLD = 0;
Direction TurnLeftOrRight(Point A, Point B, Point C)
{
int angle = ToAngle(B,C) - ToAngle(A,B);
if((angle > THRESHOLD && angle < 180 - THREASHOLD) || angle < -180 - THREASHOLD)
return Direction.Right;
else if ((angle < 0 - THREASHOLD && angle > -180 + THREASHOLD) || angle > 180 + THREASHOLD)
return Direction.Left;
else if (angle >= 0 - THREASHOLD && angle <= THREASHOLD)
return Direction.Straight
else
return Direction.UTurn;
}
You could also do tolerances between left right and strait just change the first angle > 0 to angle > 45 and the second one to angle < -45
I think your life will be simpler if you use the vector cross product.
Although strictly speaking cross product is defined only for 3D vectors, for 2D vectors p=(px,py) and q=(qx,qy), you can think of their cross product p×q as as pxqy - pyqx. This latter number will be positive if p is clockwise from q, and negative if p is counter-clockwise from q. It will be zero if p and q are parallel - i.e. point in the same or opposite directions.
In your case, you are using (lat,lon). The equivalent in (x,y) coordinates is (-lon, lat), so if you have two vectors (lat1,lon1) and (lat2,lon2), you want to calculate (-lon1, lat1)×(-lon2, lat2), which comes out to lat1*lon2-lon1*lat2.
If this number is zero, you can use the dot product to tell whether the direction is straight or a U-turn.
So your code could look like this, assuming Points and Vectors are written in (lat, lon) form (the code would be a little different if they are in x and y):
public Direction GetTurnDirection(Point A, Point B, Point C)
{
Vector v1 = B - A ;
Vector v2 = C - B ;
double cross = v1.lat*v2.lon - v1.lon*v2.lat ;
if (cross > 0) { return Direction.Left ; }
if (cross < 0) { return Direction.Right ; }
double dot = v1.lat*v2.lat + v1.lon*v2.lon ;
if (dot > 0) { return Direction.Straight ; }
return Direction.UTurn ;
}
If AB is the bearing of B from A, and BC that of C from B, the
turn angle is remainder( BC-AB, 360.0); (assuming degrees). If this
is positive the turn is to the right. In your example remainder( BC-AB, 360.0)
is remainder(271,360) = -89.