Vector Math

Vector Math Basics

This article describes vectors and their operators in the context of XNA. For explanations of what a vector is, see Wikipedia's article.

Direction From One Point To Another

Finding the direction from one point to another is very simple, you simply subtract your origin position from your destination position, and then normalize/unitize the vector.

Vector3 destination = new Vector3(10, 15, 10);
Vector3 origin = new Vector3(2, 15, 0);
 
Vector3 direction = destination - origin;
direction.Normalize();

Signed Difference Angle Between Two Vectors

/// Find the angle between two vectors. This will not only give the angle difference, but the direction.
/// For example, it may give you -1 radian, or 1 radian, depending on the direction. Angle given will be the 
/// angle from the FromVector to the DestVector, in radians.
/// </summary>
/// <param name="FromVector">Vector to start at.</param>
/// <param name="DestVector">Destination vector.</param>
/// <param name="DestVectorsRight">Right vector of the destination vector</param>
/// <returns>Signed angle, in radians</returns>        
/// <remarks>All three vectors must lie along the same plane.</remarks>
public static double GetSignedAngleBetween2DVectors(Vector3 FromVector, Vector3 DestVector, Vector3 DestVectorsRight)
{
    FromVector.Normalize();
    DestVector.Normalize();
    DestVectorsRight.Normalize();
 
    float forwardDot = Vector3.Dot(FromVector, DestVector);
    float rightDot = Vector3.Dot(FromVector, DestVectorsRight);
 
    // Keep dot in range to prevent rounding errors
    forwardDot = MathHelper.Clamp(forwardDot, -1.0f, 1.0f);
 
    double angleBetween = Math.Acos(forwardDot);
 
    if (rightDot < 0.0f)
        angleBetween *= -1.0f;
 
    return angleBetween;
}

Unsigned Difference Angle Between Two Vectors

public float UnsignedAngleBetweenTwoV3(Vector3 v1, Vector3 v2)
{
    v1.Normalize();
    v2.Normalize();
    double Angle = (float)Math.Acos(Vector3.Dot(v1, v2));
    return (float)Angle;
}

Elastic Deflection

The XNA Vector library already includes a method for deflection, accessible through Vector3.Reflect(...), however it lacks elasticity. I thought it would be beneficial for those wanting to learn the actual method. Below are both methods.

/// <summary>
/// Returns velocity of deflection
/// </summary>
/// <param name="CurrentVelocity">Velocity vector if item to be bounced</param>
/// <param name="Elasticity">Elasticity of item to be bounced</param>
/// <param name="CollisionNormal">Normal vector of plane the item is bouncing off of</param>
/// <returns>Velocity vector of deflection</returns>
public Vector3 CalculateDeflection(Vector3 CurrentVelocity, float Elasticity, Vector3 CollisionNormal)
{
    Vector3 newDirection = Elasticity * (-2 * Vector3.Dot(CurrentVelocity, CollisionNormal) * CollisionNormal + CurrentVelocity);
 
    return newDirection;
}
 
public Vector3 CalculateDeflection(Vector3 CurrentVelocity, float Elasticity, Vector3 CollisionNormal)
{
    return Vector3.Reflect(CurrentVelocity, CollisionNormal) * Elasticity;
}

Calculate the rotation for one vector to face another vector

public Matrix LookAt(Vector3 position, Vector3 lookat)
{
    Matrix rotation = new Matrix();
 
    rotation.Forward = Vector3.Normalize(lookat - position);
    rotation.Right = Vector3.Normalize(Vector3.Cross(rotation.Forward, Vector3.Up));
    rotation.Up = Vector3.Normalize(Vector3.Cross(rotation.Right, rotation.Forward));
 
    return rotation;
}

Translate a Point around an Origin

/// <summary>
/// Translates a point around an origin
/// </summary>
/// <param name="point">Point that is going to be translated</param>
/// <param name="originPoint">Origin of rotation</param>
/// <param name="rotationAxis">Axis to rotate around, this Vector should be a unit vector (normalized)</param>
/// <param name="radiansToRotate">Radians to rotate</param>
/// <returns>Translated point</returns>
public Vector3 RotateAroundPoint(Vector3 point, Vector3 originPoint, Vector3 rotationAxis, float radiansToRotate)
{
    Vector3 diffVect = point - originPoint;
 
    Vector3 rotatedVect = Vector3.Transform(diffVect, Matrix.CreateFromAxisAngle(rotationAxis, radiansToRotate));
 
    rotatedVect += originPoint;
 
    return rotatedVect;
}

Convert a Quaternion to Vector3

public Vector3 QuaternionToEuler(Quaternion q)
{
    Vector3 v = Vector3.Zero;
 
    v.X = (float)Math.Atan2 
    (
        2 * q.Y * q.W - 2 * q.X * q.Z,
           1 - 2 * Math.Pow(q.Y, 2) - 2 * Math.Pow(q.Z, 2)
    );
 
    v.Z = (float)Math.Asin
    (
        2 * q.X * q.Y + 2 * q.Z * q.W
    );
 
    v.Y = (float)Math.Atan2
    (
        2 * q.X * q.W - 2 * q.Y * q.Z,
        1 - 2 * Math.Pow(q.X, 2) - 2 * Math.Pow(q.Z, 2)
    );
 
    if (q.X * q.Y + q.Z * q.W == 0.5)
    {
        v.X = (float)(2 * Math.Atan2(q.X, q.W));
        v.Y = 0;
    }
 
    else if (q.X * q.Y + q.Z * q.W == -0.5)
    {
        v.X = (float)(-2 * Math.Atan2(q.X, q.W));
        v.Y = 0;
    }
 
    return v;
}