I am calculating the quaternion rotation between 2 quaternions.
I know that Q1 = r x Q2, so I thought that r = Q1 x Q2*
(asterisk means conjugate)
This however seems to give me the opposite of the angle I need.
ex. If Q1 and Q2 are Pi/2 off, I will get a quaternion of -Pi/2. How can I get the correct angle I need?
Example:
I am looking 45 degrees left from forward. I am moving world forward. Relative to look, I am traveling 45 degrees to the right. I need r to be a rotation of 45 degrees right in this situation.
Thank you in advance
The description of the problem wasn't quite clear to me. I think that its probably best to derive the formula that you have found. Hopefully, this should allow you to understand (and visualize) why you might be getting the result that you are.
Given two quaternions, Q1 and Q2, the relative quaternion rotation from Q1 to Q2 is:
Quaternion = relativeRotation = Q1^(-1) * Q2
Where Q1^(-1) denotes the inverse of Q1. You can think of this as applying the opposite rotation of Q1 to Q2. The result will be the relative rotation between the two. The inverse of a quaternion may be calculated by:
Q^(-1) = (Q*) / ||Q||^2
Where Q* denotes the conjugate of Q. I assume that you already know how to find the conjugate of a quaternion. Now, if Q is a unit quaternion, then both the norm and the square of the norm of Q is 1.0. Hence, we have:
Q^(-1) = Q*
This simplifies our formula to:
Quaternion relativeRotation = (Q1*) * Q2
Note that only unit quaternions represent rotations. However, since the Hamilton product of two unit quaternions is itself a unit quaternion, we don't need to normalize the result.
Now, from this relative quaternion, you can determine the angle (in radians):
float angle = 2.0f * acos(relativeRotation.w)
Here, w is the scalar component of the quaternion and acos is the arccosine trigonometric function. This angle is about the axis:
Vector3 axis = (relativeRotation.x, relativeRotation.y, relativeRotation.z)
Now, having found both the angle and the axis, you should be able to visualize the rotation.
Finally, as a side note, the Hamilton product (*) is not commutative. This means that (Q1*) * Q2 does not necessarily equal Q2 * (Q1*). Following the same derivation as above, the relative unit quaternion rotation from Q2 to Q1 can be calculated by:
relativeRotation = (Q2*) * Q1
Related
The first IMU (called S1) is placed on the shoulder and works as a reference; the other (S2) is placed on the arm. They provide the quaternion of their rotation relative to the absolute reference (magnetic north and gravity vector). The simple idea is that I need to show the Yaw, Pitch and Roll differences between these two (e.g. an ideal abduction/adduction movement should have pitch contributions only). I started using quaternions, by calculating the rotation between the two (conj(q1) * q2) and then converting it to YPR angles by using:
# Rotation quaternion q1 = (w,x,y,z)
# unit = sum of squared elements (sqx = x^2, etc.)
yaw = math.atan2(2 * q1[2] * q1[0] - 2 * q1[1] * q1[3], sqx - sqy - sqz + sqw)
pitch = math.asin(2 * (q1[1] * q1[2] + q1[3] * q1[0]) / sqx + sqy + sqz + sqw)
roll = math.atan2(2 * q1[1] * q1[0] - 2 * q1[2] * q1[3], -sqx - sqy + sqz + sqw)
but this doesn't work in my case, since pitch and roll are not consistent in different arm positions. E.g. if relative Yaw is 90 deg, pitch and roll angles are interchanged. E.g. if I apply a pitch rotation to S2, it appears to be a roll rotation (since the rotation is on the y-axis for the reference sensor S1).
How can I avoid this?
Should I simply convert both quaternion to YPR angles and then calculate the difference of each pair (without using the difference between quaternions)? Maybe the "rotation" approach is not correct, since I don't need the inverse transformation but only the actual rotation for each axis?
What I have tried already: d = |v||PQ|sin("Theta")
Now, I need to determine what theta is, so I set up a position on a makeshift graph, the graph I made was on the xy plane only as the z plane complicates things needlessly for finding theta. So, I ended up with an acute angle, and if the angle is acute, then I have to find theta which according to dot product facts is greater than 0.
I do not have access to theta, so I used the same princples from cross dots. u * v = |u||v|cos("theta") but in this case, u and v are PQ and v. A vector is a vector, right?
so now I have theta = acos((v*PQ)/(|v||PQ))
with that I get (4sqrt(10))/15 = 32.5125173162 in degrees, so the angle is 32.5125173162 degrees.
So, now that I have theta, I plug it into my distance formula |v||PQ|sin(32.5125173162)
3*sqrt(10)*sin(32.5125173162) = 5.0990195136
or for the sake of simplicity, 5.1
I however want to know if this question is correct.
If it is NOT correct, what can I do to correct it? At what points did I use incorrect information?
This is not a question with a definitive answer in the back of the book, its a question on the side of a page that said: "try this!"
There are a couple of problems with this question.
From the context it looks like you mean for both v and PQ to be vectors. The "distance" between two vectors is an awkward (not well defined) question because vectors are not position bound.
You are using the cross product formula and I have no idea why:
|AxB| = |A||B|Sin(theta)
I think what you are actually trying to do is calculate the distance between the terminal points of the vectors, (2, 1, 2) and (1, 0, 3). Just use the Pythagorean Theorem (extended to 3D) for this.
d = sqrt( (x1 - x2)^2 + (y1 - y2)^2 + (z1 - z2)^2 )
d = sqrt( (2 - 1)^2 + (1 - 2)^2 + (2 - 3)^2 )
d = sqrt( 1^2 + (-1)^2 + (-1)^2 )
d = sqrt(3)
Edit:
If what you need really is the magnitude of the cross product, |AxB| then just find the cross product (using the determinant) and then calculate the magnitude of the result. There is no need for the formula you were using.
I have two vector pairs (before and after rotation).
before rotation:
[x1,y1,z1]
[x2,y2,z2]
after rotation:
[x1',y1',z1']
[x2',y2',z2']
How to create a quaternion representing this rotation?
In most cases there is no rotation which transforms 2 vectors into 2 other vectors. Here is a simple way to visualize why: a rotation does not change the angle between vectors. If the angle between the 2 vectors before the rotation is different from the angle between the 2 vectors after the rotation, then there is no rotation which meets your criteria.
This said there may be an optimal quaternion with an acceptable error which "almost" rotates your 2 vector pairs. There are a number of algorithms which vary in speed and precision to find such a quaternion. I wrote a fast C++ algorithm for an Arduino application where the speed is critical but the precision is less important.
http://robokitchen.tumblr.com/post/67060392720/finding-a-rotation-quaternion-from-two-pairs-of-vectors
Before rotation: u0, v0. After rotation: u2, v2.
Quaternion q2 = Quaternion::fromTwoVectors(u0, u2);
Vector v1 = v2.rotate(q2.conjugate());
Vector v0_proj = v0.projectPlane(u0);
Vector v1_proj = v1.projectPlane(u0);
Quaternion q1 = Quaternion::fromTwoVectors(v0_proj, v1_proj);
return (q2 * q1).normalized();
If this does not meet the requirements of your own application try to google Wabha's problem.
Well, first you can find the rotation axis using vector-multiplication (cross-multiplication):
axis = v1 x v2;
Then you can compute the rotation angle:
sinA = |axis| / |v1|*|v2|
cosA = v1 . v2 / |v1|*|v2|
Here | | - is vector length operation, and . - is dot-multiplication
And finally, your quaternion is:
Q(w,x,y,z) = (cosA, axis.x * sinA, axis.y * sinA, axis.z * sinA)
I translated marcv81's very helpful blog post into Three.js:
const rotateVectorsSimultaneously = (u0, v0, u2, v2) => {
const q2 = new THREE.Quaternion().setFromUnitVectors(u0, u2);
const v1 = v2.clone().applyQuaternion(q2.clone().conjugate());
const v0_proj = v0.projectOnPlane(u0);
const v1_proj = v1.projectOnPlane(u0);
let angleInPlane = v0_proj.angleTo(v1_proj);
if (v1_proj.dot(new THREE.Vector3().crossVectors(u0, v0)) < 0) {
angleInPlane *= -1;
}
const q1 = new THREE.Quaternion().setFromAxisAngle(u0, angleInPlane);
const q = new THREE.Quaternion().multiplyQuaternions(q2, q1);
return q;
};
Because angleTo always returns a positive value, I manually flip the sign of the angle depending on which side of the u0-v0 plane v1 is on.
A mature solution to this problem is called Triad. Triad is one of the earliest and simplest solutions to the spacecraft attitude determination problem and is extremely efficient computationally.
With Triad, the idea is to replace your paired set of two vectors, with a paired set of three vectors, where the extra vector is generated with a cross-product. By normalizing the vectors, you can solve for a rotation matrix without a matrix inverse or an SVD (as is needed in more general instances of the problem -- see Wahba's Problem)
For full algorithm, see: https://en.wikipedia.org/wiki/Triad_method
You can then convert the solved rotation matrix from Triad to a rotation quaternion:
qw = √(1 + m00 + m11 + m22) /2
qx = (m21 - m12)/( 4 *qw)
qy = (m02 - m20)/( 4 *qw)
qz = (m10 - m01)/( 4 *qw)
In general to make the conversion to quaternion robust, you should consider looking at the matrix trace as discussed here: http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/
Finally, consider an alternative to Triad that directly computes the optimal quaternion called QUEST.
It is fine to find the quaternion from v1 to v2.
The final q = (cos A/2, sin A/2 * axis), where the A is the angle between v1 and v2, axis is the normed axis.
Multiply both side by 2 * cos A/2,
Then we have
2 * cos A/2 *q = (1+cos A, sin A * axis)
(where cos A = dot(v1, v2)/|v1|/|v2| and
axis = cross(v1, v2).normalize() = cross(v1, v2)/|v1|/|v2|/sin A.)
Then 2 * cos A/2 *q = (1+dot(v1, v2)/|v1|/|v2|, cross(v1, v2)/|v1|/|v2|)
Finally q = (1+dot(v1, v2)/|v1|/|v2|, cross(v1, v2)/|v1|/|v2|).normalize()
Ok so let me try to explain this the best way that i can.
I have two points plotted 'A' and 'B' and I am trying to plot a third point 'C' so that it is past point 'B' but along the same slope. I have the angle of the line and I would post some code but I really have no idea where to begin.
any help would be awesome!
Just a little code that i do have
CGPoint vector = ccpSub(touchedPoint, fixedPoint);
CGFloat rotateAngle = -ccpToAngle(vector);
Assuming that by this you mean you need a 3rd point C added such that all the points are colinear, all you need to do is calculate the vector that takes you from A to B, and then generate a new point by adding multiples of this vector to the point B. Choose the multiple based on the distance you want C to be from B.
As an example, say A = (2,2), B = (4,3). Then the vector from A to B is given by (2,1).
All you need to do then is work out how far your new point is from B and add a multiple K*(2,1) to your point B where K is chosen to meet the requirements of your distance
I am assuming you are in 2D, but the same method would apply in higher dimensions
My math is rusty, but the linear equation is generally represented as y=m*x+b, where m is the slope, and b is the y-intercept. You can get m, the slope, by taking the difference of the y values and dividing that by the difference in the x values, e.g., if A = (2,2) and B = (4,3), then m is (3-2)/(4-2) or 0.5. Then, you can solve the linear equation for b, the y-intercept, i.e. b=y-m*x and then plug in either of the data points, e.g. if we plug in the x and y values for point A, you get b = 2 - 0.5 * 2 = 1. Now knowing the slope, m (0.5 in this example), and the y-intercept, b (1 in this example), you can calculate the y for any x value using y=m*x+b, in this case y=0.5*x+1.
So, if touchedPoint and fixedPoint are CGPoint, you can calculate the slope and y-intercept from fixedPoint and touchedPoint like so:
double m = (fixedPoint.y - touchedPoint.y) / (fixedPoint.x - touchedPoint.x);
double b = fixedPoint.y - m * fixedPoint.x;
Now, you don't say how you want to determine where this third point, C, is. But if you, for example, knew the x coordinate for this new point C, you can calculate the y coordinate that falls on the same line as follows:
CGPoint pointC;
pointC.x = 400; // or set this to whatever you want
pointC.y = m * pointC.x + b;
I am processing a series of points which all have the same Y value, but different X values. I go through the points by incrementing X by one. For example, I might have Y = 50 and X is the integers from -30 to 30. Part of my algorithm involves finding the distance to the origin from each point and then doing further processing.
After profiling, I've found that the sqrt call in the distance calculation is taking a significant amount of my time. Is there an iterative way to calculate the distance?
In other words:
I want to efficiently calculate: r[n] = sqrt(x[n]*x[n] + y*y)). I can save information from the previous iteration. Each iteration changes by incrementing x, so x[n] = x[n-1] + 1. I can not use sqrt or trig functions because they are too slow except at the beginning of each scanline.
I can use approximations as long as they are good enough (less than 0.l% error) and the errors introduced are smooth (I can't bin to a pre-calculated table of approximations).
Additional information:
x and y are always integers between -150 and 150
I'm going to try a couple ideas out tomorrow and mark the best answer based on which is fastest.
Results
I did some timings
Distance formula: 16 ms / iteration
Pete's interperlating solution: 8 ms / iteration
wrang-wrang pre-calculation solution: 8ms / iteration
I was hoping the test would decide between the two, because I like both answers. I'm going to go with Pete's because it uses less memory.
Just to get a feel for it, for your range y = 50, x = 0 gives r = 50 and y = 50, x = +/- 30 gives r ~= 58.3. You want an approximation good for +/- 0.1%, or +/- 0.05 absolute. That's a lot lower accuracy than most library sqrts do.
Two approximate approaches - you calculate r based on interpolating from the previous value, or use a few terms of a suitable series.
Interpolating from previous r
r = ( x2 + y2 ) 1/2
dr/dx = 1/2 . 2x . ( x2 + y2 ) -1/2 = x/r
double r = 50;
for ( int x = 0; x <= 30; ++x ) {
double r_true = Math.sqrt ( 50*50 + x*x );
System.out.printf ( "x: %d r_true: %f r_approx: %f error: %f%%\n", x, r, r_true, 100 * Math.abs ( r_true - r ) / r );
r = r + ( x + 0.5 ) / r;
}
Gives:
x: 0 r_true: 50.000000 r_approx: 50.000000 error: 0.000000%
x: 1 r_true: 50.010000 r_approx: 50.009999 error: 0.000002%
....
x: 29 r_true: 57.825065 r_approx: 57.801384 error: 0.040953%
x: 30 r_true: 58.335225 r_approx: 58.309519 error: 0.044065%
which seems to meet the requirement of 0.1% error, so I didn't bother coding the next one, as it would require quite a bit more calculation steps.
Truncated Series
The taylor series for sqrt ( 1 + x ) for x near zero is
sqrt ( 1 + x ) = 1 + 1/2 x - 1/8 x2 ... + ( - 1 / 2 )n+1 xn
Using r = y sqrt ( 1 + (x/y)2 ) then you're looking for a term t = ( - 1 / 2 )n+1 0.36n with magnitude less that a 0.001, log ( 0.002 ) > n log ( 0.18 ) or n > 3.6, so taking terms to x^4 should be Ok.
Y=10000
Y2=Y*Y
for x=0..Y2 do
D[x]=sqrt(Y2+x*x)
norm(x,y)=
if (y==0) x
else if (x>y) norm(y,x)
else {
s=Y/y
D[round(x*s)]/s
}
If your coordinates are smooth, then the idea can be extended with linear interpolation. For more precision, increase Y.
The idea is that s*(x,y) is on the line y=Y, which you've precomputed distances for. Get the distance, then divide it by s.
I assume you really do need the distance and not its square.
You may also be able to find a general sqrt implementation that sacrifices some accuracy for speed, but I have a hard time imagining that beating what the FPU can do.
By linear interpolation, I mean to change D[round(x)] to:
f=floor(x)
a=x-f
D[f]*(1-a)+D[f+1]*a
This doesn't really answer your question, but may help...
The first questions I would ask would be:
"do I need the sqrt at all?".
"If not, how can I reduce the number of sqrts?"
then yours: "Can I replace the remaining sqrts with a clever calculation?"
So I'd start with:
Do you need the exact radius, or would radius-squared be acceptable? There are fast approximatiosn to sqrt, but probably not accurate enough for your spec.
Can you process the image using mirrored quadrants or eighths? By processing all pixels at the same radius value in a batch, you can reduce the number of calculations by 8x.
Can you precalculate the radius values? You only need a table that is a quarter (or possibly an eighth) of the size of the image you are processing, and the table would only need to be precalculated once and then re-used for many runs of the algorithm.
So clever maths may not be the fastest solution.
Well there's always trying optimize your sqrt, the fastest one I've seen is the old carmack quake 3 sqrt:
http://betterexplained.com/articles/understanding-quakes-fast-inverse-square-root/
That said, since sqrt is non-linear, you're not going to be able to do simple linear interpolation along your line to get your result. The best idea is to use a table lookup since that will give you blazing fast access to the data. And, since you appear to be iterating by whole integers, a table lookup should be exceedingly accurate.
Well, you can mirror around x=0 to start with (you need only compute n>=0, and the dupe those results to corresponding n<0). After that, I'd take a look at using the derivative on sqrt(a^2+b^2) (or the corresponding sin) to take advantage of the constant dx.
If that's not accurate enough, may I point out that this is a pretty good job for SIMD, which will provide you with a reciprocal square root op on both SSE and VMX (and shader model 2).
This is sort of related to a HAKMEM item:
ITEM 149 (Minsky): CIRCLE ALGORITHM
Here is an elegant way to draw almost
circles on a point-plotting display:
NEW X = OLD X - epsilon * OLD Y
NEW Y = OLD Y + epsilon * NEW(!) X
This makes a very round ellipse
centered at the origin with its size
determined by the initial point.
epsilon determines the angular
velocity of the circulating point, and
slightly affects the eccentricity. If
epsilon is a power of 2, then we don't
even need multiplication, let alone
square roots, sines, and cosines! The
"circle" will be perfectly stable
because the points soon become
periodic.
The circle algorithm was invented by
mistake when I tried to save one
register in a display hack! Ben Gurley
had an amazing display hack using only
about six or seven instructions, and
it was a great wonder. But it was
basically line-oriented. It occurred
to me that it would be exciting to
have curves, and I was trying to get a
curve display hack with minimal
instructions.