Calculate the difference in azimuth angles between 2 Poses - java

I have 2 Pose representing 2 positions of the camera and I want to get the difference between their azimuth angles.
The older pose is retrieved from an anchor set with the older camera pose, so that I shouldn't get errors from updates of ARCore's world understanding.
The newer pose is retrieved from the current frame.
I tried to use this formula from wikipedia:
psi = atan2(
2*(qw*qz + qx*qy),
1-2*(qy*qy + qz*qz)
)
Then I substract the older angle from the newer, with no success: when I move the phone to modify the pitch angle only, the result I get also varies.
I think it didn't work because it assumes +Z to be the vertical axis, whereas +Y is the vertical axis in ARCore. So I rotated the axes in the formula so that the vertical axis is Y :
psi = atan2(
2*(qw*qy + qz*qx),
1-2*(qx*qx + qy*qy)
)
It still doesn't work, the result still varies when I change the pitch only. Apparently this is not the right transformation to do.
How can I calculate the difference in azimuth angle between the 2 poses of the camera ?
This might actually be a question for Mathematics Stack Exchange, but I'm not sure if I'm misunderstanding ARCore or the maths, so here it is.

Use the following approach to calculate azimuth that always measured in two dimensions:
public float getAzimuth(PointF aim) {
float angle = Math.toDegrees(Math.atan2(aim.x - x, aim.y - y));
// the range of ± 90.0° must be corrected...
if(angle < 0.0) {
angle += 360.0;
}
return angle;
}
...the following approach to calculate a distance:
float distance = Math.sqrt((x2 – x1) / 2.0 +
(y2 – y1) / 2.0 +
(z2 – z1) / 2.0);
...and the following approach to calculate a plunge:
float plunge = Math.asin((z2 – z1) / distance)

Related

Trigonometry with Java

I'm trying to do some basic trigonometry with Java and LibGDX on android.
I've spent a long time googling "How to find an angle in right triangles".
I still don't really understand :(
I want to give an Actor subclass a random direction to follow. So what is the angle - and what should I set xSpeed and ySpeed to, in order to move at the correct angle.
I started writing an app to help me see how it works.
There are two objects - An origin point and a touch point. User presses screen, touchPoint moves to where user touched. Methods fire to figure out the appropriate values. I know the XDistance and YDistance between the two points. That means I know the Opposite length and the Adjacent length. So all I need to do is tan-1 of (opposite / adjacent), am I right?
I just don't understand what to do with the numbers my program spits out.
Some code:
In create event of main class:
stage.addListener(new ClickListener() {
#Override
public void touchDragged(InputEvent event, float x, float y, int pointer) {
touchPoint.setX(x);
touchPoint.setY(y);
touchPoint.checkDistance(); // saves x and y distances from origin in private fields
atan2D = getAtan2(touchPoint.getYDistance(), touchPoint.getXDistance());
tanhD = getTanh(touchPoint.getYDistance(), touchPoint.getXDistance());
xDistanceLbl.setText("X Distance: " + touchPoint.getXDistance());
yDistanceLbl.setText("Y Distance: " + touchPoint.getYDistance());
atan2Lbl.setText("Atan2: " + atan2D);
tanhLbl.setText("Tanh: " + tanhD);
angleLbl.setText("Angle: No idea");
}
})
...
private double getAtan2(float adjacent, float opposite) {
return Math.atan2(adjacent, opposite);
}
private double getTanh(float adjacent, float opposite) {
return Math.tanh((adjacent / opposite));
}
These two functions give me numbers between (atan2: -pi to pi) and (tanh: -1.0 to 1.0)
How do I turn these values into angles from which I can then work backwards and get the opposite and adjacent again?
Doing this should allow me to create and object with a random direction, which I can use in 2D games.
atan2 gives you direction in radians. Direction from origin (0,0) to touchPoint. If you need direction from some object to touchPoint, then subtract object coordinates. Perhaps you also want to see direction in degrees (this is only for human eyes)
dx = x - o.x
dy = y - o.y
dir = atan2(dy, dx)
dir_in_degrees = 180 * dir / Pi
I you have direction and want to retrieve coordinate differences, you need to store distance
distance = sqrt(dx*dx + dy*dy)
later
dx = distance * cos(dir)
dy = distance * sin(dir)
But note that often storing dx and dy is better, because some calculations might be performed without trigonometric functions
Just noticed - using tanh is completely wrong, this is hyperbolic tangent function, it has no relation to geometry.
You can use arctan, but it gives angle in half-range only (compared with atan2)

