Finding closest vector to point - vector

How can i sort vectors by distance from point?
For example i have three vectors: A, B, C and the point
Example image with point and vectors
And the sorted result must be something like this: (A, C, B)

Okay, this is more of a math question, but let me explain it here anyways. Take a look at this picture:
Let's define a line segment by vector A for the start point and a for the vector running through that line segment which end at the arrows end. Same is valid for the other segments B and C respectively. The point P as coordinates as also a vector.
Now let's make linear algebra our friend, yet be programatically efficient.
:-)
At the example of segment a you can do this and with the other respectively:
With the dot product of a and AP (vector from A to P) you get the projection projA on a where the where P is closest.
If you set A+ (projA)*na (na is the the normalized a vector) you get the closest Point in the vector a of P.
Let's set dA = A+projA*na - P and with its length you get the closest distance to compare.
Instead of saving the distances, try to store and compare squared distance of dA, dB and dC and compare those instead. It will save you to compute the square root which might become very expensive.
Here is some pseudocode:
vector3 AP = P-A;
vector3 projA = a.dot(AP);
vector3 nA = a.normalized();
dA = A + projA*na - P;
dA2 = dA.x*dA.x + dA.y*dA.y + dA.z*dA.z;
-> Compare and sort them by that value
Hope it helps a bit...

Related

How to calculate an orthogonal plane from a vector

I have a position in space called X1. X1 has a velocity called V1. I need to construct an orthogonal plane perpendicular to the velocity vector. The origin of the plane is X1.
I need to turn the two edges from the plane into two vectors, E1 and E2. The edges connect at the origin. So the three vectors form an axis.
I'm using the GLM library for the vector mathematics.
One way to create a frame from a vector is to use Householder transformations. This may seem complicated but the code is in quite short, at least as efficient as using cross products, and less prone to rounding error. Moreover exactly the same idea works in any number of dimensions.
The ideas is, given a vector v, find a Householder transformation that maps v to a multiple of (1,0,0), and then apply the inverse of this to (0,1,0) and (0,0,1) to get the other frame vectors. Since a Householder transformation is it's own inverse, and since they are simple to apply, the resulting code is fairly efficient. Below is C code that I use:
static void make_frame( const double* v, double* f)
{
double lv = hypot( hypot( v[0], v[1]), v[2]); // length of v
double s = v[0] > 0.0 ? -1.0 : 1.0;
double h[3] = { v[0] - s*lv, v[1], v[2]}; // householder vector for Q
double a = 1.0/(lv*(lv + fabs( v[0]))); // == 2/(h'*h)
double b;
// first frame vector is v normalised
b = 1.0/lv;
f[3*0+0] = b*v[0]; f[3*0+1] = b*v[1]; f[3*0+2] = b*v[2];
// compute other frame vectors by applying Q to (0,1,0) and (0,0,1)
b = -v[1]*a;
f[3*1+0] = b*h[0]; f[3*1+1] = 1.0 + b*h[1]; f[3*1+2] = b*h[2];
b = -v[2]*a;
f[3*2+0] = h[0]*b; f[3*2+1] = b*h[1]; f[3*2+2] = 1.0 + b*h[2];
}
In general you can define a plane in 3D using four numbers, e.g., Ax+By+Cz=D. You can think of the triple of numbers (A,B,C) as a vector that sticks out perpendicularly to the plane (called the normal vector).
The normal vector n = (A,B,C) only defines the orientation of the plane, so depending on the choice of the constant D you get planes at different distance from the origin.
If I understand your question correctly, the plane you're looking for has normal vector (A,B,C) = V1 and the constant D is obtained using the dot product: D = (A,B,C) . X1, i.e., D = AX1.x + BX1.y + C*X1.z.
Note you can also obtain the same result using the geometric equation of a plane n . ((x,y,z) - p0) = 0, where p0 is some point on the plane, which in your case is V1 . ( (x,y,z) - X1) = 0.

Check if 4 points in space are corner points of a rectangle

