Applying fast inverse to concatenated 4x4 affine transforms? - math

Is it possible to apply the fast inverse of a matrix to a concatenation of pure rotation and translation matrices, eg M = T2*R1*T1*R1?
If I have a rotation and translation stored in a 4x4 homogeneous column order matrix I can say:
M1 = [ R1 t1 ] given by [ 1 t1 ] * [ R1 0 ]
[ 0 1 ] [ 0 1 ] [ 0 1 ]
and
inv(M1) = [inv(R1) inv(R1)*-t1 ] given by [ 1 -t1 ] * [ inv(R1) 0 ]
[ 0 1 ] [ 1 1 ] * [ 0 1 ]
and since R1 is rotation only we know inv(R1) = transpose(R1) so we can simply say:
inv(M1) = [transp(R1) transp(R1)*-t1 ]
[ 0 1 ]
and now given some other similar rotation and translation matrix M2, if we say the concatenation of the the two in the form MFinal = M2 * M1 = T2*R1*T1*R1
can we say that
inv(MFinal) = [transp(MFinalRot) transp(MFinalRot)*-tfinal ]
[ 0 1 ]
where MFinalRot is the rotation part of the 4x4 matrix?
Additionally what if the order were more arbitrary for example MFinal2 = R3*T3 * T2*R2*T1*R1 , but still only individually rotations and translations?

