Quaternion interpolation issue (FInterpCurve / Squad)

I’m trying to interpolate an animation keyframe path. Unreals Spline uses FInterpCurve in cpp to achive the spline behavior. Though rotation along the spline is always in spline direction and thus we can’t use it.

Should not be to difficult to make custom use of the FInterpCurve, so I set up some code:



...
case EAnimationInterpolation::AI_Curve:
{
float inval_before = 0.f;

const bool bClosedLoop = (keyframes.Num() > 2) && (keyframes[0].transform.Equals(keyframes.Last().transform));

// Gather transforms for interpolation
TArray<FTransform, TInlineAllocator<6>> transforms;
if (keyframes.IsValidIndex(beforeIndex - 1))
{
transforms.Add(pivot * keyframes[beforeIndex - 1].transform);
}
else if (bClosedLoop && beforeIndex == 0)
{
// For a proper loop, the previous for the first is the last-1.
transforms.Add(pivot * keyframes[keyframes.Num()-2].transform);
}
transforms.Add(pivot_before);
inval_before = transforms.Num() - 1;
transforms.Add(pivot_after);
if (keyframes.IsValidIndex(afterIndex + 1))
{
transforms.Add(pivot * keyframes[afterIndex + 1].transform);
}
else if (bClosedLoop && afterIndex == keyframes.Num() - 1)
{
// For a proper loop, the next for the last is the second.
transforms.Add(pivot * keyframes[1].transform);
}

// We get speedup at start and slowdown at the end. Also there is a weird rotation thing going on at the end of the animation.
// To fix that add imaginary transform at start and end of the animation
if (!bClosedLoop && transforms.Num() >= 2)
{
if (beforeIndex == 0)
{
FTransform previousImaginary = (transforms[0] * transforms[1].Inverse()) * transforms[1];
transforms.Insert(previousImaginary, 0);
inval_before += 1;
}

if (afterIndex == keyframes.Num() - 1)
{
FTransform nextImaginary = (transforms.Last() * transforms.Last(1).Inverse()) * transforms.Last();
transforms.Add(nextImaginary);
}
}

// Create curves
FInterpCurve<FVector> curveLocation;
FInterpCurve<FQuat> curveRotation;
FInterpCurve<FVector> curveScale;

for (int32 transIdx = 0; transIdx < transforms.Num(); ++transIdx)
{
curveLocation.AddPoint(transIdx, transforms[transIdx].GetLocation());
curveLocation.Points[transIdx].InterpMode = CIM_CurveAuto;

curveRotation.AddPoint(transIdx, transforms[transIdx].GetRotation());
curveRotation.Points[transIdx].InterpMode = CIM_CurveAuto;

curveScale.AddPoint(transIdx, transforms[transIdx].GetScale3D());
curveScale.Points[transIdx].InterpMode = CIM_CurveAuto;
}

// Tangents calculation must be called manually
curveLocation.AutoSetTangents);
curveRotation.AutoSetTangents(); // Note: parameter tension does not have effect for a quaternion curve
curveScale.AutoSetTangents();

// Get the interpolated pivot transform
const FVector pivot_interp_location = curveLocation.Eval(inval_before + alpha);
const FQuat pivot_interp_quat = curveRotation.Eval(inval_before + alpha);
const FVector pivot_interp_scale = curveScale.Eval(inval_before + alpha);
const FTransform pivot_interp(pivot_interp_quat, pivot_interp_location, pivot_interp_scale);

// From pivot transform back to object transform
// Note: Pivot is relative to the object
out.transform = pivot.Inverse() * pivot_interp;
...
break;



Here is the result:

As you can see it works for the most part, but there are these 2 weird wobbels in the recorded animation regarding rotation. Isn’t that exactly what Quaternions should prevent? What is it I am missing?
Internally the FQuat interpolation in FInterpCurve uses FQuat::Sqaud.
I’m trying different things for 3 weeks now to get rid of it, but I have no clue whats wrong.

Further investigation showed that the tangent calculation is causing issues.

The path:

Rotation visualization in the good part:
Rotation direction Red->Green->Blue with Yellow beeing the tangent in Green

Rotation visualization in the bad part:

As you can see: In the good part of the interpolation, the tangent is close to the other vectors. In the bad part, the tangent is totally of.

Tangent calulation:



// Note: What we calculate here a 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 = (curInv * PrevPoint).Log();
FQuat part2 = (curInv * NextPoint).Log();
OutTangent = CurPoint * ((part1 + part2) * -0.25f).Exp();


The calculation seems to be used in multiple documents (e.g link, page 51 equation 6.15). Though I’m not able to find any mentions of issues that could occour using that formula.

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;
}
}