I have 4 points in space A(x,y,z), B(x,y,z), C(x,y,z) and D(x,y,z). How can I check if these points are the corner points of a rectangle?
You must first determine whether or not the points are all coplanar, since a rectangle is a 2D geometric object, but your points are in 3-space. You can determine they are coplanar by comparing cross products as in:
V1 = (B-A)×(B-C)
V2 = (C-A)×(C-D)
This will give you two vectors which, if A, B, C, and D are coplanar are linearly dependent. By considering what Wolfram has to say on vector dependence, we can test the vectors for linear dependence by using
C = (V1∙V1)(V2∙V2) - (V1∙V2)(V2∙V1)
If C is 0 then the vectors V1 and V2 are linearly dependent and all the points are coplanar.
Next compute the distances between each pair of points. There should be a total of 6 such distances.
D1 = |A-B|
D2 = |A-C|
D3 = |A-D|
D4 = |B-C|
D5 = |B-D|
D6 = |C-D|
Assuming none of these distances are 0, these points form a rectangle if and only if the vertices are coplanar (already verified) and these lengths can be grouped into three pairs where elements of each pair have the same length. If the figure is a square, two sets of the pairs will have be the same length and will be shorter than the remaining pair.
Update: Reading this again, I realize the the above could define a parallelogram, so an additional check is required to check that the square of the longest distance is equal to the sum of the squares of the two shorter distances. Only then will the parallelogram also be a rectangle.
Keep in mind all of this is assuming infinite precision and within a strictly mathematical construct. If you're planning to code this up, you will need to account for rounding and accept a degree of imprecision that's not really a player when speaking in purely mathematical terms.
Check if V1=B-A and V2=D-A are orthogonal using the dot product. Then check if
C-A == V1+V2
within numerical tolerances. If both are true, the points are coplanar and form a rectangle.
Here a function is defined to check whether the 4 points represents the rectangle or not .
from math import sqrt
def Verify(A, B, C, D, epsilon=0.0001):
# Verify A-B = D-C
zero = sqrt( (A[0]-B[0]+C[0]-D[0])**2 + (A[1]-B[1]+C[1]-D[1])**2 + (A[2]-B[2]+C[2]-D[2])**2 )
if zero > epsilon:
raise ValueError("Points do not form a parallelogram; C is at %g distance from where it should be" % zero)
# Verify (D-A).(B-A) = 0
zero = (D[0]-A[0])*(B[0]-A[0]) + (D[1]-A[1])*(B[1]-A[1]) + (D[2]-A[2])*(B[2]-A[2])
if abs(zero) > epsilon:
raise ValueError("Corner A is not a right angle; edge vector dot product is %g" % zero)
else:
print('rectangle')
A = [x1,y1,z1]
print(A)
B = [x2,y2,z2]
C = [x3,y3,z3]
D = [x4,y4,z4]
Verify(A, B, C, D, epsilon=0.0001)

Triangulating coordinates with an equation

