I've been trying to improve the behavior of one of the bosses in a top-down perspective shooter game that I'm working on, and one thing I haven't been able to quite implement correctly is plotting an intercept trajectory between the boss' "hook" projectile and the player according to the player's movement.
I've tried implementing it using the quadratic equation described here: https://stackoverflow.com/a/2249237/1205340
But I had pretty much the same results as this algorithm I came up with, which often will aim close to the player's expected position, but almost always misses unless the player is backpedaling away from the boss.
private float findPlayerIntercept(Pair<Float> playerPos, Pair<Float> playerVel, int delta) {
float hookSpeed = HOOK_THROW_SPEED * delta;
Pair<Float> hPos = new Pair<Float>(position);
Pair<Float> pPos = new Pair<Float>(playerPos);
// While the hook hasn't intercepted the player yet.
while(Calculate.Distance(position, hPos) < Calculate.Distance(position, pPos)) {
float toPlayer = Calculate.Hypotenuse(position, pPos);
// Move the player according to player velocity.
pPos.x += playerVel.x;
pPos.y += playerVel.y;
// Aim the hook at the new player position and move it in that direction.
hPos.x += ((float)Math.cos(toPlayer) * hookSpeed);
hPos.y += ((float)Math.sin(toPlayer) * hookSpeed);
}
// Calculate the theta value between Stitches and the hook's calculated intercept point.
return Calculate.Hypotenuse(position, hPos);
}
This method is supposed to return the theta (angle) for the boss to throw his hook in order to intercept the player according to the player's movement vector at the time the hook is thrown.
For reference, the Calculate.Hypotenuse method just uses atan2 to calculate the angle between two points. Calculate.Distance gets the distance in pixels between two positions.
Does anyone have any suggestions on how to improve this algorithm? Or a better way to approach it?
Your question is confusing (as you also talk about a quadratic equation). If your game is a 2d platform game in which the boss throws a hook with a given velocity with a certain angle with the floor, then I foud your solution:
By playing with the kinematic equations, you find that
<math xmlns="http://www.w3.org/1998/Math/MathML">
<mrow class="MJX-TeXAtom-ORD">
<mo>θ</mo>
</mrow>
<mo>=</mo>
<mrow class="MJX-TeXAtom-ORD">
<mrow class="MJX-TeXAtom-ORD">
<mfrac>
<mrow>
<mi>arcsin</mi>
<mo><!-- --></mo>
<mo stretchy="false">(</mo>
<mi>d</mi>
<mi>g</mi>
<mo stretchy="false">)</mo>
</mrow>
<mrow class="MJX-TeXAtom-ORD">
<msup>
<mi>v</mi>
<mn>2</mn>
</msup>
</mrow>
</mfrac>
</mrow>
<mo>∗<!-- ∗ --></mo>
<mrow class="MJX-TeXAtom-ORD">
<mfrac>
<mn>1</mn>
<mrow class="MJX-TeXAtom-ORD">
<mn>2</mn>
</mrow>
</mfrac>
</mrow>
</mrow>
</math>
With d being the distance between the player and the boss, g being the gravitational constant and v being the initial velocity of the hook.
The reason that the hook keeps missing is that you always use a fixed timestep of 1 unit when integrating the player and hook's motion. This means that both objects' trajectories are a series of straight-line "jumps". 1 unit is far too large a timestep for there to be accurate results - if the speeds are high enough, there is no guarantee that the loop condition while(Calculate.Distance(position, hPos) < Calculate.Distance(position, pPos)) will even be hit.
The quadratic equation approach you mentioned was along the correct lines, but since you haven't understood the link, I will try to derive a similar method here.
Let's say the player's and hook's initial positions and velocities are p0, u and q0, v respectively (2D vectors). v's direction is the unknown desired quantity. Below is a diagram of the setup:
Applying the cosine rule:
Which root should be used, and does it always exist?
If the term inside the square root is negative, there is no real root for t - no solutions (the hook will never reach the player).
If both roots (or the single root) are negative, there is also no valid solution - the hook needs to be fired "backwards in time".
If only one root is positive, use it.
If both roots are positive, use the smaller one.
If the speeds are equal, i.e. v = u, then the solution is simply:
Again, reject if negative.
Once a value for t is known, the collision point and thus the velocity direction can be calculated:
Update: sample Java code:
private float findPlayerIntercept(Pair<Float> playerPos, Pair<Float> playerVel, int delta)
{
// calculate the speeds
float v = HOOK_THROW_SPEED * delta;
float u = Math.sqrt(playerVel.x * playerVel.x +
playerVel.y * playerVel.y);
// calculate square distance
float c = (position.x - playerPos.x) * (position.x - playerPos.x) +
(position.y - playerPos.y) * (position.y - playerPos.y);
// calculate first two quadratic coefficients
float a = v * v - u * u;
float b = playerVel.x * (position.x - playerPos.x) +
playerVel.y * (position.y - playerPos.y);
// collision time
float t = -1.0f; // invalid value
// if speeds are equal
if (Math.abs(a)) < EPSILON) // some small number, e.g. 1e-5f
t = c / (2.0f * b);
else {
// discriminant
b /= a;
float d = b * b + c / a;
// real roots exist
if (d > 0.0f) {
// if single root
if (Math.abs(d) < EPSILON)
t = b / a;
else {
// how many positive roots?
float e = Math.sqrt(d);
if (Math.abs(b) < e)
t = b + e;
else if (b > 0.0f)
t = b - e;
}
}
}
// check if a valid root has been found
if (t < 0.0f) {
// nope.
// throw an exception here?
// or otherwise change return value format
}
// compute components and return direction angle
float x = playerVel.x + (playerPos.x - position.x) / t;
float y = playerVel.y + (playerPos.y - position.y) / t;
return Math.atan2(y, x);
}
Related
I have to write a program in which I write a,b c,d (coefficients of equation 3 degree) and as a result I should get X1, X2, X3 (solutions of equation). I have to use Viete's formulas and BigDecimal for this, because my lecturer requires it from me.
I came to the conclusion that I have to solve the following system of equations:
x1+x2+x3=-b/a
x1*x2+x1*x3+x2*x3=c/a
x1*x2*x3=-d/a
I have no idea how I can do it in Java.
I tried to use the JAMA package, but I don't think I can use it to solve such a system of equations.
How can I do that?
If you want to find the roots of a cubic polynomial in Java you can do it easily using Newton-Raphson's method.
The algorithm -
1. Input: initial x, func(x), derivFunc(x)
Output: Root of Func()
2. Compute values of func(x) and derivFunc(x) for given initial x
3. Compute h: h = func(x) / derivFunc(x)
4. While h is greater than allowed error ε
- h = func(x) / derivFunc(x)
- x = x – h
Here is a demonstration for solving the cubic equation x^3-x^2+2
class XYZ {
static final double EPSILON = 0.001;
// An example function whose solution
// is determined using Bisection Method.
// The function is x^3 - x^2 + 2
static double func(double x)
{
return x * x * x - x * x + 2;
}
// Derivative of the above function
// which is 3*x^x - 2*x
static double derivFunc(double x)
{
return 3 * x * x - 2 * x;
}
// Function to find the root
static void newtonRaphson(double x)
{
double h = func(x) / derivFunc(x);
while (Math.abs(h) >= EPSILON)
{
h = func(x) / derivFunc(x);
// x(i+1) = x(i) - f(x) / f'(x)
x = x - h;
}
System.out.print("The value of the"
+ " root is : "
+ Math.round(x * 100.0) / 100.0);
}
// Driver code
public static void main (String[] args)
{
// Initial values assumed
double x0 = -20;
newtonRaphson(x0);
}
}
Output - The value of root is : -1.00
To do it your way you have to solve a system of non-linear equations which is harder but can be done using the Newton Raphson's Multivariate method. You might want to look it up. Also note that this is an approximate method and guesses the roots after you put an initial 'guess' of your own (in this case its -20)
The Newton (Raphson, Kantorovich) method for the Viete equations gives you the (Weierstrass-)Durand-Kerner method of simultaneous root approximation. However, in the completed method you will no longer see the Viete identities, they kind of cancel out. You will need complex numbers over the demanded real numbers data type.
If you go with the simple Newton method like in the other answer, then after computing the one real root you can split off the linear factor belonging to it via the Horner-Ruffini scheme and then solve the remaining quadratic equation directly. Then you only need to consider the possible complex nature of the roots in constructing the output strings, as the real and imaginary parts have easy direct formulas.
I'm attempting to write a method that determines the area of a polygon (complex or simple) on a sphere. I have a paper that was written by a few guys at the JPL that more or less give you the equations for these calculations.
The pdf file can be found here:
http://trs-new.jpl.nasa.gov/dspace/handle/2014/40409
The equation can be found on page 7, under "The Spherical Case - Approximation":
I also typed the equation in Word:
Spherical_Case_Equation
I need assistance with converting this equation into the standard form (I think that's the right terminology). I've already done something similar for the Planer Case:
private double calcArea(Point2D[] shape) {
int n = shape.length;
double sum = 0.0;
if (n < 3) return 0.0;
for (int i = 0; i < n-1 ; i++) {
sum += (shape[i].getX() * shape[i+1].getY()) - (shape[i+1].getX() * shape[i].getY());
}
System.out.println(0.5 * Math.abs(sum));
return 0.5 * Math.abs(sum);
}
I just need help with doing something similar for the spherical case. Any assistance will be greatly appreciated.
I haven't read the paper you referenced. The area of a spherical polygon is proportional to the angle excess.
Area = r²(Σ Aᵢ - (n - 2)π)
To compute the corner angles, you may start with the 3D coordinates of your points. So at corner i you have vertex p[i] = (x[i],y[i],z[i]) and adjacent vertices p[i-1] and p[i+1] (resp p[(i+n-1)%n] and p[(i+1)%n] to get this cyclically correct). Then the cross products
v₁ = p[i] × p[i-1]
v₂ = p[i] × p[i+1]
will be orthogonal to the planes spanned by the incident edges and the origin which is the center of the sphere. Noe the angle between two vectors in space is given by
Aᵢ = arccos(⟨v₁,v₂⟩ / (‖v₁‖ * ‖v₂‖))
where ⟨v₁,v₂⟩ denotes the dot product between these two vectors which is proportional to the cosine of the angle, and ‖v₁‖ denotes the length of the first vector, likewise ‖v₂‖ for the second.
So, I saw this on Hacker News the other day: http://web.mit.edu/tee/www/bertrand/problem.html
It basically says what's the probability that a random chord on a circle with radius of 1 has a length greater than the square root of 3.
Looking at it, it seems obvious that the answer is 1/3, but comments on HN have people who are smarter than me debating this. https://news.ycombinator.com/item?id=10000926
I didn't want to debate, but I did want to make sure I wasn't crazy. So I coded what I thought would prove it to be P = 1/3, but I end up getting P ~ .36. So, something's got to be wrong with my code.
Can I get a sanity check?
package com.jonas.betrand;
import java.awt.geom.Point2D;
import java.util.Random;
public class Paradox {
final static double ROOT_THREE = Math.sqrt(3);
public static void main(String[] args) {
int greater = 0;
int less = 0;
for (int i = 0; i < 1000000; i++) {
Point2D.Double a = getRandomPoint();
Point2D.Double b = getRandomPoint();
//pythagorean
if (Math.sqrt(Math.pow((a.x - b.x), 2) + Math.pow((a.y - b.y), 2)) > ROOT_THREE) {
greater++;
} else {
less++;
}
}
System.out.println("Probability Observerd: " + (double)greater/(greater+less));
}
public static Point2D.Double getRandomPoint() {
//get an x such that -1 < x < 1
double x = Math.random();
boolean xsign = new Random().nextBoolean();
if (!xsign) {
x *= -1;
}
//formula for a circle centered on origin with radius 1: x^2 + y^2 = 1
double y = Math.sqrt(1 - (Math.pow(x, 2)));
boolean ysign = new Random().nextBoolean();
if (!ysign) {
y *= -1;
}
Point2D.Double point = new Point2D.Double(x, y);
return point;
}
}
EDIT: Thanks to a bunch of people setting me straight, I found that my method of finding a random point wasn't indeed so random. Here is a fix for that function which returns about 1/3.
public static Point2D.Double getRandomPoint() {
//get an x such that -1 < x < 1
double x = Math.random();
Random r = new Random();
if (!r.nextBoolean()) {
x *= -1;
}
//circle centered on origin: x^2 + y^2 = r^2. r is 1.
double y = Math.sqrt(1 - (Math.pow(x, 2)));
if (!r.nextBoolean()) {
y *= -1;
}
if (r.nextBoolean()) {
return new Point2D.Double(x, y);
} else {
return new Point2D.Double(y, x);
}
}
I believe you need to assume one fixed point say at (0, 1) and then choose a random amount of rotation in [0, 2*pi] around the circle for the location of the second point of the chord.
Just for the hell of it I wrote your incorrect version in Swift (learn Swift!):
struct P {
let x, y: Double
init() {
x = (Double(arc4random()) / 0xFFFFFFFF) * 2 - 1
y = sqrt(1 - x * x) * (arc4random() % 2 == 0 ? 1 : -1)
}
func dist(other: P) -> Double {
return sqrt((x - other.x) * (x - other.x) + (y - other.y) * (y - other.y))
}
}
let root3 = sqrt(3.0)
let total = 100_000_000
var samples = 0
for var i = 0; i < total; i++ {
if P().dist(P()) > root3 {
samples++
}
}
println(Double(samples) / Double(total))
And the answer is indeed 0.36. As the comments have been explaining, a random X value is more likely to choose the "flattened area" around pi/2 and highly unlikely to choose the "vertically squeezed" area around 0 and pi.
It is easily fixed however in the constructor for P:
(Double(arc4random()) / 0xFFFFFFFF is fancy-speak for random floating point number in [0, 1))
let angle = Double(arc4random()) / 0xFFFFFFFF * M_PI * 2
x = cos(angle)
y = sin(angle)
// outputs 0.33334509
Bertrand's paradox is exactly that: a paradox. The answer can be argued to be 1/3 or 1/2 depending on how the problem is interpreted. It seems you took the random chord approach where one side of the line is fixed and then you draw a random chord to any part of the circle. Using this method, the chances of drawing a chord that is longer than sqrt(3) is indeed 1/3.
But if you use a different approach, I'll call it the random radius approach, you'll see that it can be 1/2! The random radius is this, you draw a radius in the circle, and then you take a random chord that this radius bisects. At this point, a random chord will be longer than sqrt(3) 1/2 of the time.
Lastly, the random midpoint method. Choose a random point in the circle, and then draw a chord with this random point as the midpoint of the chord. If this point falls within a concentric circle of radius 1/2, then the chord is shorter than sqrt(3). If it falls outside the concentric circle, it is longer than sqrt(3). A circle of radius 1/2 has 1/4 the area of a circle with radius 1, so the chance of a chord smaller than sqrt(3) is 1/4.
As for your code, I haven't had time to look at it yet, but hope this clarifies the paradox (which is just an incomplete question not actually a paradox) :D
I would argue that the Bertrand paradox is less a paradox and more a cautionary lesson in probability. It's really asking the question: What do you mean by random?
Bertrand argued that there are three natural but different methods for randomly choosing a chord, giving three distinct answers. But of course, there are other random methods, but these methods are arguably not the most natural ones (that is, not the first that come to mind). For example, we could randomly position the two chord endpoints in a non-uniform manner. Or we position the chord midpoint according to some non-uniform density, like a truncated bi-variate normal.
To simulate the three methods with a programming language, you need to be able to generate uniform random variables on the unit interval, which is what all standard (pseudo)-random number generators should do. For one of the methods/solutions (the random midpoint one), you then have to take the square root of one of the uniform random variables. You then multiple the random variables by a suitable factor (or rescale). Then for each simulation method (or solution), some geometry gives the expressions for the two endpoints.
For more details, I have written a post about this problem. I recommend the links and books I have cited at the end of that post, under the section Further reading. For example, see Section 1.3 in this new set of published lecture notes. The Bertrand paradox is also in The Pleasures of Probability by Isaac. It’s covered in a non-mathematical way in the book Paradoxes from A to Z by Clark.
I have also uploaded some simulation code in MATLAB, R and Python, which can be found here.
For example, in Python (with NumPy):
import numpy as np; #NumPy package for arrays, random number generation, etc
import matplotlib.pyplot as plt #for plotting
from matplotlib import collections as mc #for plotting line chords
###START Parameters START###
#Simulation disk dimensions
xx0=0; yy0=0; #center of disk
r=1; #disk radius
numbLines=10**2;#number of lines
###END Parameters END###
###START Simulate three solutions on a disk START###
#Solution A
thetaA1=2*np.pi*np.random.uniform(0,1,numbLines); #choose angular component uniformly
thetaA2=2*np.pi*np.random.uniform(0,1,numbLines); #choose angular component uniformly
#calculate chord endpoints
xxA1=xx0+r*np.cos(thetaA1);
yyA1=yy0+r*np.sin(thetaA1);
xxA2=xx0+r*np.cos(thetaA2);
yyA2=yy0+r*np.sin(thetaA2);
#calculate midpoints of chords
xxA0=(xxA1+xxA2)/2; yyA0=(yyA1+yyA2)/2;
#Solution B
thetaB=2*np.pi*np.random.uniform(0,1,numbLines); #choose angular component uniformly
pB=r*np.random.uniform(0,1,numbLines); #choose radial component uniformly
qB=np.sqrt(r**2-pB**2); #distance to circle edge (alonge line)
#calculate trig values
sin_thetaB=np.sin(thetaB);
cos_thetaB=np.cos(thetaB);
#calculate chord endpoints
xxB1=xx0+pB*cos_thetaB+qB*sin_thetaB;
yyB1=yy0+pB*sin_thetaB-qB*cos_thetaB;
xxB2=xx0+pB*cos_thetaB-qB*sin_thetaB;
yyB2=yy0+pB*sin_thetaB+qB*cos_thetaB;
#calculate midpoints of chords
xxB0=(xxB1+xxB2)/2; yyB0=(yyB1+yyB2)/2;
#Solution C
#choose a point uniformly in the disk
thetaC=2*np.pi*np.random.uniform(0,1,numbLines); #choose angular component uniformly
pC=r*np.sqrt(np.random.uniform(0,1,numbLines)); #choose radial component
qC=np.sqrt(r**2-pC**2); #distance to circle edge (alonge line)
#calculate trig values
sin_thetaC=np.sin(thetaC);
cos_thetaC=np.cos(thetaC);
#calculate chord endpoints
xxC1=xx0+pC*cos_thetaC+qC*sin_thetaC;
yyC1=yy0+pC*sin_thetaC-qC*cos_thetaC;
xxC2=xx0+pC*cos_thetaC-qC*sin_thetaC;
yyC2=yy0+pC*sin_thetaC+qC*cos_thetaC;
#calculate midpoints of chords
xxC0=(xxC1+xxC2)/2; yyC0=(yyC1+yyC2)/2;
###END Simulate three solutions on a disk END###
Quaternion can describe not only rotation, but also an orientation, i.e. rotation from initial (zero) position.
I was wishing to model smooth rotation from one orientation to another. I calculated start orientation startOrientation and end orientation endOrientation and was wishing to describe intermediate orientations as startOrientation*(1-argument) + endOrientation*argument while argument changes from 0 to 1.
The code for monkey engine update function is follows:
#Override
public void simpleUpdate(float tpf) {
if( endOrientation != null ) {
if( !started ) {
started = true;
}
else {
fraction += tpf * speed;
argument = (float) ((1 - Math.cos(fraction * Math.PI)) / 2);
orientation = startOrientation.mult(1-argument).add(endOrientation.mult(argument));
//orientation = startOrientation.mult(1-fraction).add(endOrientation.mult(fraction));
log.debug("tpf = {}, fraction = {}, argument = {}", tpf, fraction, argument);
//log.debug("orientation = {}", orientation);
rootNode.setLocalRotation(orientation);
if( fraction >= 1 ) {
rootNode.setLocalRotation(endOrientation);
log.debug("Stopped rotating");
startOrientation = endOrientation = null;
fraction = 0;
started = false;
}
}
}
}
The cosine formula was expected to model smooth accelerating at the beginning and decelerating at the end.
The code works but not as expected: the smooth rotation starts and finishes long before fraction and argument values reach 1 and I don't understand, why.
Why the orientation value reaches endOrientation so fast?
You have stated that in your case startOrientation was being modified. However; the following remains true
Interpolating between quaternions
The method slerp is included within the Quaternion class for this purpose: interpolating between two rotations.
Assuming we have two quaternions startOrientation and endOrientation and we want the point interpolation between them then we interpolate between then using the following code:
float interpolation=0.2f;
Quaternion result=new Quaternion();
result.slerp(startOrientation, endOrientation, interpolation);
Why your approach may be dangerous
Quaternions are somewhat complex internally and follow somewhat different mathematical rules to say vectors. You have called the multiply(float scalar) method on the quaternion. Internally this looks like this
public QuaternionD mult(float scalar) {
return new QuaternionD(scalar * x, scalar * y, scalar * z, scalar * w);
}
So it just does a simple multiplication of all the elements. This explicitly does not return a rotation that is scalar times the size. In fact such a quaternion no longer represents a valid rotation at all since its no longer a unit quaternion. If you called normalise on this quaterion it would immediately undo the scaling. I'm sure Quaternion#multiply(float scalar) has some use but I am yet to find them.
It is also the case that "adding" quaternions does not combine them. In fact you multiply them. So combining q1 then q2 then q3 would be achieved as follows:
Quaternion q0 = q1.mult(q2).mult(q3);
The cheat sheet is incredibly useful for this
Formula vs slerp comparison
In your case your formula for interpolation is nearly but not quite correct. This shows a graph of yaw for interpolation between 2 quaternions using both methods
I understand that the dot (or inner) product of two quaternions is the angle between the rotations (including the axis-rotation). This makes the dot product equal to the angle between two points on the quaternion hypersphere.
I can not, however, find how to actually compute the dot product.
Any help would be appreciated!
current code:
public static float dot(Quaternion left, Quaternion right){
float angle;
//compute
return angle;
}
Defined are Quaternion.w, Quaternion.x, Quaternion.y, and Quaternion.z.
Note: It can be assumed that the quaternions are normalised.
The dot product for quaternions is simply the standard Euclidean dot product in 4D:
dot = left.x * right.x + left.y * right.y + left.z * right.z + left.w * right.w
Then the angle your are looking for is the arccos of the dot product (note that the dot product is not the angle): acos(dot).
However, if you are looking for the relative rotation between two quaternions, say from q1 to q2, you should compute the relative quaternion q = q1^-1 * q2 and then find the rotation associated withq.
Just NOTE: acos(dot) is very not stable from numerical point of view.
as was said previos, q = q1^-1 * q2 and than angle = 2*atan2(q.vec.length(), q.w)
Should it be 2 x acos(dot) to get the angle between quaternions.
The "right way" to compute the angle between two quaternions
There is really no such thing as the angle between two quaternions, there is only the quaternion that takes one quaternion to another via multiplication. However, you can measure the total angle of rotation of that mapping transformation, by computing the difference between the two quaternions (e.g. qDiff = q1.mul(q2.inverse()), or your library might be able to compute this directly using a call like qDiff = q1.difference(q2)), and then measuring the angle about the axis of the quaternion (your quaternion library probably has a routine for this, e.g. ang = qDiff.angle()).
Note that you will probably need to fix the value, since measuring the angle about an axis doesn't necessarily give the rotation "the short way around", e.g.:
if (ang > Math.PI) {
ang -= 2.0 * Math.PI;
} else if (ang < -Math.PI) {
ang += 2.0 * Math.PI;
}
Measuring the similarity of two quaternions using the dot product
Update: See this answer instead.
I assume that in the original question, the intent of treating the quaternions as 4d vectors is to enable a simple method for measuring the similarity of two quaternions, while still keeping in mind that the quaternions represent rotations. (The actual rotation mapping from one quaternion to another is itself a quaternion, not a scalar.)
Several answers suggest using the acos of the dot product. (First thing to note: the quaternions must be unit quaternions for this to work.) However, the other answers don't take into account the "double cover issue": both q and -q represent the exact same rotation.
Both acos(q1 . q2) and acos(q1 . (-q2)) should return the same value, since q2 and -q2 represent the same rotation. However (with the exception of x == 0), acos(x) and acos(-x) do not return the same value. Therefore, on average (given random quaternions), acos(q1 . q2) will not give you what you expect half of the time, meaning that it will not give you a measure of the angle between q1 and q2, assuming that you care at all that q1 and q2 represent rotations. So even if you only plan to use the dot product or acos of the dot product as a similarity metric, to test how similar q1 and q2 are in terms of the effect they have as a rotation, the answer you get will be wrong half the time.
More specifically, if you are trying to simply treat quaternions as 4d vectors, and you compute ang = acos(q1 . q2), you will sometimes get the value of ang that you expect, and the rest of the time the value you actually wanted (taking into account the double cover issue) will be PI - acos(-q1 . q2). Which of these two values you get will randomly fluctuate between these values depending on exactly how q1 and q2 were computed!.
To solve this problem, you have to normalize the quaternions so that they are in the same "hemisphere" of the double cover space. There are several ways to do this, and to be honest I'm not even sure which of these is the "right" or optimal way. They do all produce different results from other methods in some cases. Any feedback on which of the three normalization forms above is the correct or optimal one would be greatly appreciated.
import java.util.Random;
import org.joml.Quaterniond;
import org.joml.Vector3d;
public class TestQuatNorm {
private static Random random = new Random(1);
private static Quaterniond randomQuaternion() {
return new Quaterniond(
random.nextDouble() * 2 - 1, random.nextDouble() * 2 - 1,
random.nextDouble() * 2 - 1, random.nextDouble() * 2 - 1)
.normalize();
}
public static double normalizedDot0(Quaterniond q1, Quaterniond q2) {
return Math.abs(q1.dot(q2));
}
public static double normalizedDot1(Quaterniond q1, Quaterniond q2) {
return
(q1.w >= 0.0 ? q1 : new Quaterniond(-q1.x, -q1.y, -q1.z, -q1.w))
.dot(
q2.w >= 0.0 ? q2 : new Quaterniond(-q2.x, -q2.y, -q2.z, -q2.w));
}
public static double normalizedDot2(Quaterniond q1, Quaterniond q2) {
Vector3d v1 = new Vector3d(q1.x, q1.y, q1.z);
Vector3d v2 = new Vector3d(q2.x, q2.y, q2.z);
double dot = v1.dot(v2);
Quaterniond q2n = dot >= 0.0 ? q2
: new Quaterniond(-q2.x, -q2.y, -q2.z, -q2.w);
return q1.dot(q2n);
}
public static double acos(double val) {
return Math.toDegrees(Math.acos(Math.max(-1.0, Math.min(1.0, val))));
}
public static void main(String[] args) {
for (int i = 0; i < 1000; i++) {
var q1 = randomQuaternion();
var q2 = randomQuaternion();
double dot = q1.dot(q2);
double dot0 = normalizedDot0(q1, q2);
double dot1 = normalizedDot1(q1, q2);
double dot2 = normalizedDot2(q1, q2);
System.out.println(acos(dot) + "\t" + acos(dot0) + "\t" + acos(dot1)
+ "\t" + acos(dot2));
}
}
}
Also note that:
acos is known to not be very numerically accurate (given some worst-case inputs, up to half of the least significant digits can be wrong);
the implementation of acos is exceptionally slow in the JDK standard libraries;
acos returns NaN if its parameter is even slightly outside [-1,1], which is a common occurrence for dot products of even unit quaternions -- so you need to bound the value of the dot product to that range before calling acos. See this line in the code above:
return Math.toDegrees(Math.acos(Math.max(-1.0, Math.min(1.0, val))));
According to this cheatsheet Eq. (42), there is a more robust and accurate way of computing the angle between two vectors that replaces acos with atan2 (although note that this does not solve the double cover problem either, so you will need to use one of the above normalization forms before applying the following):
ang(q1, q2) = 2 * atan2(|q1 - q2|, |q1 + q2|)
I admit though that I don't understand this formulation, since quaternion subtraction and addition has no geometrical meaning.