And going even further, it turned out that FQuad::CalcTangent (or actually the FQuad::Log and FQuad::Exp functions used for tangent calculation) and FQuad::Squad are both problematic.
Here is an inheritance of FInterpCurve that fixes the issue of FQuat interpolation.
#pragma once
#include "Math/InterpCurve.h"
template <typename T>
class FInterpCurveAdv : public FInterpCurve<T>
{
public:
/**
* Copy of FInterpCurve::AutoSetTangents with modifications
*/
void AutoSetTangents(int32 fromPointIdx, int32 toPointIdx, float Tension = 0.f, bool bStationaryEndpoints = true);
// Generic function to just return the tangent unmodified
T AdjustTangentLength(const T& point1, const T& point2, const T& tangent)
{
return tangent;
}
/**
* Copy from FInterCurve::Eval with modifivations
*/
T Eval(const float InVal, const T& Default = T(ForceInit)) const;
private:
template< typename Type>
Type CubicInterp_Internal(const Type& P0, const Type& T0, const Type& P1, const Type& T1, const float& A) const
{
return FMath::CubicInterp(P0, T0, P1, T1, A);
}
template<>
FQuat CubicInterp_Internal<FQuat>(const FQuat& P0, const FQuat& T0, const FQuat& P1, const FQuat& T1, const float& A) const
{
return Squad_Fixed(P0, T0, P1, T1, A);
}
/**
* Given start and end quaternions of quat1 and quat2, and tangents at those points tang1 and tang2, calculate the point at Alpha (between 0 and 1) between them. Result is normalized.
* This will correct alignment by ensuring that the shortest path is taken.
*/
FQuat Squad_Fixed(const FQuat& quat1, const FQuat& tang1, const FQuat& quat2, const FQuat& tang2, float Alpha) const
{
const bool bUseUnrealSquad = false;
if (bUseUnrealSquad)
{
return FQuat::Squad(quat1, tang1, quat2, tang2, Alpha);
}
else
{
// Note: The tangents are final quaternions! See ComputeCurveTangent_Fixed<FQuat>
const FQuat q1 = quat1;
const FQuat q2 = tang1;
const FQuat q3 = tang2;
const FQuat q4 = quat2;
const FQuat Q1 = FQuat::Slerp(q1, q4, Alpha);
const FQuat Q2 = FQuat::Slerp(q2, q3, Alpha);
return FQuat::Slerp(Q1, Q2, 2.f * Alpha * (1.f - Alpha));
}
}
template< class T >
inline void ComputeCurveTangent_Fixed(float PrevTime, const T& PrevPoint,
float CurTime, const T& CurPoint,
float NextTime, const T& NextPoint,
float Tension,
bool bWantClamping,
T& OutTangent)
{
ComputeCurveTangent(PrevTime, PrevPoint,
CurTime, CurPoint,
NextTime, NextPoint,
Tension,
bWantClamping,
OutTangent);
}
template<>
inline void ComputeCurveTangent_Fixed(float PrevTime, const FVector& PrevPoint,
float CurTime, const FVector& CurPoint,
float NextTime, const FVector& NextPoint,
float Tension,
bool bWantClamping,
FVector& OutTangent)
{
/*ComputeCurveTangent(PrevTime, PrevPoint,
CurTime, CurPoint,
NextTime, NextPoint,
Tension,
bWantClamping,
OutTangent);*/
OutTangent = NextPoint - PrevPoint;
}
template<>
inline void ComputeCurveTangent_Fixed(float PrevTime, const FQuat& PrevPoint,
float CurTime, const FQuat& CurPoint,
float NextTime, const FQuat& NextPoint,
float Tension,
bool bWantClamping,
FQuat& OutTangent)
{
//ComputeCurveTangent(PrevTime, PrevPoint,
// CurTime, CurPoint,
// NextTime, NextPoint,
// Tension,
// bWantClamping,
// OutTangent);
//
const bool bUseUnrealTangentCalc = false;
if (bUseUnrealTangentCalc)
{
// FQuat::CalcTangent
FQuat curInv = CurPoint.Inverse();
FQuat part1 = (curInv * PrevPoint).Log();
FQuat part2 = (curInv * NextPoint).Log();
OutTangent = CurPoint * ((part1 + part2) * -0.5f).Exp();
}
else
{
// Log and Exp according to https://users.aalto.fi/~ssarkka/pub/quat.pdf
auto QuatLog = ](FQuat quat) -> FQuat
{
FVector axis(quat.X, quat.Y, quat.Z);
const float axisSize = axis.Size();
FVector resultAxis = axis / axisSize * FMath::Sin(axisSize);
FQuat result;
result.W = FMath::Loge(quat.Size());
result.X = resultAxis.X;
result.Y = resultAxis.Y;
result.Z = resultAxis.Z;
return result;
};
auto QuatExp = ](FQuat quat) -> FQuat
{
FVector axis(quat.X, quat.Y, quat.Z);
const float axisSize = axis.Size();
FVector resultAxis = axis / axisSize * FMath::Sin(axisSize);
FQuat result;
result.W = FMath::Cos(axisSize);
result.X = resultAxis.X;
result.Y = resultAxis.Y;
result.Z = resultAxis.Z;
return result * FMath::Exp(quat.W);
};
// Note: What we calculate here are actually final quaternions and not tangents (deltas) as they are for locations.
// https://books.google.de/books?id=h0NZDwAAQBAJ&pg=PA99&lpg=PA99&dq=quaternions+squad+issues&source=bl&ots=4lkZUbw8Qg&sig=ACfU3U0xUZFssRLWtpzYv9c83RehKvtHpA&hl=de&sa=X&ved=2ahUKEwiWtP_BlfXpAhWKyqYKHa0cBzAQ6AEwAXoECAwQAQ#v=onepage&q=quaternion%20inverse&f=false
// Page 98
FQuat curInv = CurPoint.Inverse();
FQuat part1 = QuatLog(curInv * PrevPoint);
FQuat part2 = QuatLog(curInv * NextPoint);
OutTangent = CurPoint * QuatExp((part1 + part2) * -0.25f);
OutTangent.Normalize();
}
}
};
template <>
FVector FInterpCurveAdv<FVector>::AdjustTangentLength(const FVector& point1, const FVector& point2, const FVector& tangent)
{
// The base class divides the tangent by 'PrevToNextTimeDiff'. This is basically useless as the results can get pretty ugly when
// one of the neighbor points is much further away than the other.
// NOTE! This is automatically eliminated by giving the tangent a fixed size below
/*const float PrevToNextTimeDiff = FMath::Max< double >(KINDA_SMALL_NUMBER, NextTime - PrevTime);
OutTangent /= PrevToNextTimeDiff;*/
// Apply a proper scaling:
// The tangent length shall be the distance between the two points
float tangentSize = tangent.Size();
if (FMath::IsNearlyZero(tangentSize))
{
return tangent;
}
return tangent * (FVector::Dist(point1, point2) / tangent.Size());
}
template <typename T>
void FInterpCurveAdv<T>::AutoSetTangents(int32 fromPointIdx, int32 toPointIdx, float Tension, bool bStationaryEndpoints)
{
if (!Points.IsValidIndex(fromPointIdx))
{
return;
}
const int32 NumPoints = Points.Num();
const int32 LastPoint = NumPoints - 1;
const int32 loopMax = Points.IsValidIndex(toPointIdx) ? toPointIdx : LastPoint;
// Iterate over all points in this InterpCurve
for (int32 PointIndex = fromPointIdx; PointIndex <= loopMax; PointIndex++)
{
const int32 PrevIndex = (PointIndex == 0) ? (bIsLooped ? LastPoint : 0) : (PointIndex - 1);
const int32 NextIndex = (PointIndex == LastPoint) ? (bIsLooped ? 0 : LastPoint) : (PointIndex + 1);
auto& ThisPoint = Points[PointIndex];
const auto& PrevPoint = Points[PrevIndex];
const auto& NextPoint = Points[NextIndex];
if (ThisPoint.InterpMode == CIM_CurveAuto || ThisPoint.InterpMode == CIM_CurveAutoClamped)
{
if (bStationaryEndpoints && (PointIndex == 0 || (PointIndex == LastPoint && !bIsLooped)))
{
// Start and end points get zero tangents if bStationaryEndpoints is true
ThisPoint.ArriveTangent = T(ForceInit);
ThisPoint.LeaveTangent = T(ForceInit);
}
else if (PrevPoint.IsCurveKey())
{
const bool bWantClamping = (ThisPoint.InterpMode == CIM_CurveAutoClamped);
T Tangent;
const float PrevTime = (bIsLooped && PointIndex == 0) ? (ThisPoint.InVal - LoopKeyOffset) : PrevPoint.InVal;
const float NextTime = (bIsLooped && PointIndex == LastPoint) ? (ThisPoint.InVal + LoopKeyOffset) : NextPoint.InVal;
ComputeCurveTangent_Fixed(
PrevTime, // Previous time
PrevPoint.OutVal, // Previous point
ThisPoint.InVal, // Current time
ThisPoint.OutVal, // Current point
NextTime, // Next time
NextPoint.OutVal, // Next point
Tension, // Tension
bWantClamping, // Want clamping?
Tangent); // Out
ThisPoint.ArriveTangent = AdjustTangentLength(PrevPoint.OutVal, ThisPoint.OutVal, Tangent);
ThisPoint.LeaveTangent = AdjustTangentLength(ThisPoint.OutVal, NextPoint.OutVal, Tangent);
}
else
{
// Following on from a line or constant; set curve tangent equal to that so there are no discontinuities
ThisPoint.ArriveTangent = PrevPoint.ArriveTangent;
ThisPoint.LeaveTangent = PrevPoint.LeaveTangent;
}
}
else if (ThisPoint.InterpMode == CIM_Linear)
{
T Tangent = NextPoint.OutVal - ThisPoint.OutVal;
ThisPoint.ArriveTangent = Tangent;
ThisPoint.LeaveTangent = Tangent;
}
else if (ThisPoint.InterpMode == CIM_Constant)
{
ThisPoint.ArriveTangent = T(ForceInit);
ThisPoint.LeaveTangent = T(ForceInit);
}
}
}
template< class T >
T FInterpCurveAdv<T>::Eval(const float InVal, const T& Default) const
{
const int32 NumPoints = Points.Num();
const int32 LastPoint = NumPoints - 1;
// If no point in curve, return the Default value we passed in.
if (NumPoints == 0)
{
return Default;
}
// Binary search to find index of lower bound of input value
const int32 Index = GetPointIndexForInputValue(InVal);
// If before the first point, return its value
if (Index == -1)
{
return Points[0].OutVal;
}
// If on or beyond the last point, return its value.
if (Index == LastPoint)
{
if (!bIsLooped)
{
return Points[LastPoint].OutVal;
}
else if (InVal >= Points[LastPoint].InVal + LoopKeyOffset)
{
// Looped spline: last point is the same as the first point
return Points[0].OutVal;
}
}
// Somewhere within curve range - interpolate.
check(Index >= 0 && ((bIsLooped && Index < NumPoints) || (!bIsLooped && Index < LastPoint)));
const bool bLoopSegment = (bIsLooped && Index == LastPoint);
const int32 NextIndex = bLoopSegment ? 0 : (Index + 1);
const auto& PrevPoint = Points[Index];
const auto& NextPoint = Points[NextIndex];
const float Diff = bLoopSegment ? LoopKeyOffset : (NextPoint.InVal - PrevPoint.InVal);
if (Diff > 0.0f && PrevPoint.InterpMode != CIM_Constant)
{
const float Alpha = (InVal - PrevPoint.InVal) / Diff;
check(Alpha >= 0.0f && Alpha <= 1.0f);
if (PrevPoint.InterpMode == CIM_Linear)
{
return FMath::Lerp(PrevPoint.OutVal, NextPoint.OutVal, Alpha);
}
else
{
//FMath::CubicInterp
return CubicInterp_Internal(PrevPoint.OutVal, PrevPoint.LeaveTangent * Diff, NextPoint.OutVal, NextPoint.ArriveTangent * Diff, Alpha);
}
}
else
{
return Points[Index].OutVal;
}
}