Change of Basis Pt.2 [Unreal]

Some time ago I wrote a very informal post about going from one coordinate system to another one, using a change of basis matrix.

This time, I am complementing that post by presenting the C++ Unreal function I created which does just that conversion, and accepts any coordinate frame as input. The code could be easily converted to other APIs.
My team uses this kind of conversion to pass transforms/poses over to and from Unreal, OpenCV, OpenGL, robots, arbitrarily moving references, etc. The on presented here is the version we use on the Unreal side.

As a reminder, here’s the general change of basis formula:

M_B = CM_AC^{-1}

Then, the actual code, which takes as input the destination frame Forward, Right and Up directions. The final parameter is of course the Transform we are about to “convert”.
Note that SomeNamespace is just to wrap the function within a namespace, and you should of course use your own name instead of this lazy placeholder!

Declaration (header file):

// Dario Mazzanti, 2021
// Some.h header file


class SOME_PROJECT_OR_PLUGIN_API SomeNamespace 
{
public:

	/**
     * Changes the provided transform basis from World frame to Destination frame
     * @param DestinationFrameForward The forward direction of the destination coordinates frame
	 * @param DestinationFrameRight The right direction of the destination coordinates frame
	 * @param DestinationFrameUp The up direction of the destination coordinates frame
	 * @param TransformToTranslateWorldFrame The transform to be changed from World frame to Destination frame
     * @return The provided transform after the change of basis, ready to be applied in the Destination frame
     */
	static FTransform ApplyChangeOfBasis(FVector DestinationFrameForward, FVector DestinationFrameRight, FVector DestinationFrameUp, FTransform TransformToTranslateWorldFrame);
};

Definition (cpp file):

// Dario Mazzanti, 2021
// Some.cpp source file

#pragma once

#include "Some.h"

FTransform SomeNamespace::ApplyChangeOfBasis(FVector DestinationFrameForward, FVector DestinationFrameRight, FVector DestinationFrameUp, FTransform TransformToTranslateWorldFrame)
{
    // we are about to perform a change of basis on a transformation: from T to T_Ref
    // the formula for doing that is T_Ref = C_WR * T * C_WR_inverse

    // C_WR is the change of basis matrix - it's like translating a language:
    // it represents the World coordinate frame by using the axes of our destination ReferenceFrame


    //////////////////////// FINDING C_WR ////////////////////////

    // C_WR is what we need, but it's more convenient for us to compute the opposite, which is C_RW.
    // That would be the matrix describing the axes of our ReferenceFrame by using our World frame.
    // We can easily compute C_WR by inverting C_RW.
    // Also, we could transpose it, since it is an orthogonal matrix and the operations are the same

    // Let's get the info we need to create the C_RW matrix:
    // These are the forward, right and up directions of our ReferenceFrame, in World coordinates:
    // in this function, they are passed as arguments

    // Now let's use these unit vectors to build the 4x4 change of basis matrix C_RW:
    const FMatrix C_RW(FPlane(DestinationFrameForward[0], DestinationFrameForward[1], DestinationFrameForward[2], 0),
                       FPlane(DestinationFrameRight[0], DestinationFrameRight[1], DestinationFrameRight[2], 0),
                       FPlane(DestinationFrameUp[0], DestinationFrameUp[1], DestinationFrameUp[2], 0),
                       FPlane(0,       0,       0,       1));

    // Now we can finally compute the C_WR matrix:
    const FMatrix C_WR = C_RW.Inverse();

    ////////////////////////////////////////////////

  
	if(TransformToTranslateWorldFrame.IsValid())
	{
		//////////////////////// APPLYING THE CHANGE OF BASIS ////////////////////////

		// let's get the matrix for the transformation T we want to translate from World to ReferenceFrame
		const FMatrix T = TransformToTranslateWorldFrame.ToMatrixWithScale();

		// Let's perform the change of basis.s
		// Again, the formula is T_Ref = C_WR * T * C_WR_inverse
		const FMatrix T_Destination = C_WR*T*C_WR.Inverse();

		// let's go back from matrix to transform,
		// so we can easily retrieve location and rotation deltas as FVector and FQuat
		const FTransform Result(T_Destination);

		return Result;

		////////////////////////////////////////////////
	}

    // you could print some debug/error information here
    return FTransform::Identity;
}