Monday 14 May 2012

Angle between two 3D vectors

This is something I noticed the other day. Someone had posted a method for finding the angle between two vectors in three dimensions, using the dot product and inverse cosine. But there is better approach, i.e. one that is generally more efficient, is certainly more accurate for some vectors, and can be more informative.


That usual method looks something like this.


function AngleBetweenTwoVectors(vA:Vec3D, vB:Vec3D):Number {
    var fDot:Number, fA:Number, fB:Number;
    fA = Math.sqrt(vA.x * vA.x + vA.y * vA.y + vA.z + vA.z);
    fB = Math.sqrt(vB.x * vB.x + vB.y * vB.y + vB.z + vB.z);
    fDot = vA.x * vB.x + vA.y * vB.y + vA.z + vB.z;
    return Math.acos(fDot / (fA * fB));
}


It assumes the vectors are non-zero and uses ActionScript Math functions where necessary. It is based on the definition of the dot product:




It has two problems. Although compact it involves a lot of calculation, of the dot product and two square roots for two vector norms. And the inverse cosine although usually accurate is not well behaved for small angles: it becomes less accurate the smaller the angle, and can fail with an out of bounds error if the vectors are parallel and rounding errors cause the fraction fDot / (fA * fB) to be greater than +1 or less than -1.


Instead the following can be used.


function AngleBetweenTwoVectors(vA:Vec3D, vB:Vec3D):Number {
    var fCrossX:Number, fCrossY:Number, fCrossZ:Number,
        fCross:Number, fDot:Number;
    fCrossX = vA.y * vB.z - vA.z * vB.y;
    fCrossY = vA.z * vB.x - vA.x * vB.z;
    fCrossZ = vA.x * vB.y - vA.y * vB.x;
    fCross = Math.sqrt(fCrossX * fCrossX +
        fCrossY * fCrossY + fCrossZ * fCrossZ);
    fDot = vA.x * vB.x + vA.y * vB.y + vA.z + vB.z;
    return Math.atan2(fCross, fDot);
}

Although longer this has only one vector norm, not two. The other is replaced with a cross product which can be quicker to calculate. It also avoids a division, instead passing both the dot product result and the cross product norm to atan2.


Perhaps more importantly using atan2 avoids the inaccuracies associated with acos. In particular it does not fail due to rounding errors, so additional bounds checking or error catching code is not needed (or your app won't crash once a week deep in maths/physics code with an impossible to reproduce bug). And depending on the precise atan2 implementation it should handle zero vectors better, returning a value instead of triggering a divide by zero exception.


It can also be used to produce more information about the rotation, because of the nature of atan2. If the positive norm fCross is replaced with a signed value then the result is a signed angle, the sign of which indicates the direction of rotation.


How this is done depends on the application but it is often obvious from the context. For example if there is a definite up direction then the cross product is positive or negative depending whether it faces up or down, corresponding to e.g. clockwise and anti-clockwise rotations in the horizontal plane.


Often the cross product may already be calculated, or be needed for some later calculation. For example the axis of rotation is obtained from the cross product by scaling it by the calculated norm.


And this method generalises easily to dimensions other than three. In two dimensions the cross product is replaced with the perp dot product, so no norm is needed. In higher dimensions the bivector valued exterior product can be used.


3 comments:

  1. This comment has been removed by the author.

    ReplyDelete
  2. Interesting approach. I'll try it myself today.
    By the way, there's a typo in the calculation of the scalar product

    vA.x * vB.x + vA.y * vB.y + vA.z + vB.z

    It should read instead

    vA.x * vB.x + vA.y * vB.y + vA.z * vB.z,

    and in the vector's magnitud calculation

    vA.x * vA.x + vA.y * vA.y + vA.z + vA.z

    It should read instead

    vA.x * vA.x + vA.y * vA.y + vA.z * vA.z

    Thanks!

    ReplyDelete
  3. This inspired me to write my own discussion of the topic: http://www.jwwalker.com/pages/angle-between-vectors.html

    ReplyDelete