Skip to main content

performance tuning - Fast Spherical Linear Interpolation of list of quaternions


An accurate way to interpolate between two quaternions is to use Spherical Linear Interpolation (Slerp) because it preserves the unit length, whereas straightforward linear interpolation does not, as shown by this example:


Clear[slerp];
slerp[q1_, q2_, f_] := Module[{omega},

  omega = ArcCos[Dot[Normalize@q1, Normalize@q2]]];
  If[PossibleZeroQ[omega], 
    q1,
    (Sin[(1 - f) omega] q1 + Sin[f omega] q2)/Sin[omega]
  ]
]

q1 = {1,0,0,0};
q2 = {0,1,0,0};
q3 = {0,0,1,0};


(* Linear interpolation using Interpolation *)
qint = Interpolation[{{0,q1},{1,q2}},InterpolationOrder->1];

Plot[{Norm@qint[t], Norm@slerp[{1, 0, 0, 0}, {0, 1, 0, 0}, t]}, {t, 0, 1}, 
  AxesLabel -> {"time", "length"}, PlotRange -> {0.7, 1.01}, 
  PlotStyle -> {Automatic, Dashed}, 
  Epilog -> {Text["Linear Interpolation", {0.5, 0.75}], 
  Text["Spherical Linear Interpolation", {0.5, 0.98}]}
]


Mathematica graphics


What's nice thing about Interpolation, however, is that it is easy to give it a list of vectors to get an interpolating function valid over the specified range, and it works fast:


Interpolation[{{0,q1},{1,q2},{2,q3}},InterpolationOrder->1]


InterpolatingFunction[{{0,2}},<>]



How can I create something like an InterpolatingFunction that preserves the unit-length property?


This is what I came up with, but it seems kludgy and it is very slow (this code does not check input bounds):



Clear[slerpInterpolation]; 
slerpInterpolation[q_List] := Function[{t}, 
  Module[{times, quaternions, pos, u, dt},
    times = q[[All, 1]];
    quaternions = q[[All, 2]];
    pos = Last@Flatten@Position[t - times, x_ /; x >= 0];
    dt = times[[pos + 1]] - times[[pos]];
    u = (t - times[[pos]])/dt;
    slerp[quaternions[[pos]], quaternions[[pos + 1]], u]
  ]

]

It is too slow in practice, giving about 10 evaluations per second on my laptop. Compare that with regular interpolation:


qlist = Transpose[{Range[100000] - 1, Normalize /@ RandomReal[{-1, 1}, {100000, 4}]}];
sint = slerpInterpolation[qlist];
lint = Interpolation[qlist, InterpolationOrder -> 1];

AbsoluteTiming[Do[sint[541.236], {10}]][[1]]/10



0.0967707



AbsoluteTiming[Do[lint[541.236], {10000}]][[1]]/10000


7.2522*10^-6




Answer



Here's a somewhat complete implementation of Shoemake's spherical linear interpolation that functions completely analogously to Interpolation[] and InterpolatingFunction[]. As already noted, much of the slowness is due to your use of a sequential search. In any event, if you prefer, you could use the built-in interpolation as suggested in the other answer, but I think using a bisection routine is a bit more instructive.


Anyway...



SphericalLinearInterpolation::inddp = "The point `1` is duplicated.";

SphericalLinearInterpolation[data_] := 
 Module[{dtr = Transpose[SortBy[data, Composition[N, First]]], diffs, times},
        SphericalInterpolatingFunction[data[[{1, -1}, 1]], dtr] /; 
        If[MemberQ[diffs = Chop[Differences[times = First[dtr]]], 0], 
           Message[SphericalLinearInterpolation::inddp, 
                   First[Extract[times, Position[diffs, 0]]]]; False, True]]

SphericalInterpolatingFunction::dmval = 

  "Input value `1` lies outside the domain of the interpolating function.";