Yes, if your 4x4 matrix is the concatenation of pure rotation and translation matrices, you should be able to compute a fast inverse as:
fast_inverse( [R1 t1] ) = [transpose(R1) transpose(R1)*(-t1)]
[0 1] [ 0 1 ]
This is because the 3x3 rotation matrix (R1 in your code), will be a product of the input rotation matrices only, so it should itself be a rotation matrix, and its transpose should be its inverse.
If any of your concatenated matrices are scaling matrices, or if the bottom row is not [0 0 0 1], then this is not true any more.
Also note that: in practice, if you multiply enough matrices together, floating point error may cause them to "drift" some, so that they may not be as close to a proper rotation matrix as a freshly-generated one. Depending on how you use it, this may not be a problem -- but if it is, you can "re-orthonormalize" it, as below:
orth(Vec3 a, Vec3 b): // return value orthogonal to b
return (a - (dot(a,b)/dot(b,b)) * b)
re_orthonormalize(Mat3x3 Rin):
Vec3 x = Rin.x;
Vec3 y = orth(Rin.y, x);
Vec3 z = orth(orth(Rin.z, x), y);
return Mat3x3(normalize(x),normalize(y),normalize(z))
As long as your input isn't too far off, this should give you a proper rotation matrix.
To see how the re_orthonormalize code works, first take the dot product of the orth output with its b input. Because the dot product is linear, we have:
dot(a - (dot(a,b)/dot(b,b)*b, b)
== dot(a,b) - (dot(a,b)/dot(b,b)) * dot(b,b)
== dot(a,b) - dot(a,b)
== 0
So, if a and b are already mostly orthogonal, ortho(a,b) adds a small amount of b to make sure the dot product really is 0.
That means in re_orthonormalize, y is exactly orthogonal to x. The tricky bit is making sure that z is orthogonal to both x and y. This only works because we have already made sure x is exactly orthogonal to y, so adding a little bit of y doesn't stop orth(Rin.z, x) from being orthogonal to x.

The inverse of the product (P=AB) of two square matrices is in general Inv(B)*Inv(A). Rotations and translations will commute. In general you have to unwind the operations in the reverse of the order in which they were applied.
In this case though, R1*T1*R2*T2=R1*R2*T1*T2 and you can then compute the inverse of the concatenation as the inverse of the composition of the individual rotations and translations.
So yes, this is sound for pure rotations and translations.

Related

Can anyone write a prog that change the sum(1 to n) to n*(n+1)/2 automatic?

with the rec sum:
let rec sum a=if a==0 then 0 else a+sum(a-1)
if the compiler use the tail recursive optimization,it may create a variable "sum" to iteration(when I use the "ocamlc -dlambda",the recursive still there.when I use "ocamlc -dinstr" got the assemably code,I can't read it now)
but on the book《Design Concepts of programming languages》,page 287,it can change the function to this(the key line):n*(n+1)/2
"You should convince yourself that the least fixed point of this
function is the computation csum that returns a summation procedure that,returns n*(n+1)/2 if its argument is a nonnegative integer in"
I can't understand it,the prog not Gauss!I think it can't chang the "rec sum" to n*(n+1)/2 automatic!only man can do it,right?
So how this book write here means?Is anyone know?Thanks!
I believe your book is merely making a small point about equivalence of pure functions. Nevertheless, optimising away a loop that only contains affine operations is relatively easy.
Equivalence of pure functions
I haven't read that book, but from the paragraph you quote, I think the book merely makes a point about pure functions. Since sum is a pure function, i.e. a function without side-effect, then in a sense,
let rec sum n =
if n = 0 then 0
else n + sum (n - 1)
is equivalent to
let sum n =
n * (n + 1) / 2
But of course "equivalent" here ignores the time and space complexity, and unless the compiler has some sort of hardcoding for common functions to optimise, I'd be extremely surprised if it optimised sum like that.
Also note that the two above functions are only equivalent so far as they are only called on a nonnegative argument. The recursive version will loop infinitely (and provoke a stack overflow) if n is negative; the direct formula version will always return a result, although that result will be nonsensical if n is negative.
Optimising loops that only contain affine operations
Nevertheless, writing a compiler that would perform such optimisations is not complete science-fiction. At the end of this answer you will find links to two blogposts which you might be interested in. In this answer I will summarise how the method described in those blog posts can be applied to your problem.
First let's rewrite function sum as a loop in pseudo-code:
function sum(n):
s := 0
i := 1
repeat n:
s += i
i += 1
return s
This kind of rewriting is similar to what happens when sum is transformed into a tail-recursive function.
Now if you consider the vector v = [s, i, 1], then the affine operations s += i and i += 1 can be described as multiplying v by a matrix:
s += i
[[ 1, 0, 0 ], # matrix Msi
[ 1, 1, 0 ],
[ 0, 0, 1 ]]
i += 1
[[ 1, 0, 0 ], # matrix Mi1
[ 0, 1, 0 ],
[ 0, 1, 1 ]]
s += i, i += 1
[[ 1, 0, 0 ], # M = Msi * Mi1
[ 1, 1, 0 ],
[ 0, 1, 1 ]]
This affine operation is wrapped in a "repeat n" loop. So we have to multiply v by this matrix M, n times. But matrix multiplication is associative; so instead of doing n multiplications by matrix M, we can raise matrix M to its nth power, and then multiply v by the resulting matrix M**n.
As it turns out:
[[1, 0, 0], [[ 1, 0, 0],
[1, 1, 0], to the nth = [ n, 1, 0],
[0, 1, 1]] [n*(n - 1)/2, n, 1]]
which represents the affine operation:
s = s + n * i + n * (n - 1) / 2
i = i + n
Starting from s, i = 0, 1, this gives us s = n * (n+1) / 2 as expected.
More reading:
Using the Quick Raise of Matrices to a Power to Write a Very Fast Interpreter of a Simple Programming Language;
Automatic Algorithms Optimization via Fast Matrix Exponentiation.

To understand how Gram-Schmidt Process is translated into this piece of code as the implementation

Trying to understand Gram-Schmidt process from this explanation:
http://mlwiki.org/index.php/Gram-Schmidt_Process
The steps of the calculation make sense to me. However the Python implementation included in the same article doesn't seem to be aligned.
def normalize(v):
return v / np.sqrt(v.dot(v))
n = len(A)
A[:, 0] = normalize(A[:, 0])
for i in range(1, n):
Ai = A[:, i]
for j in range(0, i):
Aj = A[:, j]
t = Ai.dot(Aj)
Ai = Ai - t * Aj
A[:, i] = normalize(Ai)
From above code, we see it does dot product for V1 and b, however the (V1,V1) part is not done as the denominator (refer to below equation). I wonder how below equation is translated into code residing in the for loop?
This is what the code does exactly
Basically it normalize the previous vector (column in A) and project the current one to it and to be subtracted by the current one.
Normalization happens with every vector for neat calculation.
The V2 equation above doesn't normalize the previous vector hence the difference.
Try this vectorized implementation.
Also I would suggest to go through David C lay book for theory.
def replace_zero(array):
for i in range(len(array)) :
if array[i] == 0 :
array[i] = 1
return array
def gram_schmidt(self,A, norm=True, row_vect=False):
"""Orthonormalizes vectors by gram-schmidt process
Parameters
-----------
A : ndarray,
Matrix having vectors in its columns
norm : bool,
Do you need Normalized vectors?
row_vect: bool,
Does Matrix A has vectors in its rows?
Returns
-------
G : ndarray,
Matrix of orthogonal vectors
Gram-Schmidt Process
--------------------
The Gram–Schmidt process is a simple algorithm for
producing an orthogonal or orthonormal basis for any
nonzero subspace of Rn.
Given a basis {x1,....,xp} for a nonzero subspace W of Rn,
define
v1 = x1
v2 = x2 - (x2.v1/v1.v1) * v1
v3 = x3 - (x3.v1/v1.v1) * v1 - (x3.v2/v2.v2) * v2
.
.
.
vp = xp - (xp.v1/v1.v1) * v1 - (xp.v2/v2.v2) * v2 - .......
.... - (xp.v(p-1) / v(p-1).v(p-1) ) * v(p-1)
Then {v1,.....,vp} is an orthogonal basis for W .
In addition,
Span {v1,.....,vp} = Span {x1,.....,xp} for 1 <= k <= p
References
----------
Linear Algebra and Its Applications - By David.C.Lay
"""
if row_vect :
# if true, transpose it to make column vector matrix
A = A.T
no_of_vectors = A.shape[1]
G = A[:,0:1].copy() # copy the first vector in matrix
# 0:1 is done to to be consistent with dimensions - [[1,2,3]]
# iterate from 2nd vector to number of vectors
for i in range(1,no_of_vectors):
# calculates weights(coefficents) for every vector in G
numerator = A[:,i].dot(G)
denominator = np.diag(np.dot(G.T,G)) #to get elements in diagonal
weights = np.squeeze(numerator/denominator)
# projected vector onto subspace G
projected_vector = np.sum(weights * G,
axis=1,
keepdims=True)
# orthogonal vector to subspace G
orthogonalized_vector = A[:,i:i+1] - projected_vector
# now add the orthogonal vector to our set
G = np.hstack((G,orthogonalized_vector))
if norm :
# to get orthoNormal vectors (unit orthogonal vectors)
# replace zero to 1 to deal with division by 0 if matrix has 0 vector
# or normazalization value comes out to be zero
G = G/self.replace_zero(np.linalg.norm(G,axis=0))
if row_vect:
return G.T
return G
G = np.array([[1,0,0],[1,1,0],[1,1,1],[1,1,1]])
gram_schmidt(G)
>
array([[ 0.5 , -0.8660254 , 0. ],
[ 0.5 , 0.28867513, -0.81649658],
[ 0.5 , 0.28867513, 0.40824829],
[ 0.5 , 0.28867513, 0.40824829]])

Netlogo Flocking Model Code Explanation

I'm investigating the flocking model of netlogo. It has the following code that is strange. What does this code do mathematically?
If I would see this in mathematical notations I would understand this. I suppose this is how trigonometry gets implemented in netlogo ?
to heading
turn-towards average-heading max-align-turn
end
to-report average-heading
let x-component sum [dx] of flock
let y-component sum [dy] of flock
ifelse x-component = 0 and y-component = 0
[ report heading ]
[ report atan x-component y-component ]
end
to turn-towards [new-heading max-turn]
turn-at-most (subtract-headings new-heading heading) max-turn
end
to turn-at-most [turn max-turn]
ifelse abs turn > max-turn
[ ifelse turn > 0
[ rt max-turn ]
[ lt max-turn ] ]
[ rt turn ]
end
to-report average-heading
let x-component sum [dx] of flock
let y-component sum [dy] of flock
ifelse x-component = 0 and y-component = 0
[ report heading ]
[ report atan x-component y-component ]
end
dy and dy are the sine and cosine of the turtles headings so what we are looking at is
The procedure reports
The arctangent of the sum of the sines of the turtle-headings , sum of the cosines of the turtle-headings.
it is so the mean heading of the set of angles obviously if we just up the headings and divide by the number of turtles we end up with a lot of problems.

pixel coordinates on diamond

I got an image with a couple of diamond put side by side like on the image below
The only coordinates I know on the image are the top corners (green text).
When I click on the image I get the coordinates of that point, but I'm not able to get which diamond I'm on.
For example I click on the red dot, how do I know that x:260, y:179 = the top diamond ?
And the blue belongs to the left ? etc...
Thank you very much for your help.
EDIT:
I finally used Canvas, but I think SVG would have worked as well for what I needed to do.
I see two possible approaches: direct check whether a point is inside a diamond and using affine transformations. I will describe both.
Direct point position check
To determine whether a point is inside a diamond you have to check its deviation from the middle point of a diamond. You have to put the X and Y deviations in proportion with the X and Y extents of the diamond, you will get two factors. For all points inside the diamond the sum of the modulo values for these factors is smaller or equal 1. In code this looks like this:
var dx = Math.abs(coords[0] - middle[0]);
var dy = Math.abs(coords[1] - middle[1]);
if (dx / size[0] + dy / size[1] <= 1)
alert("Inside diamond");
else
alert("Outside diamond");
So all you have to do now is determining the middle point for each diamond (size is the same in all cases) and checking whether the point you are testing is located inside them.
Working example: http://jsfiddle.net/z98hr/
Affine transformations
Using affine transformations you can change the corner coordinates of your top diamond into (0,0), (1,0), (0,1) and (1,1). If you then apply the same transformation to the point you need to test, determining which diamond it belongs to becomes trivial.
First you will need a translation vector to move the (225,2) point into the origin of coordinates. Let's say that you have four coordinates determining your top diamond (left and right coordinate, top and bottom coordinate):
var topDiamond = [[113, 2], [337, 227]];
Then the translation vector to move the top point of the diamond to the zero coordinate would be:
var translationVector = [-(topDiamond[0][0] + topDiamond[1][0]) / 2,
-topDiamond[0][1]];
You can apply it to the original coordinates like this:
function add(vector1, vector2)
{
return [vector1[0] + vector2[0], vector1[1] + vector2[1]];
}
topDiamond = [add(topDiamond[0], translationVector),
add(topDiamond[1], translationVector)];
Then you will need a rotation matrix:
var angle = -Math.atan2(topDiamond[1][1] - topDiamond[0][1],
topDiamond[1][0] - topDiamond[0][0]);
var rotMatrix = [[Math.cos(angle), -Math.sin(angle)],
[Math.sin(angle), Math.cos(angle)]];
After the multiplication with this matrix the points (225,2) and (337,114.5) are aligned on the X axis. But what you have now is a trapeze, you now need a horizontal shear transformation to get the other side of the diamond aligned on the Y axis:
function multiply(matrix, vector)
{
return [matrix[0][0] * vector[0] + matrix[0][1] * vector[1],
matrix[1][0] * vector[0] + matrix[1][1] * vector[1]];
}
var point = [topDiamond[0][0], (topDiamond[0][1] + topDiamond[1][1]) / 2];
point = multiply(rotMatrix, point);
var shearMatrix = [[1, -point[0] / point[1]], [0, 1]];
After multiplication with this matrix you have a rectangle now. Now you only need a scaling matrix to make sure that the X and Y coordinates of the corners have the value 0 and 1:
point = multiply(shearMatrix, point);
var point2 = [topDiamond[1][0], (topDiamond[0][1] + topDiamond[1][1]) / 2];
point2 = multiply(rotMatrix, point2);
point2 = multiply(shearMatrix, point2);
var scaleMatrix = [[1/point2[0], 0], [0, 1/point[1]]];
And there you have it, now you can apply these transformations to any point:
alert(
multiply(scaleMatrix,
multiply(shearMatrix,
multiply(rotMatrix,
add(translationVector, [260, 179])
)
)
)
);
This gives you 0.94,0.63 - both values are in the (0..1) range meaning that it is the top diamond. With [420,230] as input you get 1.88,0.14 - X in (1..2) range and Y in 0..1 range means right diamond. And so on.
Working example: http://jsfiddle.net/FzWHe/
In the retrospective, this was probably too much work for a simple geometrical figure like a diamond.
Essentially, what you have there is possibly an isometric view of 4 tiles (based on your comment about the diamonds appearing as trapezoids).
One quick way of doing this is to create 2 lines that are parallel with the "axes" of the "diamonds" (but still are crossing with each other...this is important as well). In the example image given, that would mean two lines that are vertical to each other but rotated by 45 degrees. In the isometric case, the lines will not be vertical to each other but at some other angle depending on your view.
Once you have these two lines you can create a "hitTest()" function that will be taking the coordinates of the point that was clicked and will be evaluating the two line equations. You are not really interested on the actual number returned by the line equations but only the signs. The sign shows you which side of the line does your point resides.
This means that your "diamonds" will correspond to these sign pairs (one sign for each line equation) [-,-], [-,+], [+,-], [+,+].
(Please note that the sign depends on the way that the line was defined, in other words for a given point P, the sign from some line equation (L) will be different if the line was defined as running "from left to right" or "from right to left", or more generally the sign will be the reverse for reciprocal directions.)
A bit more information about the form of the line equation you need can be obtained from here
Using matrices, you can derive a quick formula for which diamond is selected.
You want a transformation from (x,y) into "diamond-space". That is, a coordinate system where (0,0) is the top diamond, (1,0) is the one below to the right, and (0,1) below to the left.
A * x = y
where A is the transformation, x is the image coordinates, and y is the diamond-coordinates. To deal with the translation ((0,0) not being the same point in both spaces), you can add another row to the vectors, which is always 1.
You can transform multiple vectors at the same time, by putting them beside each other, so they form a matrix.
[ a b dx ] [ 225 337 113 ] [ 0 1 0 ]
[ c d dy ] * [ 2 114 114 ] = [ 0 0 1 ]
[ 0 0 1 ] [ 1 1 1 ] [ 1 1 1 ]
^ ^ ^-left ^-^-^--- new coordinates for each point
| '-right
'-top diamond
To solve for the coefficients in the first matrix, you need to divide by the second matrix (or multiply by the inverse).
[ a b dx ] [ 0 1 0 ] [ 225 337 113 ]^-1
[ c d dy ] = [ 0 0 1 ] * [ 2 114 114 ]
[ 0 0 1 ] [ 1 1 1 ] [ 1 1 1 ]
The result is:
[ a b dx ] [ (1/224) (1/224) (-227/224) ]
[ c d dy ] = [ (-1/224) (1/224) (223/224) ]
[ 0 0 1 ] [ 0 0 1 ]
To put this into program code:
function getDiamond(x, y) {
return [(x + y - 227) / 224, (-x + y + 223) / 224];
}
Example:
> getDiamond(260,179); // red
[0.9464285714285714, 0.6339285714285714]
> getDiamond(250,230); // green
[1.1294642857142858, 0.90625]
> getDiamond(189,250); // blue
[0.9464285714285714, 1.2678571428571428]
> getDiamond(420,230); // yellow
[1.8883928571428572, 0.14732142857142858]
If you look at the integer parts, you can see which diamond the coordinate corresponds to. The red one is at (0.94, 0.63) which is in region (0,0) pretty close to the edge of (1,0).
NB. The blue and green points in OP is drawn in the wrong location (or given wrong coordinates), so the result of my function places them in a different relative location.
If you do the calculations symbolically, you end up with this:
[ a b dx ] [ (y2 - y0)/M -(x2 - x0)/M -(x0*y2 - y0*x2)/M ]
[ c d dy ] = [-(y1 - y0)/M (x1 - x0)/M (x0*y1 - y0*x1)/M ]
[ 0 0 1 ] [ 0 0 1 ]
where M = x1*y2 - x2*y1 - y0*x1 + y0*x2 + x0*y1 - x0*y2.
Point 0 being the position of top diamond, point 1 being the position of right diamond, and point 2 being the position of left diamond.
Here is a function to calculate this:
function DiamondMaker(topx,topy, leftx,lefty, rightx,righty)
{
var M = topx*lefty - topx*righty +
leftx*righty - leftx*topy +
rightx*topy - rightx*lefty;
var a = -(topy - righty)/M;
var b = (topx - rightx)/M;
var dx = -(topx*righty - topy*rightx)/M;
var c = (topy - lefty)/M;
var d = -(topx - leftx)/M;
var dy = (topx*lefty - topy*leftx)/M;
return function(x, y) {
return [a * x + b * y + dx, c * x + d * y + dy];
};
}
var getDiamond = DiamondMaker(225,2, 337,114, 113,114);
// (same example as before)
All you need - just stady what is roration. Here is link: http://en.wikipedia.org/wiki/Rotation_(mathematics)
You should rotate you point in order to make sides of squares in parrallel with coordinate's grid. Point of rotaion should be 1 corner of dimonds you will threat as 0,0 diamond. After rotaion you can easily define how many daimond you point away from 0,0

Finding quaternion representing the rotation from one vector to another

I have two vectors u and v. Is there a way of finding a quaternion representing the rotation from u to v?
Quaternion q;
vector a = crossproduct(v1, v2);
q.xyz = a;
q.w = sqrt((v1.Length ^ 2) * (v2.Length ^ 2)) + dotproduct(v1, v2);
Don't forget to normalize q.
Richard is right about there not being a unique rotation, but the above should give the "shortest arc," which is probably what you need.
Half-Way Vector Solution
I came up with the solution that I believe Imbrondir was trying to present (albeit with a minor mistake, which was probably why sinisterchipmunk had trouble verifying it).
Given that we can construct a quaternion representing a rotation around an axis like so:
q.w == cos(angle / 2)
q.x == sin(angle / 2) * axis.x
q.y == sin(angle / 2) * axis.y
q.z == sin(angle / 2) * axis.z
And that the dot and cross product of two normalized vectors are:
dot == cos(theta)
cross.x == sin(theta) * perpendicular.x
cross.y == sin(theta) * perpendicular.y
cross.z == sin(theta) * perpendicular.z
Seeing as a rotation from u to v can be achieved by rotating by theta (the angle between the vectors) around the perpendicular vector, it looks as though we can directly construct a quaternion representing such a rotation from the results of the dot and cross products; however, as it stands, theta = angle / 2, which means that doing so would result in twice the desired rotation.
One solution is to compute a vector half-way between u and v, and use the dot and cross product of u and the half-way vector to construct a quaternion representing a rotation of twice the angle between u and the half-way vector, which takes us all the way to v!
There is a special case, where u == -v and a unique half-way vector becomes impossible to calculate. This is expected, given the infinitely many "shortest arc" rotations which can take us from u to v, and we must simply rotate by 180 degrees around any vector orthogonal to u (or v) as our special-case solution. This is done by taking the normalized cross product of u with any other vector not parallel to u.
Pseudo code follows (obviously, in reality the special case would have to account for floating point inaccuracies -- probably by checking the dot products against some threshold rather than an absolute value).
Also note that there is no special case when u == v (the identity quaternion is produced -- check and see for yourself).
// N.B. the arguments are _not_ axis and angle, but rather the
// raw scalar-vector components.
Quaternion(float w, Vector3 xyz);
Quaternion get_rotation_between(Vector3 u, Vector3 v)
{
// It is important that the inputs are of equal length when
// calculating the half-way vector.
u = normalized(u);
v = normalized(v);
// Unfortunately, we have to check for when u == -v, as u + v
// in this case will be (0, 0, 0), which cannot be normalized.
if (u == -v)
{
// 180 degree rotation around any orthogonal vector
return Quaternion(0, normalized(orthogonal(u)));
}
Vector3 half = normalized(u + v);
return Quaternion(dot(u, half), cross(u, half));
}
The orthogonal function returns any vector orthogonal to the given vector. This implementation uses the cross product with the most orthogonal basis vector.
Vector3 orthogonal(Vector3 v)
{
float x = abs(v.x);
float y = abs(v.y);
float z = abs(v.z);
Vector3 other = x < y ? (x < z ? X_AXIS : Z_AXIS) : (y < z ? Y_AXIS : Z_AXIS);
return cross(v, other);
}
Half-Way Quaternion Solution
This is actually the solution presented in the accepted answer, and it seems to be marginally faster than the half-way vector solution (~20% faster by my measurements, though don't take my word for it). I'm adding it here in case others like myself are interested in an explanation.
Essentially, instead of calculating a quaternion using a half-way vector, you can calculate the quaternion which results in twice the required rotation (as detailed in the other solution), and find the quaternion half-way between that and zero degrees.
As I explained before, the quaternion for double the required rotation is:
q.w == dot(u, v)
q.xyz == cross(u, v)
And the quaternion for zero rotation is:
q.w == 1
q.xyz == (0, 0, 0)
Calculating the half-way quaternion is simply a matter of summing the quaternions and normalizing the result, just like with vectors. However, as is also the case with vectors, the quaternions must have the same magnitude, otherwise the result will be skewed towards the quaternion with the larger magnitude.
A quaternion constructed from the dot and cross product of two vectors will have the same magnitude as those products: length(u) * length(v). Rather than dividing all four components by this factor, we can instead scale up the identity quaternion. And if you were wondering why the accepted answer seemingly complicates matters by using sqrt(length(u) ^ 2 * length(v) ^ 2), it's because the squared length of a vector is quicker to calculate than the length, so we can save one sqrt calculation. The result is:
q.w = dot(u, v) + sqrt(length_2(u) * length_2(v))
q.xyz = cross(u, v)
And then normalize the result. Pseudo code follows:
Quaternion get_rotation_between(Vector3 u, Vector3 v)
{
float k_cos_theta = dot(u, v);
float k = sqrt(length_2(u) * length_2(v));
if (k_cos_theta / k == -1)
{
// 180 degree rotation around any orthogonal vector
return Quaternion(0, normalized(orthogonal(u)));
}
return normalized(Quaternion(k_cos_theta + k, cross(u, v)));
}
The problem as stated is not well-defined: there is not a unique rotation for a given pair of vectors. Consider the case, for example, where u = <1, 0, 0> and v = <0, 1, 0>. One rotation from u to v would be a pi / 2 rotation around the z-axis. Another rotation from u to v would be a pi rotation around the vector <1, 1, 0>.
I'm not much good on Quaternion. However I struggled for hours on this, and could not make Polaris878 solution work. I've tried pre-normalizing v1 and v2. Normalizing q. Normalizing q.xyz. Yet still I don't get it. The result still didn't give me the right result.
In the end though I found a solution that did. If it helps anyone else, here's my working (python) code:
def diffVectors(v1, v2):
""" Get rotation Quaternion between 2 vectors """
v1.normalize(), v2.normalize()
v = v1+v2
v.normalize()
angle = v.dot(v2)
axis = v.cross(v2)
return Quaternion( angle, *axis )
A special case must be made if v1 and v2 are paralell like v1 == v2 or v1 == -v2 (with some tolerance), where I believe the solutions should be Quaternion(1, 0,0,0) (no rotation) or Quaternion(0, *v1) (180 degree rotation)
Why not represent the vector using pure quaternions? It's better if you normalize them first perhaps.
q1 = (0 ux uy uz)'
q2 = (0 vx vy vz)'
q1 qrot = q2
Pre-multiply with q1-1
qrot = q1-1 q2
where q1-1 = q1conj / qnorm
This is can be thought of as "left division".
Right division, which is not what you want is:
qrot,right = q2-1 q1
From algorithm point of view , the fastest solution looks in pseudocode
Quaternion shortest_arc(const vector3& v1, const vector3& v2 )
{
// input vectors NOT unit
Quaternion q( cross(v1, v2), dot(v1, v2) );
// reducing to half angle
q.w += q.magnitude(); // 4 multiplication instead of 6 and more numerical stable
// handling close to 180 degree case
//... code skipped
return q.normalized(); // normalize if you need UNIT quaternion
}
Be sure that you need unit quaternions (usualy, it is required for interpolation).
NOTE:
Nonunit quaternions can be used with some operations faster than unit.
Some of the answers don't seem to consider possibility that cross product could be 0. Below snippet uses angle-axis representation:
//v1, v2 are assumed to be normalized
Vector3 axis = v1.cross(v2);
if (axis == Vector3::Zero())
axis = up();
else
axis = axis.normalized();
return toQuaternion(axis, ang);
The toQuaternion can be implemented as follows:
static Quaternion toQuaternion(const Vector3& axis, float angle)
{
auto s = std::sin(angle / 2);
auto u = axis.normalized();
return Quaternion(std::cos(angle / 2), u.x() * s, u.y() * s, u.z() * s);
}
If you are using Eigen library, you can also just do:
Quaternion::FromTwoVectors(from, to)
Working just with normalized quaternions, we can express Joseph Thompson's answer in the follwing terms.
Let q_v = (0, u_x, v_y, v_z) and q_w = (0, v_x, v_y, v_z) and consider
q = q_v * q_w = (-u dot v, u x v).
So representing q as q(q_0, q_1, q_2, q_3) we have
q_r = (1 - q_0, q_1, q_2, q_3).normalize()
According to the derivation of the quaternion rotation between two angles, one can rotate a vector u to vector v with
function fromVectors(u, v) {
d = dot(u, v)
w = cross(u, v)
return Quaternion(d + sqrt(d * d + dot(w, w)), w).normalize()
}
If it is known that the vectors u to vector v are unit vectors, the function reduces to
function fromUnitVectors(u, v) {
return Quaternion(1 + dot(u, v), cross(u, v)).normalize()
}
Depending on your use-case, handling the cases when the dot product is 1 (parallel vectors) and -1 (vectors pointing in opposite directions) may be needed.
The Generalized Solution
function align(Q, u, v)
U = quat(0, ux, uy, uz)
V = quat(0, vx, vy, vz)
return normalize(length(U*V)*Q - V*Q*U)
To find the quaternion of smallest rotation which rotate u to v, use
align(quat(1, 0, 0, 0), u, v)
Why This Generalization?
R is the quaternion closest to Q which will rotate u to v. More importantly, R is the quaternion closest to Q whose local u direction points in same direction as v.
This can be used to give you all possible rotations which rotate from u to v, depending on the choice of Q. If you want the minimal rotation from u to v, as the other solutions give, use Q = quat(1, 0, 0, 0).
Most commonly, I find that the real operation you want to do is a general alignment of one axis with another.
// If you find yourself often doing something like
quatFromTo(toWorldSpace(Q, localFrom), worldTo)*Q
// you should instead consider doing
align(Q, localFrom, worldTo)
Example
Say you want the quaternion Y which only represents Q's yaw, the pure rotation about the y axis. We can compute Y with the following.
Y = align(quat(Qw, Qx, Qy, Qz), vec(0, 1, 0), vec(0, 1, 0))
// simplifies to
Y = normalize(quat(Qw, 0, Qy, 0))
Alignment as a 4x4 Projection Matrix
If you want to perform the same alignment operation repeatedly, because this operation is the same as the projection of a quaternion onto a 2D plane embedded in 4D space, we can represent this operation as the multiplication with 4x4 projection matrix, A*Q.
I = mat4(
1, 0, 0, 0,
0, 1, 0, 0,
0, 0, 1, 0,
0, 0, 0, 1)
A = I - leftQ(V)*rightQ(U)/length(U*V)
// which expands to
A = mat4(
1 + ux*vx + uy*vy + uz*vz, uy*vz - uz*vy, uz*vx - ux*vz, ux*vy - uy*vx,
uy*vz - uz*vy, 1 + ux*vx - uy*vy - uz*vz, uy*vx + ux*vy, uz*vx + ux*vz,
uz*vx - ux*vz, uy*vx + ux*vy, 1 - ux*vx + uy*vy - uz*vz, uz*vy + uy*vz,
ux*vy - uy*vx, uz*vx + ux*vz, uz*vy + uy*vz, 1 - ux*vx - uy*vy + uz*vz)
// A can be applied to Q with the usual matrix-vector multiplication
R = normalize(A*Q)
//LeftQ is a 4x4 matrix which represents the multiplication on the left
//RightQ is a 4x4 matrix which represents the multiplication on the Right
LeftQ(w, x, y, z) = mat4(
w, -x, -y, -z,
x, w, -z, y,
y, z, w, -x,
z, -y, x, w)
RightQ(w, x, y, z) = mat4(
w, -x, -y, -z,
x, w, z, -y,
y, -z, w, x,
z, y, -x, w)

Resources