Ugo Belfiore

Rotate a point around another in 3d space is not so easy if we want to handle any axis, without gimble lock.

The goal of this function is to use pure math, without any Transform whatsoever. Not just for the beauty of it, but mainly because sometime we want to calculate this kind of stuff without having to use, or generate empty gameObjects. I will use matrix, quaternion and some basic math. Here are the given parameters:

```
Vector3 pivotPoint
```
(pivot reference for the rotation)
```
Vector3 pointToRotate
```
(point to rotate along the given up axis)
```
Vector3 upNormalized
```
(up axis)
```
Vector3 rotationAxis
```
(X, Y, Z rotation to apply)

Common way of rotating things in global space

If you are using transform, a simple

transform.Rotate(X, Y, Z)

is often enought to rotate a transform, localy or globaly. Internally for a given euler Angle(X, Y, Z), Unity use this kind of calculation to rotate their transform in global space:

Quaternion rotationToApply = Quaternion.Euler(X, Y, Z);
currentRotation = currentRotation * (Quaternion.Inverse(currentRotation) * rotationToApply * currentRotation);

CurrentRotation is the rotation of the Matrix TRS of the transform.

When you want to rotate a point around a pivot in global space (from a world with Vector3.up as the real up), these function bellow are enought:

public static Vector3 RotatePointAroundPivot(Vector3 pivot, Vector3 point, Vector3 angles)
{
   return RotatePointAroundPivot(point, pivot, Quaternion.Euler(angles));
}

public static Vector3 RotatePointAroundPivot(Vector3 pivot, Vector3 point, Quaternion rotation)
{
   return rotation * (point - pivot) + pivot;
}

As I said, this method work ONLY with a World with these references:

```
up: Vector3.up (0, 1, 0)
```
```
forward: Vector3.forward (0, 0, 1)
```
```
right: Vector3.right (1, 0, 0)
```

But what if I want to rotate from an other rotation reference ?

Well, use our own matrix to rotate properly from a local space:

Full functions:

function RotatePointAroundAxis() and RotateVectorAroundAxis()

Copy

/// <summary>
/// Rotate a Point around an axis
/// usage:
/// Vector3 position = ExtRotation.RotatePointAroundAxis(pivotPosition, pointToRotate, pivotUp, _rotateAxis * TimeEditor.deltaTime);
/// </summary>
public static Vector3 RotatePointAroundAxis(Vector3 pivotPoint, Vector3 pointToRotate, Vector3 upNormalized, Vector3 rotationAxis)
{
    Vector3 vectorDirector = pointToRotate - pivotPoint;
    Vector3 finalPoint = RotateWithMatrix(pivotPoint, vectorDirector, upNormalized, rotationAxis);
    return (finalPoint);
}

/// <summary>
/// Rotate a vectorDirector around an axis
/// usage:
/// Vector3 vectorDirector = ExtRotation.RotateVectorAroundAxis(pivotPoint, vectorDirector, pivotUp, _rotateAxis * TimeEditor.deltaTime);
/// </summary>
public static Vector3 RotateVectorAroundAxis(Vector3 pivotPoint, Vector3 vectorDirector, Vector3 upNormalized, Vector3 rotationAxis)
{
    Vector3 finalPoint = RotateWithMatrix(pivotPoint, vectorDirector, upNormalized, rotationAxis);
    return (finalPoint - pivotPoint);
}

private static Vector3 RotateWithMatrix(Vector3 pivotPoint, Vector3 vectorDirector, Vector3 upNormalized, Vector3 rotationAxis)
{
    Quaternion constrainRotation = TurretLookRotation(vectorDirector, upNormalized);            //constrain rotation from up !!

    //create a TRS matrix from point & rotation
    Matrix4x4 rotationMatrix = Matrix4x4.TRS(pivotPoint, constrainRotation, Vector3.one);

    Vector3 projectedForward = ExtVector3.ProjectAOnB(vectorDirector, rotationMatrix.ForwardFast());
    Vector3 projectedUp = ExtVector3.ProjectAOnB(vectorDirector, upNormalized);
    float distanceForward = projectedForward.magnitude;
    float distanceUp = projectedUp.magnitude;

    if (ExtVector3.DotProduct(upNormalized, vectorDirector) < 0)
    {
        distanceUp *= -1;
    }

    //rotate matrix in x, y & z
    rotationMatrix = Matrix4x4.TRS(pivotPoint, constrainRotation * Quaternion.Euler(rotationAxis), Vector3.one);
    Vector3 finalPoint = rotationMatrix.MultiplyPoint3x4(new Vector3(0, distanceUp, distanceForward));
    return finalPoint;
}