MakeBoxes[SphericalInterpolatingFunction[range_, rest__], _] ^:= 
 InterpretationBox[RowBox[
    {"SphericalInterpolatingFunction", "[", "{", #1, ",", #2, "}", ",", "\"<>\"", "]"}], 
    SphericalInterpolatingFunction[range, rest]] & @@ Map[ToBoxes, range]

SphericalInterpolatingFunction[stuff__][l_List] :=
 SphericalInterpolatingFunction[stuff] /@ l


SphericalInterpolatingFunction[range_, __]["Domain"] := range

SphericalInterpolatingFunction[{r_, s_}, data_][t_?NumericQ] :=
  (Message[SphericalInterpolatingFunction::dmval, t]; $Failed) /; ! (r <= t <= s)

slerp = Compile[{{q1, _Real, 1}, {q2, _Real, 1}, {f, _Real}}, 
   Module[{n1 = Norm[q1], n2 = Norm[q2], omega, so},
    (* vector angle formula by Velvel Kahan *)
    omega = 2 ArcTan[Norm[q1 n2 + n1 q2], Norm[q1 n2 - n1 q2]];
    If[Chop[so = Sin[omega]] == 0, q1, Sin[{1 - f, f} omega].{q1, q2}/so]]];    


SphericalInterpolatingFunction[range_, data_][t_?NumericQ] := Module[{times, quats, k},
        {times, quats} = data;
        k = GeometricFunctions`BinarySearch[times, t];
        slerp[quats[[k]], quats[[k + 1]], Rescale[t, times[[{k, k + 1}]], {0, 1}]]]


See J. Blow's article on why spherical linear interpolation might not always be the best method for interpolating quaternions.


Comments

Post a Comment

Popular posts from this blog

mathematical optimization - Minimizing using indices, error: Part::pkspec1: The expression cannot be used as a part specification

I want to use Minimize where the variables to minimize are indices pointing into an array. Here a MWE that hopefully shows what my problem is. vars = u@# & /@ Range[3]; cons = Flatten@ { Table[(u[j] != #) & /@ vars[[j + 1 ;; -1]], {j, 1, 3 - 1}], 1 vec1 = {1, 2, 3}; vec2 = {1, 2, 3}; Minimize[{Total@((vec1[[#]] - vec2[[u[#]]])^2 & /@ Range[1, 3]), cons}, vars, Integers] The error I get: Part::pkspec1: The expression u[1] cannot be used as a part specification. >> Answer Ok, it seems that one can get around Mathematica trying to evaluate vec2[[u[1]]] too early by using the function Indexed[vec2,u[1]] . The working MWE would then look like the following: vars = u@# & /@ Range[3]; cons = Flatten@{ Table[(u[j] != #) & /@ vars[[j + 1 ;; -1]], {j, 1, 3 - 1}], 1 vec1 = {1, 2, 3}; vec2 = {1, 2, 3}; NMinimize[ {Total@((vec1[[#]] - Indexed[vec2, u[#]])^2 & /@ R...
Post a Comment
Read more

functions - Get leading series expansion term?

Given a function f[x] , I would like to have a function leadingSeries that returns just the leading term in the series around x=0 . For example: leadingSeries[(1/x + 2)/(4 + 1/x^2 + x)] x and leadingSeries[(1/x + 2 + (1 - 1/x^3)/4)/(4 + x)] -(1/(16 x^3)) Is there such a function in Mathematica? Or maybe one can implement it efficiently? EDIT I finally went with the following implementation, based on Carl Woll 's answer: lds[ex_,x_]:=( (ex/.x->(x+O[x]^2))/.SeriesData[U_,Z_,L_List,Mi_,Ma_,De_]:>SeriesData[U,Z,{L[[1]]},Mi,Mi+1,De]//Quiet//Normal) The advantage is, that this one also properly works with functions whose leading term is a constant: lds[Exp[x],x] 1 Answer Update 1 Updated to eliminate SeriesData and to not return additional terms Perhaps you could use: leadingSeries[expr_, x_] := Normal[expr /. x->(x+O[x]^2) /. a_List :> Take[a, 1]] Then for your examples: leadingSeries[(1/x + 2)/(4 + 1/x^2 + x), x] leadingSeries[Exp[x], x] leadingSeries[(1/x + 2 + (1 - 1/x...
Post a Comment
Read more

What is and isn't a valid variable specification for Manipulate?

I have an expression whose terms have arguments (representing subscripts), like this: myExpr = A[0] + V[1,T] I would like to put it inside a Manipulate to see its value as I move around the parameters. (The goal is eventually to plot it wrt one of the variables inside.) However, Mathematica complains when I set V[1,T] as a manipulated variable: Manipulate[Evaluate[myExpr], {A[0], 0, 1}, {V[1, T], 0, 1}] (*Manipulate::vsform: Manipulate argument {V[1,T],0,1} does not have the correct form for a variable specification. >> *) As a workaround, if I get rid of the symbol T inside the argument, it works fine: Manipulate[ Evaluate[myExpr /. T -> 15], {A[0], 0, 1}, {V[1, 15], 0, 1}] Why this behavior? Can anyone point me to the documentation that says what counts as a valid variable? And is there a way to get Manpiulate to accept an expression with a symbolic argument as a variable? Investigations I've done so far: I tried using variableQ from this answer , but it says V[1...
Post a Comment
Read more