I am currently working on code for use with DWM1000 modules I am using two anchors and one tag I recieve the distance between both anchors to the tag and trying to get the x,y position of the tag. But my x coords for the tag isn't right and seems to be acting like y coord as it changes when i move the tag closer to the anchors I was hoping someone could look at my code and see what I am doing wrong.
float a_r = (pow(-dist_right,2) + pow(dist_left,2) - pow(dist_l_r,2)) / (-2*dist_l_r);
x = dist_l_r/2 - a_r;
float t = pow(dist_right,2) - pow(a_r,2);
if(dist_left < dist_right){
Serial.println("Left");;
}else{
Serial.println("Right");
}
Serial.print("Distance Right: ");Serial.println(dist_right);
Serial.print("Distance Left: ");Serial.println(dist_left);
diff = abs(dist_left - dist_right);
Serial.print("Difference: ");Serial.println(diff);
Serial.print("A_R: ");Serial.println(a_r);
Serial.print("T: ");Serial.println(t);
Serial.print("X: ");Serial.println(x);
Two reference point trilateration yields two solutions - the actual location and a false location. In the case where the reference points form a base line along some impassable barrier such as a wall, then the ambiguity is resolvable by this additional information.
Given:
Attribution: By NavigationGuy - Own work, CC BY-SA 4.0, https://commons.wikimedia.org/w/index.php?curid=75898777
Then
Thus the code:
x = ((dist_left * dist_left) - (dist_right * dist-right) + (dist_l_r * dist_l_r)) /
(2 * dist_l_r) ;
y = sqrt((dist_left * dist_left) - (x * x)) ;
In your code your a_r variable corresponds to x above (my frame of reference is rotated by 90 degrees with respect to yours, but is arbitrary in any case), but your x variable bares no resemblance to y above. Your x variable is proportional to a_r not orthogonal to it, so it is not at all surprising that it varies depending on a_r.
To maintain your frame of reference and variable naming then:
x = sqrt((dist_left * dist_left) - (a_r * a_r)) ;
of if you prefer:
x = sqrt(pow(dist_left,2) - pow(a_r, 2)) ;
Related
I am reverse engineering a game from 1999 and I came across a function which looks to be checking if the player is within range of a 3d point for the triggering of audio sources. The decompiler mangles the code pretty bad but I think I understand it.
// Position Y delta
v1 = * (float * )(this + 16) - LocalPlayerZoneEntry - > y;
// Position X delta
v2 = * (float * )(this + 20) - LocalPlayerZoneEntry - > x;
// Absolute value
if (v1 < 0.0)
v1 = -v1;
// Absolute value
if (v2 < 0.0)
v2 = -v2;
// What is going on here?
if (v1 <= v2)
v1 = v1 * 0.5;
else
v2 = v2 * 0.5;
// Z position delta
v3 = * (float * )(this + 24) - LocalPlayerZoneEntry - > z;
// Absolute value
if (v3 < 0.0)
v3 = -v3;
result = v3 + v2 + v1;
// Radius
if (result > * (float * )(this + 28))
return 0.0;
return result;
Interestingly enough, when in game, it seemed like the triggering was pretty inconsistent and would sometimes be quite a bit off depending on from which side I approached the trigger.
Does anyone have any idea if this was a common algorithm used back in the day?
Note: The types were all added by me so they may be incorrect. I assume that this is a function of type bool.
The best way to visualize a distance function (a metric) is to plot its unit sphere (the set of points at unit distance from origin -- the metric in question is norm induced).
First rewrite it in a more mathematical form:
N(x,y,z) = 0.5*|x| + |y| + |z| when |x| <= |y|
= |x| + 0.5*|y| + |z| otherwise
Let's do that for 2d (assume that z = 0). The absolute values make the function symmetric in the four quadrants. The |x| <= |y| condition makes it symmetric in all the eight sectors. Let's focus on the sector x > 0, y > 0, x <= y. We want to find the curve when N(x,y,0) = 1. For that sector it reduces to 0.5x + y = 1, or y = 1 - 0.5x. We can go and plot that line. For when x > 0, y > 0, x > y, we get x = 1 - 0.5y. Plotting it all gives the following unit 'circle':
For comparison, here is an Euclidean unit circle overlaid:
In the third dimension it behaves like a taxicab metric, effectively giving you a 'diamond' shaped sphere:
So yes, it is a cheap distance function, though it lacks rotational symmetries.
I'm trying to do a polar transform on the first image below and end up with the second. However my result is the third image. I have a feeling it has to do with what location I choose as my "origin" but am unsure.
radius = sqrt(width**2 + height**2)
nheight = int(ceil(radius)/2)
nwidth = int(ceil(radius/2))
for y in range(0, height):
for x in range(0, width):
t = int(atan(y/x))
r = int(sqrt(x**2+y**2)/2)
color = getColor(getPixel(pic, x, y))
setColor( getPixel(radial,r,t), color)
There are a few differences / errors:
They use the centre of the image as the origin
They scale the axis appropriately. In your example, you're plotting your angle (between 0 and in your case, pi), instead of utilising the full height of the image.
You're using the wrong atan function (atan2 works a lot better in this situation :))
Not amazingly important, but you're rounding unnecessarily quite a lot, which throws off accuracy a little and can slow things down.
This is the code combining my suggested improvements. It's not massively efficient, but it should hopefully work :)
maxradius = sqrt(width**2 + height**2)/2
rscale = width / maxradius
tscale = height / (2*math.pi)
for y in range(0, height):
dy = y - height/2
for x in range(0, width):
dx = x - width/2
t = atan2(dy,dx)%(2*math.pi)
r = sqrt(dx**2+dy**2)
color = getColor(getPixel(pic, x, y))
setColor( getPixel(radial,int(r*rscale),int(t*tscale)), color)
In particular, it fixes the above problems in the following ways:
We use dx = x - width / 2 as a measure of distance from the centre, and similarly with dy. We then use these in replace of x, y throughout the computation.
We will have our r satisfying 0 <= r <= sqrt( (width/2)^2 +(height/2)^2 ), and our t eventually satisfying 0 < t <= 2 pi so, I create the appropriate scale factors to put r and t along the x and y axes respectively.
Normal atan can only distinguish based on gradients, and is computationally unstable near vertical lines... Instead, atan2 (see http://en.wikipedia.org/wiki/Atan2) solves both problems, and accepts (y,x) pairs to give an angle. atan2 returns an angle -pi < t <= pi, so we can find the remainder modulo 2 * math.pi to it to get it in the range 0 < t <= 2pi ready for scaling.
I've only rounded at the end, when the new pixels get set.
Any questions, just ask!
I'm trying to rotate one point around a central point by an angle - standard problem. I've seen lots of posts about this but I can't get my implementation to work:
void Point::Rotate(const Point Pivot, const float Angle)
{
if (Angle == 0)
return;
float s = sin(Angle);
float c = cos(Angle);
x -= Pivot.x;
y -= Pivot.y;
x = (x * c) - (y * s) + Pivot.x;
y = (x * s) + (y * c) + Pivot.y;
}
This is my code, the logic of which I've gleaned from numerous source, for example here, and here.
As far as I'm aware, it should work. However, when I apply it to rotating for example, the point (0, 100) by 90 degrees (Pi/2 is given to the function) around (0, 0), the rotated point is apparently at (-100, -100); 100px below where it should be.
When trying to draw a circle (36 points) - it creates a vague heart shape. It looks like a graph I saw that I think was in polar coordinates - do I need to convert my point to Cartesian or something?
Can anyone spot anything wrong with my code?
Edit: Sorry, this function is a member function to a Point class - x and y are the member variables :/
You're almost there, but you're modifying x in the next-to-last line, meaning that the value of that coordinate fed into the y calculation is incorrect!
Instead, use temporary variables for the new x and y and then add the Pivot coordinates on afterwards:
double nx = (x * c) - (y * s);
double ny = (x * s) + (y * c);
x = nx + Pivot.x;
y = ny + Pivot.y;
My game needs to move by a certain angle. To do this I get the vector of the angle via sin and cos. Unfortunately sin and cos are my bottleneck. I'm sure I do not need this much precision. Is there an alternative to a C sin & cos and look-up table that is decently precise but very fast?
I had found this:
float Skeleton::fastSin( float x )
{
const float B = 4.0f/pi;
const float C = -4.0f/(pi*pi);
float y = B * x + C * x * abs(x);
const float P = 0.225f;
return P * (y * abs(y) - y) + y;
}
Unfortunately, this does not seem to work. I get significantly different behavior when I use this sin rather than C sin.
Thanks
A lookup table is the standard solution. You could Also use two lookup tables on for degrees and one for tenths of degrees and utilize sin(A + B) = sin(a)cos(b) + cos(A)sin(b)
For your fastSin(), you should check its documentation to see what range it's valid on. The units you're using for your game could be too big or too small and scaling them to fit within that function's expected range could make it work better.
EDIT:
Someone else mentioned getting it into the desired range by subtracting PI, but apparently there's a function called fmod for doing modulus division on floats/doubles, so this should do it:
#include <iostream>
#include <cmath>
float fastSin( float x ){
x = fmod(x + M_PI, M_PI * 2) - M_PI; // restrict x so that -M_PI < x < M_PI
const float B = 4.0f/M_PI;
const float C = -4.0f/(M_PI*M_PI);
float y = B * x + C * x * std::abs(x);
const float P = 0.225f;
return P * (y * std::abs(y) - y) + y;
}
int main() {
std::cout << fastSin(100.0) << '\n' << std::sin(100.0) << std::endl;
}
I have no idea how expensive fmod is though, so I'm going to try a quick benchmark next.
Benchmark Results
I compiled this with -O2 and ran the result with the Unix time program:
int main() {
float a = 0;
for(int i = 0; i < REPETITIONS; i++) {
a += sin(i); // or fastSin(i);
}
std::cout << a << std::endl;
}
The result is that sin is about 1.8x slower (if fastSin takes 5 seconds, sin takes 9). The accuracy also seemed to be pretty good.
If you chose to go this route, make sure to compile with optimization on (-O2 in gcc).
I know this is already an old topic, but for people who have the same question, here is a tip.
A lot of times in 2D and 3D rotation, all vectors are rotated with a fixed angle. In stead of calling the cos() or sin() every cycle of the loop, create variable before the loop which contains the value of cos(angle) or sin(angle) already. You can use this variable in your loop. This way the function only has to be called once.
If you rephrase the return in fastSin as
return (1-P) * y + P * (y * abs(y))
And rewrite y as (for x>0 )
y = 4 * x * (pi-x) / (pi * pi)
you can see that y is a parabolic first-order approximation to sin(x) chosen so that it passes through (0,0), (pi/2,1) and (pi,0), and is symmetrical about x=pi/2.
Thus we can only expect our function to be a good approximation from 0 to pi. If we want values outside that range we can use the 2-pi periodicity of sin(x) and that sin(x+pi) = -sin(x).
The y*abs(y) is a "correction term" which also passes through those three points. (I'm not sure why y*abs(y) is used rather than just y*y since y is positive in the 0-pi range).
This form of overall approximation function guarantees that a linear blend of the two functions y and y*y, (1-P)*y + P * y*y will also pass through (0,0), (pi/2,1) and (pi,0).
We might expect y to be a decent approximation to sin(x), but the hope is that by picking a good value for P we get a better approximation.
One question is "How was P chosen?". Personally, I'd chose the P that produced the least RMS error over the 0,pi/2 interval. (I'm not sure that's how this P was chosen though)
Minimizing this wrt. P gives
This can be rearranged and solved for p
Wolfram alpha evaluates the initial integral to be the quadratic
E = (16 π^5 p^2 - (96 π^5 + 100800 π^2 - 967680)p + 651 π^5 - 20160 π^2)/(1260 π^4)
which has a minimum of
min(E) = -11612160/π^9 + 2419200/π^7 - 126000/π^5 - 2304/π^4 + 224/π^2 + (169 π)/420
≈ 5.582129689596371e-07
at
p = 3 + 30240/π^5 - 3150/π^3
≈ 0.2248391013559825
Which is pretty close to the specified P=0.225.
You can raise the accuracy of the approximation by adding an additional correction term. giving a form something like return (1-a-b)*y + a y * abs(y) + b y * y * abs(y). I would find a and b by in the same way as above, this time giving a system of two linear equations in a and b to solve, rather than a single equation in p. I'm not going to do the derivation as it is tedious and the conversion to latex images is painful... ;)
NOTE: When answering another question I thought of another valid choice for P.
The problem is that using reflection to extend the curve into (-pi,0) leaves a kink in the curve at x=0. However, I suspect we can choose P such that the kink becomes smooth.
To do this take the left and right derivatives at x=0 and ensure they are equal. This gives an equation for P.
You can compute a table S of 256 values, from sin(0) to sin(2 * pi). Then, to pick sin(x), bring back x in [0, 2 * pi], you can pick 2 values S[a], S[b] from the table, such as a < x < b. From this, linear interpolation, and you should have a fair approximation
memory saving trick : you actually need to store only from [0, pi / 2], and use symmetries of sin(x)
enhancement trick : linear interpolation can be a problem because of non-smooth derivatives, humans eyes is good at spotting such glitches in animation and graphics. Use cubic interpolation then.
What about
x*(0.0174532925199433-8.650935142277599*10^-7*x^2)
for deg and
x*(1-0.162716259904269*x^2)
for rad on -45, 45 and -pi/4 , pi/4 respectively?
This (i.e. the fastsin function) is approximating the sine function using a parabola. I suspect it's only good for values between -π and +π. Fortunately, you can keep adding or subtracting 2π until you get into this range. (Edited to specify what is approximating the sine function using a parabola.)
you can use this aproximation.
this solution use a quadratic curve :
http://www.starming.com/index.php?action=plugin&v=wave&ajax=iframe&iframe=fullviewonepost&mid=56&tid=4825
I'm creating a 2D game and want to test for collision between an OBB (Oriented Bounding Box) and a Circle. I'm unsure of the maths and code to do this. I'm creating the game in c++ and opengl.
Since both your shapes are convex, you can use the Separating Axis Theorem. Here's a tutorial on how to implement an algorithm to do this.
Essentially, you try to find if it's possible to put a line somewhere that's between the two shapes and if you can't find one, then you know they're colliding.
References and general answer taken from this question.
Here's what I would do, in pseudocode:
function does_line_go_through_circle (original_line, circle_centerpoint, radius):
original_slope = get_slope_of_line (original_line)
perpendicular_slope = 1/original_slope
perpendicular_line = create_line_with_slope_through_point (perpendicular_slope, circle_centerpoint)
intersect_point = intersection_of_infinite_lines (perpendicular_line, original_line)
if point_is_on_line (intersect_point, original_line):
finite_line_along_radius = create_finite_line_between_points (circle_centerpoint, intersect_point)
if length_of_line (finite_line_along_radius) < length_of_line (radius):
return true
end
end
return false
end
function does_box_intersect_with_circle (bounding_box, circle):
for each side in bounding_box:
if does_line_go_through_circle (side, circle.center, circle.radius):
return true
end
end
return false
end
Keep in mind, I'm a little rusty on this stuff, I might be wrong.
Anyway, it should be trivial to implement this in C++.
We will divide the rectangle into 4 finite lines.
We can construct the line equation ax + by + c = 0 connecting the points (x1, y1) and (x2, y2) as follows:
mx - y + c = 0
where m = (y2-y1)/(x2-x1)
Shortest (perpendicular) distance from a line ax + by + c = 0 to a point (xc, yc) is given by the expression:
d = (a*xc + b*yc + c) / sqrt(a*a + b*b)
or d = (m*xc - yc + c) / sqrt(m*m + 1) according the the above equation
For infinite lines, you can check if 'd' is less than the radius of the circle.
But for finite lines, you'd also have to make sure whether the point of contact is within the line.
Now m = tan(angle). You can have
cos = 1 / sqrt(m*m + 1); sin = m / sqrt(m*m + 1)
Then you can calculate the point of contact as
xp = xc + d*cos; yp = yc + d*sin
And to check whether (xp, yp) lies in between the points connecting line, you can do a simple check as
if(x1 < xp < x2 && y1 < yp < y2)
return true
depending upon which is greater among x1 x2 and y1 y2.
You can repeat the same algorithm for all the four lines of a rectangle by passing the points.
Please correct me if I'm wrong somewhere.