public static Quaternion TurretLookRotation(Vector3 approximateForward, Vector3 exactUp)
{
    Quaternion rotateZToUp = Quaternion.LookRotation(exactUp, -approximateForward);
    Quaternion rotateYToZ = Quaternion.Euler(90f, 0f, 0f);

    return rotateZToUp * rotateYToZ;
}

public static Vector3 ProjectAOnB(Vector3 A, Vector3 B)
{
    float sqrMag = DotProduct(B, B);
    if (sqrMag < Mathf.Epsilon)
    {
        return (Vector3.zero);
    }
    else
    {
        var dot = DotProduct(A, B);
        return new Vector3(B.x * dot / sqrMag,
            B.y * dot / sqrMag,
            B.z * dot / sqrMag);
    }
}

public static float DotProduct(Vector3 a, Vector3 b)
{
    return (a.x * b.x + a.y * b.y + a.z * b.z);
}

How does it work ?

First, let's recapitulate what we need:

```
Vector3 pivotPoint
```
(pivot reference for the rotation)
```
Vector3 pointToRotate
```
(point to rotate along the given up axis)
```
Vector3 upNormalized
```
(up axis)
```
Vector3 rotationAxis
```
(X, Y, Z rotation to apply)

From there we can visualize the Up vector, and calculate the vector Director

Vector3 vectorDirector = pointToRotate - pivotPoint;

Create a TRS Matrix

A TRS matrix is the representation of a position, rotation and scale in the world. we have the position

Vector3 pivotPoint

, the scale is

Vector3.One

, but we miss the rotation. We need to turn our

vectorDirector

in Quaternion.

This Quatenion needs to be align to the up vector. It represents the blue arrow on the picture bellow. The following function is doing just that:

Quaternion constrainRotation = TurretLookRotation(vectorDirector, upNormalized);

Now let's create our matrix:

Matrix4x4.TRS(pivotPoint, constrainRotation, Vector3.one);

To get the forward, right & up of a matrix, I have created convenient extension:

Matrix4x4.UpFast()
Matrix4x4.ForwardFast()
Matrix4x4.RightFast()

They simply apply a

Matrix4x4.getColumn(i)

to get the appropriate rotation

Now you need to get these yellow vectors for later

A vector projection is done with this function:

Vector3 projectedVector = ProjectAOnB(Vector3 A, Vector3 B)

We need the magnitude forward and up. Therefor we calculate the 2 vectors, taking care of the potential negative value for up

Vector3 projectedForward = ProjectAOnB(vectorDirector, rotationMatrix.ForwardFast());
Vector3 projectedUp = ProjectAOnB(vectorDirector, upNormalized);
float distanceForward = projectedForward.magnitude;
float distanceUp = projectedUp.magnitude;

if (DotProduct(upNormalized, vectorDirector) < 0)
{
    distanceUp *= -1;
}

Apply the rotation of the matrix

Two more step to go. We can finally rotate the matrix. I have decide by simplicity to create another Matrix, with a new rotation offset

rotationMatrix = Matrix4x4.TRS(pivotPoint, constrainRotation * Quaternion.Euler(rotationAxis), Vector3.one);

Get the final point:

And after all this work, we can use the function

MultiplyPoint3x4

, which transfrom our previous local distance Up and Forward to a global final point

Vector3 finalPoint = rotationMatrix.MultiplyPoint3x4(new Vector3(0, distanceUp, distanceForward));

BELFIORE Ugo

> CV <

CV

Back to projects

Back to devlogs

Rotate a point around another from any axis, using matrix, and without using any Transforms