Nearest Neighbours using Quaternions - distance

Given a quaternion value, I would like to find its nearest neighbour in a set of quaternions. To do this, I clearly need a way to compare the "distance" between two quaternions. What distance representation is needed for such a comparison and how is it computed?
Thanks,
Josh

This is an old question, but it seemed to need a little more answer. If the quaternions are unit-length quaternions being used to represent rotations, then Euclidean distance will give some funny results because quaternions provide 2x redundant representation of rotation space; i.e., a quaternion and its negation represent the same orientation. In this case, the correct distance metric is the angle between the quaternions, constrained to fall within [0,pi/2]:
theta = acos(q1.w*q2.w + q1.x*q2.x + q1.y*q2.y + q1.z*q2.z);
if (theta>pi/2) theta = pi - theta;

Is your quaternion just a point in 3D space with an orientation?
Then the distance between two quaternions x1,y1,z1,w1 and x2,y2,x2,w2 is given by:
distance = sqrt((x1-x2)^2 + (y1-y2)^2 + (z1-z2)^2), assuming that the w component is used for orientation. I.e. this is the same as the distance between two 3D points.
Is your quaternion a point in 4D space?
Then the distance between them is given by:
distance = sqrt((x1-x2)^2 + (y1-y2)^2 + (z1-z2)^2 + (w1-w2)^2).
Which is just the extension to 4D space. This euclidean distance formula works in any number of dimensions.

This really depends on what you use your quaternions for. A simple distance measure would be the absolute value of their difference.
If
x = a + b i + c j + d k
y = e + f i + g j + h k
than the Euclidean distance would be
|x-y| = sqrt( (a-e)² + (b-f)² + (c-g)² + (d-h)² )

If "distance" you mean the shortest arc rotation between 2 orientations , than simple Euclidean distance is ok (L2 or norm2).
because angle between orientations can be written as
theta = acos(q1.w*q2.w + q1.x*q2.x + q1.y*q2.y + q1.z*q2.z);
Than, the bigger L2, the bigger distance.
NOTE: all quaternions before query should be negated if provide negative dot product. Than you can use usual KNN match to speedup your queries.

Related

Cheapest way to find Vector magnitude from a given point and angle

I am trying to determine a players depth position on a plane, which defines the walkable ground in a 2D brawler game. The problem is depictured in the following drawing:
C represents the players current position. I need to find the magnitude of vector V. Since I am not strong on linear algebra, the one thing I can think of is: determining the intersection point P of L1 and L2, and then take the magnitude from AP. However, I get the feeling there must be an easier way to find V, since I already know the angle the vector should have, given by vector from AB.
Any input would be appreciated, since I am looking forward to step up my linear algebra game.
Edit: As it is unclear thanks to my lack of drawing skills: the geometry depicted above is a parallelogram. The vector V I am looking for is parallel to the left and right side of the parallelogram. Depth does not mean, that I am looking for the vector perpendicular to the top side, but it refers to the fake depth of a purely 2D game. The parallelogram is therefore used as a means for creating the feeling of walking along a z axis.
The depth of your player (length of V) as measured from the top line in your drawing, is just the difference between A.y and C.y. This is seperate from the slant in the parralelogram, as we're just looking at depth.
example:
float v;
Vector2 a = new Vector2(100, 100); //The point you're measuring from
Vector2 c = new Vector2(150, 150); //Your character position
v = c.y - a.y; // This is the length of V.
//In numbers: 50 = 150 - 100
Illustrated: image not to scale
This works for any coördinate in your plane.
Now if you'd want to get the length of AC is when you'd need to apply some pythagoras, which is a² + b² = c². In the example that would mean in code:
Vector2 a = new Vector2(100, 100);
Vector2 c = new Vector2(150, 150);
float ac1 = Mathf.Sqrt(Mathf.Pow(c.x - a.x, 2) + Mathf.Pow(c.y - a.y, 2));
Now that is quite a sore to have to type out every time, and looks quite scary. But Unity has you covered! There is a Vector method called Distance
float ac2 = Vector2.Distance(a, c);
Which both return 70.71068 which is the length of AC.
This works because for any point c in your area you can draw a right angled triangle from a to c.
Edit as per comment:
If you want your "depth" vector to be parallel with the sides of the paralellogram we can just create a triangle in the parallelogram of which we calculate the hypotenuse.
Since we want the new hypotenuse of our triangle to be parallel to the parallelogram we can use the same angle θ as point B has in your drawing (indicated by pink in mine), of which I understood you know the value.
We also know the length of the adjacent (indicated in blue) side of this new triangle, as that is the height we calculated earlier (c.y - a.y).
Using these two values we can use cosine to find the length of hypotenuse (indicated in red) of the triangle, which is equal to the vector V, in parallel with the parallelogram.
the formula for that is: hypotenuse = adjacent/cos(θ)
Now if we were to put some numbers in this, and for my example I took 55 for the angle θ. It would look like this
float v = 50/(cos(55));
image not to scale
Let's call the lower right vertex of the parallelogram D.
If the long sides of the parallelogram are horizontal, you can find magnitude of V vector by:
V.magnitude = (c.y - a.y) / sin(BAD)
Or if you prefer:
V.magnitude = AB.magnitude * (c.y - a.y)/(b.y - a.y)