Rotating point about pivot point in 3d space

I've made a Java program that displays lines in 3d space projected onto the 2d view, and so far, it's been working pretty well. I tried to make it possible to essentially rotate the 'world' about the camera's position about any axis, but now I'm running into some problems.
public void rotate(){
float ax = main.angleX; //main = camera
float ay = main.angleY;
float az = main.angleZ;
for(Line3d line : lines){ //all lines in the world
Vector3d start = Vector3d.Vector3dPMinus(line.start, main.getPoint()); //vetor value of starting point of line - camera's position
Vector3d end = Vector3d.Vector3dPMinus(line.end, main.getPoint());
start.rotate(ax, ay, az);
end.rotate(ax, ay, az); //rotate each vector
line.start = Point3d.pointFromVector3d(start).add(main.getPoint());
line.end = Point3d.pointFromVector3d(end).add(main.getPoint()); //vectors back into points
}
}
Rotation function:
public Vector3d rotate(float ax, float ay, float az){
Math.toRadians(ax *= 90);
Math.toRadians(ay *= 90);
Math.toRadians(az *= 90);
y = (float) (y * Math.cos(ax) - z * Math.sin(ax));
z = (float) (y * Math.sin(ax) + z * Math.cos(ax));
x = (float) (x * Math.cos(ay) + z * Math.sin(ay));
z = (float) (z * Math.cos(ay) - x * Math.sin(ay));
x = (float) (x * Math.cos(az) - y * Math.sin(az));
y = (float) (x * Math.sin(az) + y * Math.cos(az));
return this;
}
I've set it rotate about the x axis 3 times per second, and it displays exactly what I want it to before it starts rotating, but once it starts rotating, there's just some unidentifiable mess of usually just one horizontal line.
Is the method I used for rotating not right? Is there a better way to do it?
A Rotation of a 3d vector around the 3 axis is not that trivial as one might think. The thing you are probably trying to do is rotating with so called Euler Angles. You should be familiar with matrices to work with 3d space. I checked your rotations and they should work fine. BUT you should keep the following in mind: When you are rotating around an angle. The following rotations are affected by the previous rotation. You are rotating the rotation axis too.
To avoid this behaivour one ugly possibility is to rotate around a free axis. And when you are rotating around the x axis first. You rotate the y axis in reverse and rotate then your point around this y'. When you are rotating around z you need to rotate z axis with your reversed x and y rotation and then rotate around this z'. When you are doing this, you can always rotate around your world coordinatesystem. You should really use some math Library to accomplish this. It makes your life much easier. When you want to code it yourself. You need a proper matrice class with multiplication of matrices and vectors and some inversion method.
Your approach appears to take a reasonable general form. It looks like you are rotating the line endpoints relative to the current camera position, which is correct, and the three specific rotations you are performing could also be correct (but see below).
However, the three Math.toRadians() calls cannot be doing anything useful, because you ignore their results. Moreover, the expression ax *= 90 and its mates look awfully suspicious: are ax, ay, and az really expressed as fractions of a quarter-circle? That seems doubtful, and if it were the case then you would want to multiply by Math.PI/2 and skip toRadians(). On the other hand, if they are expressed in degrees then the following version of rotate() is correct for one reasonable definition of ax, ay, and az, and some possible Vector3D implementations:
public Vector3d rotate(double ax, double ay, double az){
ax = Math.toRadians(ax);
ay = Math.toRadians(ay);
az = Math.toRadians(az);
y = (float) ( y * Math.cos(ax) - z * Math.sin(ax));
z = (float) ( y * Math.sin(ax) + z * Math.cos(ax));
x = (float) ( x * Math.cos(ay) + z * Math.sin(ay));
z = (float) (-x * Math.sin(ay) + z * Math.cos(ay));
x = (float) ( x * Math.cos(az) - y * Math.sin(az));
y = (float) ( x * Math.sin(az) + y * Math.cos(az));
return this;
}
Overall, though, I am also a bit dubious of the correctness of the ax, ay, and az for this purpose. In particular, be aware that you cannot just accumulate separate x, y, and z rotation increments independently, as the resulting aggregate rotation depends greatly on the order in which the incremental rotations are performed. Moreover, even if ax, ay, and az correctly describe the orientation of the camera, it is unlikely that applying the same rotation to the world is what you actually want to do.
DESPITE ALL THE FOREGOING, though, I don't see any reason why your code would distort the model as you describe it doing. I don't think it will apply the rotation you want (even with my suggested fix), but the reason for the distortion is likely somewhere else.

