How to get rotation from two vectors?

Posted on | Edited on | In
This article tells what is happening when you try to get a rotation that rotate a vector so that it eventually aligns to another vector.

Introduction

It is fairly common to calcualte a rotator that rotate a vector $v$ to another vector $u$, especially when we handle something like animation IK.

Game engines should have their own method for this feature. Quaternion.FromToRotation() in Unity, and FQuat::FindBetweenNormals() in UE4. But it is always good to know what is actually happending deep inside.

There are already some really goot posts about this issue you might want to read as well:
http://lolengine.net/blog/2014/02/24/quaternion-from-two-vectors-final
http://www.euclideanspace.com/maths/algebra/vectors/angleBetween/index.htm

Math Logic

A Naive Method

As we all know, a quaternion can be presented by rotating $\theta$ angle around an axis $w$:

Angle can be easily calculated by dot production: 

1
2
3
4
5
6
7
quat quat::fromtwovectors(vec3 u, vec3 v)
{
    float cos_theta = dot(normalize(u), normalize(v));
    float angle = acos(cos_theta);
    vec3 w = normalize(cross(u, v));
    return quat::fromaxisangle(angle, w);
}

It works, in a naive way, though.

How To Build a Quaternion?

It is worth mentioning how a quaterion is built by an axis and an angle.

Quaternion is constructed by:

So the code for quat::fromaxisangle is like this:

1
2
3
4
5
6
7
8
9
quat quat::fromaxisangle(float angle, vec3 axis)
{
    float half_sin = sin(0.5f * angle);
    float half_cos = cos(0.5f * angle);
    return quat(half_cos,
                half_sin * axis.x,
                half_sin * axis.y,
                half_sin * axis.z);
}

Optimization

Avoiding Trigonometry

Trigonometry is expensive. Thus we need to avoid trigonometry if we can. Inigo Quilez has wrote an article about this: https://www.iquilezles.org/www/articles/noacos/noacos.htm

Sadly, in our naive method, we firstly compute $\theta$ by $\cos{\theta}$, then we compute $\cos{\frac{\theta}{2}}$ and $\sin{\frac{\theta}{2}}$. This is, as a matter of fact, really expensive.

Thus we can save performance by half-angle by:

So we can save these performance by: 

1
2
3
4
5
6
7
8
9
10
11
quat quat::fromtwovectors(vec3 u, vec3 v)
{
    float cos_theta = dot(normalize(u), normalize(v));
    float half_cos = sqrt(0.5f * (1.f + cos_theta));
    float half_sin = sqrt(0.5f * (1.f - cos_theta));
    vec3 w = normalize(cross(u, v));
    return quat(half_cos,
                half_sin * w.x,
                half_sin * w.y,
                half_sin * w.z);
}

Avoid One Square Root

Still, we can do better. We need to normalize 3 vectors: u, v andcross(u, v). But we can get the norm of w through:

And we know $\sin{\theta}$ by:

And we also have :

So we can save 2 square roots:

1
2
3
4
5
6
7
8
9
10
11
12
quat quat::fromtwovectors(vec3 u, vec3 v)
{
    float norm_u_norm_v = sqrt(sqlength(u) * sqlength(v));
    float cos_theta = dot(u, v) / norm_u_norm_v;
    float half_cos = sqrt(0.5f * (1.f + cos_theta));
    float half_sin = sqrt(0.5f * (1.f - cos_theta));
    vec3 w = cross(u, v) / (norm_u_norm_v * 2.f * half_sin * half_cos);
    return quat(half_cos,
                half_sin * w.x,
                half_sin * w.y,
                half_sin * w.z);
}

Avoid One More Square Root

And we can simplify this by get rid of $\sin{\theta / 2}$:

1
2
3
4
5
6
7
8
quat quat::fromtwovectors(vec3 u, vec3 v)
{
    float norm_u_norm_v = sqrt(sqlength(u) * sqlength(v));
    float cos_theta = dot(u, v) / norm_u_norm_v;
    float half_cos = sqrt(0.5f * (1.f + cos_theta));
    vec3 w = cross(u, v) / (norm_u_norm_v * 2.f * half_cos);
    return quat(half_cos, w.x, w.y, w.z);
}

This is basically code used by ORGE3D

Consider SIMD

On many SIMD architectures, normalizing a quaterion can be very fast.

So we should take this advantage:

1
2
3
4
5
6
7
8
quat quat::fromtwovectors(vec3 u, vec3 v)
{
    float norm_u_norm_v = sqrt(sqlength(u) * sqlength(v));
    float cos_theta = dot(u, v) / norm_u_norm_v;
    float half_cos = sqrt(0.5f * (1.f + cos_theta));
    vec3 w = cross(u, v) / norm_u_norm_v;
    return normalize(quat(2.f * half_cos * half_cos, w.x, w.y, w.z));
}

More over, half_cos comes from a sqrt, thus we can get rid of this square root :

1
2
3
4
5
6
7
quat quat::fromtwovectors(vec3 u, vec3 v)
{
    float norm_u_norm_v = sqrt(sqlength(u) * sqlength(v));
    float cos_theta = dot(u, v) / norm_u_norm_v;
    vec3 w = cross(u, v) / norm_u_norm_v;
    return normalize(quat(1.f + cos_theta, w.x, w.y, w.z));
}

And for the same reason, we can have:

1
2
3
4
5
6
7
quat quat::fromtwovectors(vec3 u, vec3 v)
{
    float norm_u_norm_v = sqrt(sqlength(u) * sqlength(v));
    vec3 w = cross(u, v);
    quat q = quat(norm_u_norm_v + dot(u, v), w.x, w.y, w.z);
    return normalize(q);
}

Unit Vector

If vector u and v are unit vectors, we can have:

1
2
3
4
5
6
quat quat::fromtwovectors(vec3 u, vec3 v)
{
    vec3 w = cross(u, v);
    quat q = quat(1.f + dot(u, v), w.x, w.y, w.z);
    return normalize(q);
}

For Non-Unit Vector

Because we need to calculate $dot(u, v)$ and $cross(u, v)$ no matter what, thus norm_u_norm_v can be calculated in a different way: 

1
2
3
4
5
6
7
quat quat::fromtwovectors(vec3 u, vec3 v)
{
    vec3 w = cross(u, v);
    quat q = quat(dot(u, v), w.x, w.y, w.z);
    q.w += length(q);
    return normalize(q);
}

We can get rid of at least 3 multiplications in this way.

Final Version

I just copy code from : http://www.euclideanspace.com/maths/algebra/vectors/angleBetween/index.htm

1
2
3
4
5
6
7
8
9
10
11
12
13
14
/** note v1 and v2 dont have to be nomalised, thanks to minorlogic for telling me about this:
* https://www.euclideanspace.com/maths/algebra/vectors/angleBetween/minorlogic.htm
*/
sfquat angleBetween(sfvec3f v1,sfvec3f v2) { 
	float d = sfvec3f.dot(v1,v2); 
	sfvec3f axis = v1;
	axis.cross(v2);
    float qw = (float)Math.sqrt(v1.len_squared()*v2.len_squared()) + d;
	if (qw < 0.0001) { // vectors are 180 degrees apart
		return (new sfquat(0,-v1.z,v1.y,v1.x)).norm;
	} 
	sfquat q= new sfquat(qw,axis.x,axis.y,axis.z); 
    return q.norm();
}
I am so cute, please give me money...