Calculating a spiral in MATLAB

We have these logarithmic spirals which are circling around the centre of the coordinate system:
x = ebθ cos(θ)
y = ebθ sin(θ)
where the ebθ is the distance between the point (which is on the spiral) and the centre; and the θ is the angle between the line connecting the point and the origin and the axis x.
Consider a spiral where the angle is θ ϵ <0,10π> and the parameter is b=0.1. By thickening points on the spirals (and the angle θ) calculate the circumference with the relative precision better than 1%. Draw the spiral!
I'm preparing for a (MATLAB) test and I'm stuck with this exercise. Please help, any hint is appreciated.
Start by computing a list of x,y for your range of theta and value of b. For more accurate results, have your theta increment in smaller steps (I chose 5000 arbitrarily). Then, its simply computing the distance for each pair of consecutive points and summing them up.
t = linspace(0,10*pi,5000);
b = 0.1;
x = exp(b*t).*cos(t);
y = exp(b*t).*sin(t);
result = sum(sqrt((x(2:end) - x(1:end-1)).^2 + (y(2:end)-y(1:end-1)).^2))

Find Position based on signal strength (intersection area between circles)

I'm trying to estimate a position based on signal strength received from 4 Wi-Fi Access Points. I measure the signal strength from 4 access points located in each corner of a square room with 100 square meters (10x10). I recorded the signal strengths in a known position (x, y) = (9.5, 1.5) using an Android phone. Now I want to check how accurate can a multilateration method be under the circumstances.
Using MATLAB, I applied a formula to calculate distance using the signal strength. The following MATLAB function shows the application of the formula:
function [ d_vect ] = distance( RSS )
% Calculate distance from signal strength
result = (27.55 - (20 * log10(2400)) + abs(RSS)) / 20;
d_vect = power(10, result);
end
The input RSS is a vector with the four signal strengths measured in the test point (x,y) = (9.5, 1.5). The RSS vector looks like this:
RSS =
-57.6000
-60.4000
-44.7000
-54.4000
and the resultant vector with all the estimated distances to each access points looks like this:
d_vect =
7.5386
10.4061
1.7072
5.2154
Now I want to estimate my position based on these distances and the access points position in order to find the error between the estimated position and the known position (9.5, 1.5). I want to find the intersection area (In order to estimate a position) between four circles where each access point is the center of one of the circles and the distance is the radius of the circle.
I want to find the grey area as shown in this image :
http://www.biologycorner.com/resources/venn4.gif
If you want an alternative way of estimating the location without estimating the intersection of circles you can use trilateration. It is a common technique in navigation (e.g. GPS) to estimate a position given a set of distance measurements.
Also, if you wanted the area because you also need an estimate of the uncertainty of the position I would recommend solving the trilateration problem using least squares which will easily give you an estimate of the parameters involved and an error propagation to yield an uncertainty of the location.
I found an answear that solved perfectly the question. It is explained in detail in this link:
https://gis.stackexchange.com/questions/40660/trilateration-algorithm-for-n-amount-of-points
I also developed some MATLAB code for the problem. Here it goes:
Estimate distances from the Access Points:
function [ d_vect ] = distance( RSS )
result = (27.55 - (20 * log10(2400)) + abs(RSS)) / 20;
d_vect = power(10, result);
end
The trilateration function:
function [] = trilat( X, d, real1, real2 )
cla
circles(X(1), X(5), d(1), 'edgecolor', [0 0 0],'facecolor', 'none','linewidth',4); %AP1 - black
circles(X(2), X(6), d(2), 'edgecolor', [0 1 0],'facecolor', 'none','linewidth',4); %AP2 - green
circles(X(3), X(7), d(3), 'edgecolor', [0 1 1],'facecolor', 'none','linewidth',4); %AP3 - cyan
circles(X(4), X(8), d(4), 'edgecolor', [1 1 0],'facecolor', 'none','linewidth',4); %AP4 - yellow
axis([0 10 0 10])
hold on
tbl = table(X, d);
d = d.^2;
weights = d.^(-1);
weights = transpose(weights);
beta0 = [5, 5];
modelfun = #(b,X)(abs(b(1)-X(:,1)).^2+abs(b(2)-X(:,2)).^2).^(1/2);
mdl = fitnlm(tbl,modelfun,beta0, 'Weights', weights);
b = mdl.Coefficients{1:2,{'Estimate'}}
scatter(b(1), b(2), 70, [0 0 1], 'filled')
scatter(real1, real2, 70, [1 0 0], 'filled')
hold off
end
Where,
X: matrix with APs coordinates
d: distance estimation vector
real1: real position x
real2: real position y
If you have three sets of measurements with (x,y) coordinates of location and corresponding signal strength. such as:
m1 = (x1,y1,s1)
m2 = (x2,y2,s2)
m3 = (x3,y3,s3)
Then you can calculate distances between each of the point locations:
d12 = Sqrt((x1 - x2)^2 + (y1 - y2)^2)
d13 = Sqrt((x1 - x3)^2 + (y1 - y3)^2)
d23 = Sqrt((x2 - x3)^2 + (y2 - y3)^2)
Now consider that each signal strength measurement signifies an emitter for that signal, that comes from a location somewhere at a distance. That distance would be a radius from the location where the signal strength was measured, because one would not know at this point the direction from where the signal came from. Also, the weaker the signal... the larger the radius. In other words, the signal strength measurement would be inversely proportional to the radius. The smaller the signal strength the larger the radius, and vice versa. So, calculate the proportional, although not yet accurate, radius's of our three points:
r1 = 1/s1
r2 = 1/s2
r3 = 1/s3
So now, at each point pair, set apart by their distance we can calculate a constant (C) where the radius's from each location will just touch one another. For example, for the point pair 1 & 2:
Ca * r1 + Ca * r2 = d12
... solving for the constant Ca:
Ca = d12 / (r1 + r2)
... and we can do this for the other two pairs, as well.
Cb = d13 / (r1 + r3)
Cc = d23 / (r2 + r3)
All right... select the largest C constant, either Ca, Cb, or Cc. Then, use the parametric equation for a circle to find where the coordinates meet. I will explain.
The parametric equation for a circle is:
x = radius * Cos(theta)
y = radius * Sin(theta)
If Ca was the largest constant found, then you would compare points 1 & 2, such as:
Ca * r1 * Cos(theta1) == Ca * r2 * Cos(theta2) &&
Ca * r1 * Sin(theta1) == Ca * r2 * Sin(theta2)
... iterating theta1 and theta2 from 0 to 360 degrees, for both circles. You might write code like:
for theta1 in 0 ..< 360 {
for theta2 in 0 ..< 360 {
if( abs(Ca*r1*cos(theta1) - Ca*r2*cos(theta2)) < 0.01 && abs(Ca*r1*sin(theta1) - Ca*r2*sin(theta2)) < 0.01 ) {
print("point is: (", Ca*r1*cos(theta1), Ca*r1*sin(theta1),")")
}
}
}
Depending on what your tolerance was for a match, you wouldn't have to do too many iterations around the circumferences of each signal radius to determine an estimate for the location of the signal source.
So basically you need to intersect 4 circles. There can be many approaches to it, and there are two that will generate the exact intersection area.
First approach is to start with one circle, intersect it with the second circle, then intersect the resulting area with the third circle and so on. that is, on each step you know current intersection area, and you intersect it with a new circle. The intersection area will always be a region bounded by circle arcs, so to intersect it with a new circle you walk along the boundary of the area and check whether each bounding arc intersects with a new circle. If it does, then you leave only the part of the arc that lies inside a new circle, remember that you should continue with an arc from a new circle, and continue traversing the boundary until you find the next intersection.
Another approach that seems to result in a worse time complexity, but in your case of 4 circles this will not be important, is to find all the intersection points of two circles and choose only those points that are of interest for you, that is which lie inside all other circles. These points will be the corners of your area, and then it is rather easy to reconstruct the area. After googling a bit, I have even found a live demo of this approach.