Ok, I know this sounds really daft to be asking here, but it is programming related.
I'm working on a game, and I'm thinking of implementing a system that allows users to triangulate their 3D coordinates to locate something (eg for a task).
I also want to be able to let the user make the coordinates of the points they are using for triangulation have user-determined coordinates (so the location's coordinate is relative, probably by setting up a beacon or something).
I have a method in place for calculating the distance between the points, so essentially I can calculate the lengths of the sides of the triangle/pyramid as well as all but the coordinate I am after.
It has been a long time since I have done any trigonometry and I am rusty with the sin, cos and tan functions, I have a feeling they are required but have no clue how to implement them.
Can anyone give me a demonstration as to how I would go about doing this in a mathematical/programatical way?
extra info:
My function returns the exact distance between the two points, so say you set two points to 0,0,0 and 4,4,0 respectively, and those points are set to scale(the game world is divided into a very large 3d grid, with each 'block' area being represented by a 3d coordinate) then it would give back a value at around 5.6.
The key point about it varying is that the user can set the points, so say they set a point to read 0,0,0, the actual location could be something like 52, 85, 93. However, providing they then count the blocks and set their other points correctly (eg, set a point 4,4,0 at the real point 56, 89, 93) then the final result will return the relative position (eg the object they are trying to locate is at real point 152, 185, 93, it will return the relative value 100,100,0). I need to be able to calculate it knowing every point but the one it's trying to locate, as well as the distances between all points.
Also, please don't ask why I can't just calculate it by using the real coordinates, I'm hoping to show the equation up on screen as it calculates the result.7
Example:
Here is a diagram
Imagine these are points in my game on a flat plain.
I want to know the point f.
I know the values of points d and e, and the sides A,B and C.
Using only the data I know, I need to find out how to do this.
Answered Edit:
After many days of working on this, Sean Kenny has provided me with his time, patience and intellect, and thus I have now got a working implementation of a triangulation method.
I hope to place the different language equivalents of the code as I test them so that future coders may use this code and not have the same problem I have had.
I spent a bit of time working on a solution but I think the implementer, i.e you, should know what it's doing, so any errors encountered can be tackled later on. As such, I'll give my answer in the form of strong hints.
First off, we have a vector from d to e which we can work out: if we consider the coordinates as position vectors rather than absolute coordinates, how can we determine what the vector pointing from d to e is? Think about how you would determine the displacement you had moved if you only knew where you started and where you ended up? Displacement is a straight line, point A to B, no deviation, not: I had to walk around that house so I walked further. A straight line. If you started at the point (0,0) it would be easy.
Secondly, the cosine rule. Do you know what it is? If not, read up on it. How can we rearrange the form given in the link to find the angle d between vectors DE and DF? Remember you need the angle, not a function of the angle (cos is a function remember).
Next we can use a vector 'trick' called the scalar product. Notice there is a cos function in there. Now, you may be thinking, we've just found the angle, why are we doing it again?
Define DQ = [1,0]. DQ is a vector of length 1, a unit vector, along the x-axis. Which other vector do we know? Do we know of two position vectors?
Once we have two vectors (I hope you worked out the other one) we can use the scalar product to find the angle; again, just the angle, not a function of it.
Now, hopefully, we have 2 angles. Could we take one from the other to get yet another angle to our desired coordinate DF? The choice of using a unit vector earlier was not arbitrary.
The scalar product, after some cancelling, gives us this : cos(theta) = x / r
Where x is the x ordinate for F and r is the length of side A.
The end result being:
theta = arccos( xe / B ) - arccos( ( (A^2) + (B^2) - (C^2) ) / ( 2*A*B ) )
Where theta is the angle formed between a unit vector along the line y = 0 where the origin is at point d.
With this information we can find the x and y coordinates of point f relative to d. How?
Again, with the scalar product. The rest is fairly easy, so I'll give it to you.
x = r.cos(theta)
y = r.sin(theta)
From basic trigonometry.
I wouldn't advise trying to code this into one value.
Instead, try this:
//pseudo code
dx = 0
dy = 0 //initialise coordinates somehow
ex = ex
ey = ey
A = A
B = B
C = C
cosd = ex / B
cosfi = ((A^2) + (B^2) - (C^2)) / ( 2*A*B)
d = acos(cosd) //acos is a method in java.math
fi = acos(cosfi) //you will have to find an equivalent in your chosen language
//look for a method of inverse cos
theta = fi - d
x = A cos(theta)
y = A sin(theta)
Initialise all variables as those which can take decimals. e.g float or double in Java.
The green along the x-axis represents the x ordinate of f, and the purple the y ordinate.
The blue angle is the one we are trying to find because, hopefully you can see, we can then use simple trig to work out x and y, given that we know the length of the hypotenuse.
This yellow line up to 1 is the unit vector for which scalar products are taken, this runs along the x-axis.
We need to find the black and red angles so we can deduce the blue angle by simple subtraction.
Hope this helps. Extensions can be made to 3D, all the vector functions work basically the same for 3D.
If you have the displacements from an origin, regardless of whether this is another user defined coordinate or not, the coordinate for that 3D point are simply (x, y, z).
If you are defining these lengths from a point, which also has a coordinate to take into account, you can simply write (x, y, z) + (x1, y1, z1) = (x2, y2, z2) where x2, y2 and z2 are the displacements from the (0, 0, 0) origin.
If you wish to find the length of this vector, i.e if you defined the line from A to B to be the x axis, what would the x displacement be, you can use Pythagoras for 3D vectors, it works just the same as with 2D:
Length l = sqrt((x^2) + (y^2) + (z^2))
EDIT:
Say you have a user defined point A (x1, y1, z1) and you want to define this as the origin (0,0,0). You have another user chosen point B (x2, y2, z2) and you know the distance from A to B in the x, y and z plane. If you want to work out what this point is, in relation to the new origin, you can simply do
B relative to A = (x2, y2, z2) - (x1, y1, z1) = (x2-x1, y2-y1, z2-z1) = C
C is the vector A>B, a vector is a quantity which has a magnitude (the length of the lines) and a direction (the angle from A which points to B).
If you want to work out the position of B relative to the origin O, you can do the opposite:
B relative to O = (x2, y2, z2) + (x1, y1, z1) = (x1+x2, y1+y2, z1+z2) = D
D is the vector O>B.
Edit 2:
//pseudo code
userx = x;
usery = y;
userz = z;
//move origin
for (every block i){
xi = xi-x;
yi = yi - y;
zi = zi -z;
}

How to find the two opposite normals or two segments?

I have a two segments AB and CD (in red). These two segments are facing each others. They are not completely parallel but will never be perpendicular to each others either.
From that, I need to find the two normals of these segments (in blue) that oppose each others (i.e. the two normals are outside ABCD). I know how to calculate the normals of the segments but obviously each segment has two normals, and I cannot figure out how to programmatically select the ones I need. Any suggestion?
Calculate the vector v between the midpoints of the two segments, pointing from AB to CD. Now the projection of the desired normal to AB onto v must be negative and the projection of the desired normal to CD onto v must be positive. So just calculate the normals, check against v, and negate the normals if needed to make them satisfy the condition.
Here it is in Python:
# use complex numbers to define minimal 2d vector datatype
def vec2d(x,y): return complex(x,y)
def rot90(v): return 1j * v
def inner_prod(u, v): return (u * v.conjugate()).real
def outward_normals(a, b, c, d):
n1 = rot90(b - a)
n2 = rot90(d - c)
mid = (c + d - a - b) / 2
if inner_prod(n1, mid) > 0:
n1 = -n1
if inner_prod(n2, mid) < 0:
n2 = -n2
return n1, n2
Note that I assume the endpoints define lines meeting the conditions in the problem. Nor do I check for the edge case when the lines have the same midpoint; the notion of "outside" doesn't apply in that case.
I think there are two cases to consider:
Case 1: Intersection between lines occurs outside the endpoints of either segment.
In this case the midpoint method suggested by #Michael J. Barber will work for sure. So form a vector between the midpoints of the segments, compute the dot product of your normal vectors with this midpoint vector and check the sign.
If you're computing the normal for lineA, the dot product of the normal with the vector midB -> midA should be +ve.
Case 2: Intersection between lines occurs inside the endpoints of one segment.
In this case form a vector between either one of the endpoints of the segment that does not enclose the intersection point and the intersection point itself.
The dot product of the normal for the segment that does enclose the intersection point and this new vector should be +ve.
You can find the outward normal for the other segment by requiring that the dot product between the two normals is -ve (which would only be ambiguous in the case of perpendicular segments).
I've assumed that the segments are not co-linear or actually intersecting.
Hope this helps.
You can reduce the four combinations for the signs as follows:
Calculate the dot product of the normals, a negative sign indicates that both show outside or inside.
As I suppose that your normals have unit lenght, you can detect parallelity if the dot product has magnitude one. A positive value indicates that both show in the same direction, a negative value says that both show in different directions.
It the normals are not parallel: parametrize lines as x(t) = x0 + t * n for a normal n and calculate the t for which both intersect. A negative t will indicate that both show outside. It is enough if you do this for one of the normals, as you reduced your combinations from 4 to 2 in step 1.
If both normals are parralel: Calculate the time t for which the normals hit the midpoint between of your segments. As in 2. is enough if you do this for one of the normals, as you reduced your combinations from 4 to 2 in step 1.

Minimal perpendicular vector between a point and a line

Okay so I'm trying to get a separating axis theorem algorithm to work (for collision detection) and I need to find the minimal perpendicular vector between a point and a line. I'm not asking for the minimum perpendicular distance (which I know how to find) but rather the vector that would have the same magnitude as that distance and that goes from an arbitrary point and a point on the line. I know the location of the point, a point on the line, and a unit vector giving the direction of the line.
What I tried doing was first finding the minimal distance between the point and the line.
The next part is confusing but I:
1) Found the vector between the point and the point on the line I know
2) Found the vector between the point on the line and the point on the line plus the unit vector giving the direction of the line
3) Took the cross product of these two vectors (I'll call this cross product A)
4) Took the cross product of the unit vector giving the direction of the line and the vector from cross product A (I'll call this cross product B)
5) Normalized cross product B
6) Scaled cross product B by the minimal distance
Anyways that whole attempt failed miserably. Can anyone tell me how I am supposed to find this vector?
If I understood your question correctly, I believe this is what you're looking for:
P - point
D - direction of line (unit length)
A - point in line
X - base of the perpendicular line
P
/|
/ |
/ v
A---X----->D
(P-A).D == |X-A|
X == A + ((P-A).D)D
Desired perpendicular: X-P
where the period represents the dot product and |X-A| means magnitude.
From the above figure, you have:
q = p + s --> s = q - p = q - (p2-p1) = q + p1 - p2
==> s^ = |q - p2 - p1| / |s| (unitary vector)
Also: |s| = |q| sin c = |q|sin(b-a)
b = arcsin (qy / |q|); a = arcsin( p1y / |p1| )
where: |q| = (qx^2 + qy^2)^1/2

Resources