FrameTypes.h

Go to the documentation of this file.

// Copyright Epic Games, Inc. All Rights Reserved.
 
 
#pragma once
 
#include "VectorTypes.h"
#include "VectorUtil.h"
#include "Quaternion.h"
#include "TransformTypes.h"
 
namespace UE
{
namespace Geometry
{
 
using namespace UE::Math;
 
template<typename RealType>
struct TFrame3
{
    TVector<RealType> Origin;
 
    TQuaternion<RealType> Rotation;
 
    TFrame3()
    {
        Origin = TVector<RealType>::Zero();
        Rotation = TQuaternion<RealType>::Identity();
    }
 
    explicit TFrame3(const TVector<RealType>& OriginIn)
    {
        Origin = OriginIn;
        Rotation = TQuaternion<RealType>::Identity();
    }
 
    TFrame3(const TVector<RealType>& OriginIn, const TQuaternion<RealType> RotationIn)
    {
        Origin = OriginIn;
        Rotation = RotationIn;
    }
 
    TFrame3(const TVector<RealType>& OriginIn, const TVector<RealType>& SetZ)
    {
        Origin = OriginIn;
        Rotation.SetFromTo(TVector<RealType>::UnitZ(), SetZ);
    }
 
    TFrame3(const TVector<RealType>& OriginIn, const TVector<RealType>& X, const TVector<RealType>& Y, const TVector<RealType>& Z)
    {
        Origin = OriginIn;
        Rotation = TQuaternion<RealType>( TMatrix3<RealType>(X, Y, Z, false) );
    }
 
    explicit TFrame3(const FTransform& Transform)
    {
        Origin = TVector<RealType>(Transform.GetTranslation());
        Rotation = TQuaternion<RealType>(Transform.GetRotation());
    }
 
    explicit TFrame3(const FPlane& Plane)
    {
        FVector Normal(Plane.X, Plane.Y, Plane.Z);
        Origin = (RealType)Plane.W * TVector<RealType>(Normal);
        Rotation.SetFromTo(TVector<RealType>::UnitZ(), (TVector<RealType>)Normal);
    }
 
 
    explicit TFrame3(const FVector& OriginIn, const FQuat& RotationIn)
    {
        Origin = TVector<RealType>(OriginIn);
        Rotation = TQuaternion<RealType>(RotationIn);
    }
 
    template<typename RealType2>
    explicit TFrame3(const TFrame3<RealType2>& OtherFrame)
    {
        Origin = static_cast<TVector<RealType>>(OtherFrame.Origin);
        Rotation = static_cast<TQuaternion<RealType>>(OtherFrame.Rotation);
    }
 
    
    TVector<RealType> GetAxis(int AxisIndex) const
    {
        switch (AxisIndex)
        {
        case 0:
            return Rotation.AxisX();
        case 1:
            return Rotation.AxisY();
        case 2:
            return Rotation.AxisZ();
        default:
            checkNoEntry();
            return TVector<RealType>::Zero(); // compiler demands a return value
        }
    }
 
    void GetAxes(TVector<RealType>& X, TVector<RealType>& Y, TVector<RealType>& Z) const
    {
        Rotation.GetAxes(X, Y, Z);
    }
 
    TVector<RealType> X() const
    {
        return Rotation.AxisX();
    }
 
    TVector<RealType> Y() const
    {
        return Rotation.AxisY();
    }
 
    TVector<RealType> Z() const
    {
        return Rotation.AxisZ();
    }
 
    FTransform ToFTransform() const
    {
        return FTransform((FQuat)Rotation, (FVector)Origin);
    }
 
    FTransform ToInverseFTransform() const
    {
        TQuaternion<RealType> InverseRotation = Rotation.Inverse();
        return FTransform((FQuat)InverseRotation, (FVector)(InverseRotation * (-Origin)));
    }
 
    FPlane ToFPlane() const
    {
        return FPlane((FVector)Origin, (FVector)Z());
    }
 
    TTransformSRT3<RealType> ToTransform() const
    {
        return TTransformSRT3<RealType>(Rotation, Origin);
    }
 
    TTransformSRT3<RealType> ToInverseTransform() const
    {
        TQuaternion<RealType> InverseRotation = Rotation.Inverse();
        return TTransformSRT3<RealType>(InverseRotation, InverseRotation * (-Origin));
    }
 