finding the intersection of a line with a non monotonic arbitrary surface?

I have a surface Z on a X-Y grid for which I want to find the intersection point with a line. I used so far this code for finding the intersection:
x_ray = x_source + t * x_dir
y_ray = y_source + t * y_dir
z_ray = z_source + t * z_dir
height_above_plane = #(t) z_source + t * z_dir - interp2(X, Y, Z, ...
x_source + t*x_dir, y_source + t*y_dir)
t_intercept = fzero(height_above_plane, 0);
my problem is that when my surface is "wiggly", the function has several zero crossing points, and I want to find the minimal out of them.
How can I do that?
Thanks
A possible approach is to project the ray onto the XY domain and draw the corresponding Bresenham line. As you go along this line, grid cell per grid cell, you will compute the Z altitudes along the ray and check if their range overlaps the range of altitudes of the surface (i.e. the min and max value in this cell).
If yes, you have to find the 3D intersection between the ray and the interpolating surface, an hyperbolic paraboloid. If the intersection does fall inside the grid cell considered, you are done. Otherwise, continue the march along the ray.
Convert the surface to matlab mesh, then use this code.

How to calculate realistic distance between two points on isometric maps?

I'am developing an isometric game, I have to calculate if an item is in the range of another, so in a normal projection a circle would be ok I mean:
float dist = sqrt((x1-x2)^2 + (y1-y2)^2);
but as this is an isometric projection where a "x" unit is two times a "y" unity, how could I calculate the range of an item?
Any tip would be helpful
I'm not sure whether I understand which unit is bigger than the other and in which reference system but one of the below expressions should work:
float dist = sqrt(((x1-x2)/2)^2 + (y1-y2)^2);
or
float dist = sqrt((x1-x2)^2 + ((y1-y2)/2)^2);
This is the correct formula:
float dist = sqrt((x2-x1) + (2*(y2-y1))^2);
The y axis is half that of x in isometric.
Therefore the distance travelled in y is twice as much in the projection, but note this is not the same as halving the x-distance.
I hope this helps.