Calculating distance in pixels between two locations

I'm trying to create a method which calculates the x and y in a grid and eventually the distance between that point and the middle. The problem is, I only know a few values. To explain the case a bit better an image:
(the values between '(..,..)' are lat/long combinations).
As you can see, I know the following values:
start of canvas: xy(0,0)
middle of canvas: xy(540,800) and lat/long(52.3702160, 4.8951680)
max dimension of canvas: x 1080, y 1600
point: xy(?,?) and lat/long(52.4167267, 4.8052174)
point: xy(?,?) and lat/long(52,2422306, 5.1129068)
First, I need to do something to calculate the missing x and y's from the points.
I already tried doing the following:
double mapWidth = screenWidth;
double mapHeight = screenHeight;
// get x value
x = (location.getLongitude()+180)*(mapWidth/360);
// convert from degrees to radians
double latRad = location.getLatitude()*Math.PI/180;
// get y value
double mercN = Math.log(Math.tan((Math.PI/4)+(latRad/2)));
y = (mapHeight/2)-(mapWidth*mercN/(2*Math.PI));
point = new PointF((float)x,(float)y);
This works but I'm getting the wrong x and y values.
For example if my points lat/long are further away the x and y's are getting bigger (more to the middle). But they need to be more at the side because the lat/long point is further away.
All lat/long points inside 2km diameter need to be in my grid, if the point is for example 0.9km away from the center it needs to be nearly at the side.
After that I need to calculate the distance between the two points. I already got that part using the following:
Math.sqrt((point.x - point2.x) * (point.x - point2.x) + (point.y - point2.y) * (point.y - point2.y));
My main problem is calculating the x and y from my lat/long points.
If anyone wants to help, thanks in advance!
I completely rethought my way of calculating the x and y.
I solved it by doing the following:
double distanceMeters = mCurrentLocation.distanceTo(location);
x = ((1000+distanceMeters)*mMiddleCoords.x)/1000; //1000 = radius.
y = ((1000+distanceMeters)*mMiddleCoords.y)/1000;
You can't directly use the distance formula from latitude and longitude. You'll have to take into account the curvature of the sphere to calculate the distance.
The minimum distance between two points on a sphere (and hence earth, simplifying it to a perfect sphere) is the length of the chord on what is called the Great Circle running through those points. A Great Circle is a circle with its center running through the center of the sphere).
From Wikipedia:
C = SQRT(X^2 + Y^2 + Z^2)
where:
X = cos(lat2) * cos(long2) - cos(lat1) * cos(long1)
Y = cos(lat2) * sin(long2) - cos(lat1) * sin(long1)
Z = sin(lat2) - sin(lat1)
And the distance is (2 * R * arcsin(C/2)) where R is the radius of the earth or 6371 km
The other alternative - if you know you will always have the Android libraries - is the Location.distanceTo() method.

How does the rotation angle using a rotation matrix correspond to a degree

I am trying to rotate a box in java using a rotation matrix.
(I am using the LWJGL and Slick 2D libraries)
my code to rotate 1 point around the center point is this:
point1X = (float) (centerX * Math.cos(rotation) - centerY * Math.sin(rotation));
point1Y = (float) (centerX * Math.sin(rotation) + centerY * Math.cos(rotation));
Right now I just update the rotation every update like so:
rotation += delta * 0.001;
This works great except the rotation number does not seem to correspond to a degree from 0˚ to 360˚
Is there a formula or something that will translate the rotation number to a readable degree and vice versa?
Normally, trig functions expect their arguments to be in radians, not degrees.
2*pi radians = 360 degrees.

Angle that a moving ball will bounce off of an inert ball