    TVector<RealType> PointAt(RealType X, RealType Y, RealType Z) const
    {
        return Rotation * TVector<RealType>(X,Y,Z) + Origin;
    }
 
    TVector<RealType> PointAt(const TVector<RealType>& Point) const
    {
        return Rotation * TVector<RealType>(Point.X, Point.Y, Point.Z) + Origin;
    }
    
    TVector<RealType> ToFramePoint(const TVector<RealType>& Point) const
    {
        return Rotation.InverseMultiply((Point-Origin));
    }
    TVector<RealType> FromFramePoint(const TVector<RealType>& Point) const
    {
        return Rotation * Point + Origin;
    }
 
 
    TVector<RealType> ToFrameVector(const TVector<RealType>& Vector) const
    {
        return Rotation.InverseMultiply(Vector);
    }
    TVector<RealType> FromFrameVector(const TVector<RealType>& Vector) const
    {
        return Rotation * Vector;
    }
 
 
    TQuaternion<RealType> ToFrame(const TQuaternion<RealType>& Quat) const
    {
        return Rotation.Inverse() * Quat;
    }
    TQuaternion<RealType> FromFrame(const TQuaternion<RealType>& Quat) const
    {
        return Rotation * Quat;
    }
 
 
    TRay<RealType> ToFrame(const TRay<RealType>& Ray) const
    {
        return TRay<RealType>(ToFramePoint(Ray.Origin), ToFrameVector(Ray.Direction));
    }
    TRay<RealType> FromFrame(const TRay<RealType>& Ray) const
    {
        return TRay<RealType>(FromFramePoint(Ray.Origin), FromFrameVector(Ray.Direction));
    }
 
 
    TFrame3<RealType> ToFrame(const TFrame3<RealType>& Frame) const
    {
        return TFrame3<RealType>(ToFramePoint(Frame.Origin), ToFrame(Frame.Rotation));
    }
    TFrame3<RealType> FromFrame(const TFrame3<RealType>& Frame) const
    {
        return TFrame3<RealType>(FromFramePoint(Frame.Origin), FromFrame(Frame.Rotation));
    }
 
 
 
 
    TVector2<RealType> ToPlaneUV(const TVector<RealType>& Pos, int PlaneNormalAxis = 2) const
    {
        int Axis0 = 0, Axis1 = 1;
        if (PlaneNormalAxis == 0)
        {
            Axis0 = 2;
        }
        else if (PlaneNormalAxis == 1)
        {
            Axis1 = 2;
        }
        TVector<RealType> LocalPos = Pos - Origin;
        RealType U = LocalPos.Dot(GetAxis(Axis0));
        RealType V = LocalPos.Dot(GetAxis(Axis1));
        return TVector2<RealType>(U, V);
    }
 
 
 
    TVector<RealType> FromPlaneUV(const TVector2<RealType>& PosUV, int PlaneNormalAxis = 2) const
    {
        TVector<RealType> PlanePos(PosUV[0], PosUV[1], 0);
        if (PlaneNormalAxis == 0)
        {
            PlanePos[0] = 0; PlanePos[2] = PosUV[0];
        }
        else if (PlaneNormalAxis == 1)
        {
            PlanePos[1] = 0; PlanePos[2] = PosUV[1];
        }
        return Rotation*PlanePos + Origin;
    }
 
 
 