Let there be two balls, one of which is moving about in the Cartesian coordinate plane, while the other is stationary and immobile. At some point, the moving ball collides with the inert ball. Assuming the moving ball is traveling in a straight line, how can one derive the new angle that the moving ball will be propelled given the following information:
The moving ball's center coordinates (X0, Y0), radius (R0), and angle of travel before impact (A0)
The stationary ball's center coordinates (X1, Y1) and radius (R1)
If your second ball has infinite mass:
Where phi (after a long calc) is:
phi= -ArcTan[
( 2 R^2 Sin[A0] + 2 (YD Cos[A0] - XD Sin[A0]) (2 H Cos[A0] +
2 XD Sin[A0]^2 - YD Sin[2 A0])) /
((2 R^2 - XD^2 - 3 YD^2) Cos[A0] + (XD^2 - YD^2) Cos[3 A0] +
8 XD YD Cos[A0]^2 Sin[A0] + 4 H Sin[A0] (-YD Cos[A0] + XD Sin[A0]))
]
Where:
H = (R0 + R1)^2 - ((Y0 - Y1) Cos[A0] + (X0 - X1) Sin[A0])^2
R^2 = (R0 + R1)^2
XD = X1 - X0
YD = Y1 - Y0
Edit
To determine the whole trajectory, you'll also need the coordinates for the center of the moving ball at the time of impact. They are:
{X,Y}= {X1+Sin[A0] ((Y1-Y0) Cos[A0]+ (X0-X1) Sin[A0])-Cos[A0] Sqrt[H],
Y1+Cos[A0] ((Y0-Y1) Cos[A0]+(-X0+X1) Sin[A0])-Sin[A0] Sqrt[H]}
Page 3 of Pool Hall Lessons by Joe van den Heuvel, Miles Jackson gives a great example of how to do this.
// First, find the normalized vector n from the center of circle1 to the center of circle2
Vector n = circle1.center - circle2.center;
n.normalize();
// Find the length of the component of each of the movement vectors along n.
float a1 = v1.dot(n);
float a2 = v2.dot(n);
float optimizedP = (2.0 * (a1 - a2)) / (circle1.mass + circle2.mass);
// Calculate v1', the new movement vector of circle1
// v1 = v1 - optimizedP * m2 * n
Vector v1 = v1 - optimizedP * circle2.mass * n;
// Calculate v2', the new movement vector of circle2
// v2 = v2 + optimizedP * m1 * n
Vector v2 = v2 + optimizedP * circle1.mass * n;
circle1.setMovementVector(v1);
circle2.setMovementVector(v2);
Read at least page three to understand whats going on here.
You should take a look at the elastic collision article on wikipedia. I would explain here, but everything I could have said, wikipedia says it better and with clear examples and equations.
[A long, long time ago I studied this as an undergrad. ]
You need to be clear on the masses. Probably you are assuming equal mass for both balls, as opposed to one being of infinite mass.
The second thing is: Are you interested in considering rolling constraints as well as linear momentum. The treatments you will come across which talk along the lines of a simplistic elastic collision ignore all this. As an example, consider shots in pool/ snooker where you deliberately strike the ball away from the midpoint to generate front or backspin.
Do you want to able to do this?
If so, you need to consider the friction between a spinning ball and the surface.
For example in a "simple" straight-on collision between a rolling ball and a stationary one, if we assume perfectly elastic (again not quite true):
the initial collision stops the moving ball 'A'
the stationary ball 'B' starts moving at the impact speed of 'A'
'A' still has spin, it grips the surface and picks up some small velocity
'B' starts without spin and has to match it to its speed in order to roll. This results in it slowing slightly.
For the simplistic case, the calculation is much easier if you transform to the coordinates of the centre of mass. In that frame, the collision is always a straight-on collision, reversing the direction of the balls. You then just transform back to get the resultants.
Assuming indetical masses and speeds prior to the impact of v1 and w1.
V0 = centre of mass speed = (v1+w1)/2
v1_prime = v of mass_1 in transformed coords = v1 - V0
w1_prime = w1 - V0
Post collision, we have a simple reflection:
v2_prime = -v1_prime (== w1_prime)
w2_prime = -vw_prime (== v1_prime)
v2 = v2_prime + V0
w2 = w2_prime + V0
It simply reflects from the stationary ball. So compute the point of contact (the centres of the balls will be R0 + R1 apart) and the axis of reflection will be the line joining the centres.
EDIT: By which I mean the line joining the centres at the point of contact will have an angle, and you can use this angle to help compute the new angle of the moving ball.

Categories