    TVector<RealType> ToPlane(const TVector<RealType>& Pos, int PlaneNormalAxis = 2) const
    {
        TVector<RealType> Normal = GetAxis(PlaneNormalAxis);
        TVector<RealType> LocalVec = Pos - Origin;
        RealType SignedDist = LocalVec.Dot(Normal);
        return Pos - SignedDist * Normal;
    }
 
 
 
 
    void Rotate(const TQuaternion<RealType>& Quat)
    {
        TQuaternion<RealType> NewRotation = Quat * Rotation;
        if (NewRotation.Normalize() > 0 )
        {
            Rotation = NewRotation;
        }
        else
        {
            checkSlow(false);
        }
    }
 
 
    void Transform(const FTransform& XForm)
    {
        Origin = TVector<RealType>(XForm.TransformPosition((FVector)Origin));
        Rotate(TQuaternion<RealType>(XForm.GetRotation()));
    }
 
 
    void Transform(const TTransformSRT3<RealType>& XForm)
    {
        Origin = XForm.TransformPosition(Origin);
        Rotate(XForm.GetRotation());
    }
 
 
    void AlignAxis(int AxisIndex, const TVector<RealType>& ToDirection)
    {
        TQuaternion<RealType> RelRotation(GetAxis(AxisIndex), ToDirection);
        Rotate(RelRotation);
    }
 
 
    void ConstrainedAlignAxis(int AxisIndex, const TVector<RealType>& ToDirection, const TVector<RealType>& AroundVector)
    {
        //@todo PlaneAngleSigned does acos() and then SetAxisAngleD() does cos/sin...can we optimize this?
        TVector<RealType> AxisVec = GetAxis(AxisIndex);
        RealType AngleDeg = VectorUtil::PlaneAngleSignedD(AxisVec, ToDirection, AroundVector);
        TQuaternion<RealType> RelRotation;
        RelRotation.SetAxisAngleD(AroundVector, AngleDeg);
        Rotate(RelRotation);
    }
 
 
    void ConstrainedAlignPerpAxes(int PerpAxis1 = 0, int PerpAxis2 = 1, int NormalAxis = 2, 
        const TVector<RealType>& UpAxis = TVector<RealType>::UnitZ(),
        const TVector<RealType>& FallbackAxis = TVector<RealType>::UnitX(),
        RealType UpDotTolerance = (RealType)0.707)
    {
        check(PerpAxis1 != PerpAxis2 && PerpAxis1 != NormalAxis && PerpAxis2 != NormalAxis);
        const TVector<RealType> NormalVec = GetAxis(NormalAxis);
 
        // decide if we should use Fallback (polar-cap) axis or main (equator-region) axis
        const TVector<RealType>& TargetAxis =
            (TMathUtil<RealType>::Abs(NormalVec.Dot(UpAxis)) > UpDotTolerance) ?
            FallbackAxis : UpAxis;
 
        // figure out which PerpAxis is closer to target, and align that one to +/- TargetAxis (whichever is smaller rotation)
        TVector2<RealType> Dots(GetAxis(PerpAxis1).Dot(TargetAxis), GetAxis(PerpAxis2).Dot(TargetAxis));
        int UseAxis = (TMathUtil<RealType>::Abs(Dots.X) > TMathUtil<RealType>::Abs(Dots.Y)) ? 0 : 1;
        RealType UseSign = Dots[UseAxis] < 0 ? -1 : 1;
        ConstrainedAlignAxis(UseAxis, UseSign*TargetAxis, NormalVec);
    }
 
 
    bool RayPlaneIntersection(const TVector<RealType>& RayOrigin, const TVector<RealType>& RayDirection, int PlaneNormalAxis, UE::Math::TVector<RealType>& HitPointOut) const
    {
        TVector<RealType> Normal = GetAxis(PlaneNormalAxis);
        RealType PlaneD = -Origin.Dot(Normal);
        RealType NormalDot = RayDirection.Dot(Normal);
        if (VectorUtil::EpsilonEqual(NormalDot, (RealType)0, TMathUtil<RealType>::ZeroTolerance))
        {
            HitPointOut = TVector<RealType>(TNumericLimits<RealType>::Max(), TNumericLimits<RealType>::Max(), TNumericLimits<RealType>::Max());
            return false;
        }
        RealType t = -( RayOrigin.Dot(Normal) + PlaneD) / NormalDot;
        if (t < 0)
        {
            HitPointOut = TVector<RealType>(TNumericLimits<RealType>::Max(), TNumericLimits<RealType>::Max(), TNumericLimits<RealType>::Max());
            return false;
        }
        HitPointOut = RayOrigin + t * RayDirection;
        return true;
    }
 
};
 
template <typename T>
TFrame3<T> Lerp(const TFrame3<T>& A, const TFrame3<T>& B, T Alpha)
{
    return TFrame3<T>(
        UE::Geometry::Lerp(A.Origin, B.Origin, Alpha),
        TQuaternion<T>(A.Rotation, B.Rotation, Alpha));
}
 
 
 
typedef TFrame3<float> FFrame3f;
typedef TFrame3<double> FFrame3d;
 
} // end namespace UE::Geometry
} // end namespace UE