2.9 Animating Transformations

pbrt supports keyframe matrix animation for cameras and geometric primitives in the scene. Rather than just supplying a single transformation to place the corresponding object in the scene, the user may supply a number of keyframe transformations, each one associated with a particular point in time. This makes it possible for the camera to move and for objects in the scene to be moving during the time the simulated camera’s shutter is open. Figure 2.15 shows three spheres animated using keyframe matrix animation in pbrt.

In general, the problem of interpolating between keyframe matrices is under-defined. As one example, if we have a rotation about the axis of 181 degrees followed by another of 181 degrees, does this represent a small rotation of 2 degrees or a large rotation of degrees? For another example, consider two matrices where one is the identity and the other is a 180-degree rotation about the axis. There are an infinite number of ways to go from one orientation to the other.

Keyframe matrix interpolation is an important problem in computer animation, where a number of different approaches have been developed. Fortunately, the problem of matrix interpolation in a renderer is generally less challenging than it is for animation systems for two reasons.

First, in a renderer like pbrt, we generally have a keyframe matrix at the camera shutter open time and another at the shutter close time; we only need to interpolate between the two of them across the time of a single image. In animation systems, the matrices are generally available at a lower time frequency, so that there are many frames between pairs of keyframe matrices; as such, there’s more opportunity to notice shortcomings in the interpolation.

Second, in a physically based renderer, the longer the period of time over which we need to interpolate the pair of matrices, the longer the virtual camera shutter is open and the more motion blur there will be in the final image; the increased amount of motion blur often hides sins of the interpolation.

The most straightforward approach to interpolate transformations defined by keyframe matrices—directly interpolating the individual components of the matrices—is not a good one, as it will generally lead to unexpected and undesirable results. For example, if the transformations apply different rotations, then even if we have a rigid-body motion, the intermediate matrices may scale the object, which is clearly undesirable. (If the matrices have a full 180-degree rotation between them, the object may be scaled down to nothing at the middle of the interpolation!)

Figure 2.16 shows a sphere that rotates 90 degrees over the course of the frame; direct interpolation of matrix elements gives a less accurate result than the approach implemented in this section.

The approach used for transformation interpolation in pbrt is based on matrix decomposition—given an arbitrary transformation matrix , we decompose it into a concatenation of scale (), rotation (), and translation () transformations,

where each of those components is independently interpolated and then the composite interpolated matrix is found by multiplying the three interpolated matrices together.

Interpolation of translation and scale can be performed easily and accurately with linear interpolation of the components of their matrices; interpolating rotations is more difficult. Before describing the matrix decomposition implementation in pbrt, we will first introduce quaternions, an elegant representation of rotations that leads to effective methods for interpolating them.

2.9.1 Quaternions

Quaternions were originally invented by Sir William Rowan Hamilton in 1843 as a generalization of complex numbers. He determined that just as in two dimensions , where complex numbers could be defined as a sum of a real and an imaginary part , with , a generalization could be made to four dimensions, giving quaternions.

A quaternion is a four-tuple,

(2.4)

where , , and are defined so that . Other important relationships between the components are that and . This implies that quaternion multiplication is generally not commutative.

A quaternion can be represented as a quadruple or as , where is an imaginary 3-vector and is the real part. We will use both representations interchangeably in this section.

An expression for the product of two arbitrary quaternions can be found by expanding their definition in terms of real and imaginary components:

Collecting terms and using identities among the components like those listed above (e.g., ), the result can be expressed concisely using vector cross and dot products:

(2.5)

There is a useful relationship between unit quaternions (quaternions whose components satisfy ) and the space of rotations in : specifically, a rotation of angle about a unit axis can be mapped to a unit quaternion , in which case the following quaternion product is equivalent to applying the rotation to a point expressed in homogeneous coordinate form:

Furthermore, the product of several rotation quaternions produces another quaternion that is equivalent to applying the rotations in sequence.

The implementation of the Quaternion class in pbrt is in the files core/quaternion.h and core/quaternion.cpp. The default constructor initializes a unit quaternion.

<<Quaternion Public Methods>>=
Quaternion() : v(0, 0, 0), w(1) { }

We use a Vector3f to represent the components of the quaternion; doing so lets us make use of various methods of Vector3f in the implementation of some of the methods below.

<<Quaternion Public Data>>=
Vector3f v; Float w;

Addition and subtraction of quaternions is performed component-wise. This follows directly from the definition in Equation (2.4). For example,

Other arithmetic methods (subtraction, multiplication, and division by a scalar) are defined and implemented similarly and won’t be included here.

<<Quaternion Public Methods>>+=
Quaternion &operator+=(const Quaternion &q) { v += q.v; w += q.w; return *this; }

The inner product of two quaternions is implemented by its Dot() method, and a quaternion can be normalized by dividing by its length.

<<Quaternion Inline Functions>>=
inline Float Dot(const Quaternion &q1, const Quaternion &q2) { return Dot(q1.v, q2.v) + q1.w * q2.w; }

<<Quaternion Inline Functions>>+=
inline Quaternion Normalize(const Quaternion &q) { return q / std::sqrt(Dot(q, q)); }

It’s useful to be able to compute the transformation matrix that represents the same rotation as a quaternion. In particular, after interpolating rotations with quaternions in the AnimatedTransform class, we’ll need to convert the interpolated rotation back to a transformation matrix to compute the final composite interpolated transformation.

To derive the rotation matrix for a quaternion, recall that the transformation of a point by a quaternion is given by . We want a matrix that performs the same transformation, so that . If we expand out the quaternion multiplication using Equation (2.5), simplify with the quaternion basis identities, collect terms, and represent the result in a matrix, we can determine that the following matrix represents the same transformation:

(2.6)

This computation is implemented in the method Quaternion::ToTransform(). We won’t include its implementation here since it’s a direct implementation of Equation (2.6).

<<Quaternion Public Methods>>+=
Transform ToTransform() const;

Note that we could alternatively use the fact that a unit quaternion represents a rotation of angle around the unit axis to compute a rotation matrix. First we would compute the angle of rotation as , and then we’d use the previously defined Rotate() function, passing it the axis and the rotation angle . However, this alternative would be substantially less efficient, requiring multiple calls to trigonometric functions, while the approach implemented here only uses floating-point addition, subtraction, and multiplication.

It is also useful to be able to create a quaternion from a rotation matrix. For this purpose, Quaternion provides a constructor that takes a Transform. The appropriate quaternion can be computed by making use of relationships between elements of the rotation matrix in Equation (2.6) and quaternion components. For example, if we subtract the transpose of this matrix from itself, then the component of the resulting matrix has the value . Thus, given a particular instance of a rotation matrix with known values, it’s possible to use a number of relationships like this between the matrix values and the quaternion components to generate a system of equations that can be solved for the quaternion components.

We won’t include the details of the derivation or the actual implementation here in the text; for more information about how to derive this technique, including handling numerical robustness, see Shoemake (1991).

<<Quaternion Public Methods>>+=
Quaternion(const Transform &t);

2.9.2 Quaternion Interpolation

The last quaternion function we will define, Slerp(), interpolates between two quaternions using spherical linear interpolation. Spherical linear interpolation gives constant speed motion along great circle arcs on the surface of a sphere and consequently has two desirable properties for interpolating rotations:

• The interpolated rotation path exhibits torque minimization: the path to get between two rotations is the shortest possible path in rotation space.
• The interpolation has constant angular velocity: the relationship between change in the animation parameter and the change in the resulting rotation is constant over the course of interpolation (in other words, the speed of interpolation is constant across the interpolation range).

See the “Further Reading” section at the end of the chapter for references that discuss more thoroughly what characteristics a good interpolated rotation should have.

Spherical linear interpolation for quaternions was originally presented by Shoemake (1985) as follows, where two quaternions and are given and is the parameter value to interpolate between them:

An intuitive way to understand Slerp() was presented by Blow (2004). As context, given the quaternions to interpolate between, and , denote by the angle between them. Then, given a parameter value , we’d like to find the intermediate quaternion that makes angle between it and , along the path from to .

An easy way to compute is to first compute an orthogonal coordinate system in the space of quaternions where one axis is and the other is a quaternion orthogonal to such that the two axes form a basis that spans and . Given such a coordinate system, we can compute rotations with respect to . (See Figure 2.17, which illustrates the concept in the 2D setting.) An orthogonal vector can be found by projecting onto and then subtracting the orthogonal projection from ; the remainder is guaranteed to be orthogonal to :

(2.7)

Given the coordinate system and a normalized , quaternions along the animation path are given by

(2.8)

The implementation of the Slerp() function checks to see if the two quaternions are nearly parallel, in which case it uses regular linear interpolation of quaternion components in order to avoid numerical instability. Otherwise, it computes an orthogonal quaternion qperp using Equation (2.7) and then computes the interpolated quaternion with Equation (2.8).

<<Quaternion Method Definitions>>=
Quaternion Slerp(Float t, const Quaternion &q1, const Quaternion &q2) { Float cosTheta = Dot(q1, q2); if (cosTheta > .9995f) return Normalize((1 - t) * q1 + t * q2); else { Float theta = std::acos(Clamp(cosTheta, -1, 1)); Float thetap = theta * t; Quaternion qperp = Normalize(q2 - q1 * cosTheta); return q1 * std::cos(thetap) + qperp * std::sin(thetap); } }

2.9.3 AnimatedTransform Implementation

Given the foundations of the quaternion infrastructure, we can now implement the AnimatedTransform class, which implements keyframe transformation interpolation in pbrt. Its constructor takes two transformations and the time values they are associated with.

As mentioned earlier, AnimatedTransform decomposes the given composite transformation matrices into scaling, rotation, and translation components. The decomposition is performed by the AnimatedTransform::Decompose() method.

<<AnimatedTransform Method Definitions>>=
AnimatedTransform::AnimatedTransform(const Transform *startTransform, Float startTime, const Transform *endTransform, Float endTime) : startTransform(startTransform), endTransform(endTransform), startTime(startTime), endTime(endTime), actuallyAnimated(*startTransform != *endTransform) { Decompose(startTransform->m, &T[0], &R[0], &S[0]); Decompose(endTransform->m, &T[1], &R[1], &S[1]); <<Flip R[1] if needed to select shortest path>>
if (Dot(R[0], R[1]) < 0) R[1] = -R[1];
hasRotation = Dot(R[0], R[1]) < 0.9995f; <<Compute terms of motion derivative function>>
if (hasRotation) { Float cosTheta = Dot(R[0], R[1]); Float theta = std::acos(Clamp(cosTheta, -1, 1)); Quaternion qperp = Normalize(R[1] - R[0] * cosTheta); Float t0x = T[0].x; Float t0y = T[0].y; Float t0z = T[0].z; Float t1x = T[1].x; Float t1y = T[1].y; Float t1z = T[1].z; Float q1x = R[0].v.x; Float q1y = R[0].v.y; Float q1z = R[0].v.z; Float q1w = R[0].w; Float qperpx = qperp.v.x; Float qperpy = qperp.v.y; Float qperpz = qperp.v.z; Float qperpw = qperp.w; Float s000 = S[0].m[0][0]; Float s001 = S[0].m[0][1]; Float s002 = S[0].m[0][2]; Float s010 = S[0].m[1][0]; Float s011 = S[0].m[1][1]; Float s012 = S[0].m[1][2]; Float s020 = S[0].m[2][0]; Float s021 = S[0].m[2][1]; Float s022 = S[0].m[2][2]; Float s100 = S[1].m[0][0]; Float s101 = S[1].m[0][1]; Float s102 = S[1].m[0][2]; Float s110 = S[1].m[1][0]; Float s111 = S[1].m[1][1]; Float s112 = S[1].m[1][2]; Float s120 = S[1].m[2][0]; Float s121 = S[1].m[2][1]; Float s122 = S[1].m[2][2]; c1[0] = DerivativeTerm(-t0x + t1x, (-1 + q1y*q1y + q1z*q1z + qperpy*qperpy + qperpz*qperpz)*s000 + q1w*q1z*s010 - qperpx*qperpy*s010 + qperpw*qperpz*s010 - q1w*q1y*s020 - qperpw*qperpy*s020 - qperpx*qperpz*s020 + s100 - q1y*q1y*s100 - q1z*q1z*s100 - qperpy*qperpy*s100 - qperpz*qperpz*s100 - q1w*q1z*s110 + qperpx*qperpy*s110 - qperpw*qperpz*s110 + q1w*q1y*s120 + qperpw*qperpy*s120 + qperpx*qperpz*s120 + q1x*(-(q1y*s010) - q1z*s020 + q1y*s110 + q1z*s120), (-1 + q1y*q1y + q1z*q1z + qperpy*qperpy + qperpz*qperpz)*s001 + q1w*q1z*s011 - qperpx*qperpy*s011 + qperpw*qperpz*s011 - q1w*q1y*s021 - qperpw*qperpy*s021 - qperpx*qperpz*s021 + s101 - q1y*q1y*s101 - q1z*q1z*s101 - qperpy*qperpy*s101 - qperpz*qperpz*s101 - q1w*q1z*s111 + qperpx*qperpy*s111 - qperpw*qperpz*s111 + q1w*q1y*s121 + qperpw*qperpy*s121 + qperpx*qperpz*s121 + q1x*(-(q1y*s011) - q1z*s021 + q1y*s111 + q1z*s121), (-1 + q1y*q1y + q1z*q1z + qperpy*qperpy + qperpz*qperpz)*s002 + q1w*q1z*s012 - qperpx*qperpy*s012 + qperpw*qperpz*s012 - q1w*q1y*s022 - qperpw*qperpy*s022 - qperpx*qperpz*s022 + s102 - q1y*q1y*s102 - q1z*q1z*s102 - qperpy*qperpy*s102 - qperpz*qperpz*s102 - q1w*q1z*s112 + qperpx*qperpy*s112 - qperpw*qperpz*s112 + q1w*q1y*s122 + qperpw*qperpy*s122 + qperpx*qperpz*s122 + q1x*(-(q1y*s012) - q1z*s022 + q1y*s112 + q1z*s122)); c2[0] = DerivativeTerm(0., -(qperpy*qperpy*s000) - qperpz*qperpz*s000 + qperpx*qperpy*s010 - qperpw*qperpz*s010 + qperpw*qperpy*s020 + qperpx*qperpz*s020 + q1y*q1y*(s000 - s100) + q1z*q1z*(s000 - s100) + qperpy*qperpy*s100 + qperpz*qperpz*s100 - qperpx*qperpy*s110 + qperpw*qperpz*s110 - qperpw*qperpy*s120 - qperpx*qperpz*s120 + 2*q1x*qperpy*s010*theta - 2*q1w*qperpz*s010*theta + 2*q1w*qperpy*s020*theta + 2*q1x*qperpz*s020*theta + q1y*(q1x*(-s010 + s110) + q1w*(-s020 + s120) + 2*(-2*qperpy*s000 + qperpx*s010 + qperpw*s020)*theta) + q1z*(q1w*(s010 - s110) + q1x*(-s020 + s120) - 2*(2*qperpz*s000 + qperpw*s010 - qperpx*s020)*theta), -(qperpy*qperpy*s001) - qperpz*qperpz*s001 + qperpx*qperpy*s011 - qperpw*qperpz*s011 + qperpw*qperpy*s021 + qperpx*qperpz*s021 + q1y*q1y*(s001 - s101) + q1z*q1z*(s001 - s101) + qperpy*qperpy*s101 + qperpz*qperpz*s101 - qperpx*qperpy*s111 + qperpw*qperpz*s111 - qperpw*qperpy*s121 - qperpx*qperpz*s121 + 2*q1x*qperpy*s011*theta - 2*q1w*qperpz*s011*theta + 2*q1w*qperpy*s021*theta + 2*q1x*qperpz*s021*theta + q1y*(q1x*(-s011 + s111) + q1w*(-s021 + s121) + 2*(-2*qperpy*s001 + qperpx*s011 + qperpw*s021)*theta) + q1z*(q1w*(s011 - s111) + q1x*(-s021 + s121) - 2*(2*qperpz*s001 + qperpw*s011 - qperpx*s021)*theta), -(qperpy*qperpy*s002) - qperpz*qperpz*s002 + qperpx*qperpy*s012 - qperpw*qperpz*s012 + qperpw*qperpy*s022 + qperpx*qperpz*s022 + q1y*q1y*(s002 - s102) + q1z*q1z*(s002 - s102) + qperpy*qperpy*s102 + qperpz*qperpz*s102 - qperpx*qperpy*s112 + qperpw*qperpz*s112 - qperpw*qperpy*s122 - qperpx*qperpz*s122 + 2*q1x*qperpy*s012*theta - 2*q1w*qperpz*s012*theta + 2*q1w*qperpy*s022*theta + 2*q1x*qperpz*s022*theta + q1y*(q1x*(-s012 + s112) + q1w*(-s022 + s122) + 2*(-2*qperpy*s002 + qperpx*s012 + qperpw*s022)*theta) + q1z*(q1w*(s012 - s112) + q1x*(-s022 + s122) - 2*(2*qperpz*s002 + qperpw*s012 - qperpx*s022)*theta)); c3[0] = DerivativeTerm(0., -2*(q1x*qperpy*s010 - q1w*qperpz*s010 + q1w*qperpy*s020 + q1x*qperpz*s020 - q1x*qperpy*s110 + q1w*qperpz*s110 - q1w*qperpy*s120 - q1x*qperpz*s120 + q1y*(-2*qperpy*s000 + qperpx*s010 + qperpw*s020 + 2*qperpy*s100 - qperpx*s110 - qperpw*s120) + q1z*(-2*qperpz*s000 - qperpw*s010 + qperpx*s020 + 2*qperpz*s100 + qperpw*s110 - qperpx*s120))*theta, -2*(q1x*qperpy*s011 - q1w*qperpz*s011 + q1w*qperpy*s021 + q1x*qperpz*s021 - q1x*qperpy*s111 + q1w*qperpz*s111 - q1w*qperpy*s121 - q1x*qperpz*s121 + q1y*(-2*qperpy*s001 + qperpx*s011 + qperpw*s021 + 2*qperpy*s101 - qperpx*s111 - qperpw*s121) + q1z*(-2*qperpz*s001 - qperpw*s011 + qperpx*s021 + 2*qperpz*s101 + qperpw*s111 - qperpx*s121))*theta, -2*(q1x*qperpy*s012 - q1w*qperpz*s012 + q1w*qperpy*s022 + q1x*qperpz*s022 - q1x*qperpy*s112 + q1w*qperpz*s112 - q1w*qperpy*s122 - q1x*qperpz*s122 + q1y*(-2*qperpy*s002 + qperpx*s012 + qperpw*s022 + 2*qperpy*s102 - qperpx*s112 - qperpw*s122) + q1z*(-2*qperpz*s002 - qperpw*s012 + qperpx*s022 + 2*qperpz*s102 + qperpw*s112 - qperpx*s122))*theta); c4[0] = DerivativeTerm(0., -(q1x*qperpy*s010) + q1w*qperpz*s010 - q1w*qperpy*s020 - q1x*qperpz*s020 + q1x*qperpy*s110 - q1w*qperpz*s110 + q1w*qperpy*s120 + q1x*qperpz*s120 + 2*q1y*q1y*s000*theta + 2*q1z*q1z*s000*theta - 2*qperpy*qperpy*s000*theta - 2*qperpz*qperpz*s000*theta + 2*qperpx*qperpy*s010*theta - 2*qperpw*qperpz*s010*theta + 2*qperpw*qperpy*s020*theta + 2*qperpx*qperpz*s020*theta + q1y*(-(qperpx*s010) - qperpw*s020 + 2*qperpy*(s000 - s100) + qperpx*s110 + qperpw*s120 - 2*q1x*s010*theta - 2*q1w*s020*theta) + q1z*(2*qperpz*s000 + qperpw*s010 - qperpx*s020 - 2*qperpz*s100 - qperpw*s110 + qperpx*s120 + 2*q1w*s010*theta - 2*q1x*s020*theta), -(q1x*qperpy*s011) + q1w*qperpz*s011 - q1w*qperpy*s021 - q1x*qperpz*s021 + q1x*qperpy*s111 - q1w*qperpz*s111 + q1w*qperpy*s121 + q1x*qperpz*s121 + 2*q1y*q1y*s001*theta + 2*q1z*q1z*s001*theta - 2*qperpy*qperpy*s001*theta - 2*qperpz*qperpz*s001*theta + 2*qperpx*qperpy*s011*theta - 2*qperpw*qperpz*s011*theta + 2*qperpw*qperpy*s021*theta + 2*qperpx*qperpz*s021*theta + q1y*(-(qperpx*s011) - qperpw*s021 + 2*qperpy*(s001 - s101) + qperpx*s111 + qperpw*s121 - 2*q1x*s011*theta - 2*q1w*s021*theta) + q1z*(2*qperpz*s001 + qperpw*s011 - qperpx*s021 - 2*qperpz*s101 - qperpw*s111 + qperpx*s121 + 2*q1w*s011*theta - 2*q1x*s021*theta), -(q1x*qperpy*s012) + q1w*qperpz*s012 - q1w*qperpy*s022 - q1x*qperpz*s022 + q1x*qperpy*s112 - q1w*qperpz*s112 + q1w*qperpy*s122 + q1x*qperpz*s122 + 2*q1y*q1y*s002*theta + 2*q1z*q1z*s002*theta - 2*qperpy*qperpy*s002*theta - 2*qperpz*qperpz*s002*theta + 2*qperpx*qperpy*s012*theta - 2*qperpw*qperpz*s012*theta + 2*qperpw*qperpy*s022*theta + 2*qperpx*qperpz*s022*theta + q1y*(-(qperpx*s012) - qperpw*s022 + 2*qperpy*(s002 - s102) + qperpx*s112 + qperpw*s122 - 2*q1x*s012*theta - 2*q1w*s022*theta) + q1z*(2*qperpz*s002 + qperpw*s012 - qperpx*s022 - 2*qperpz*s102 - qperpw*s112 + qperpx*s122 + 2*q1w*s012*theta - 2*q1x*s022*theta)); c5[0] = DerivativeTerm(0., 2*(qperpy*qperpy*s000 + qperpz*qperpz*s000 - qperpx*qperpy*s010 + qperpw*qperpz*s010 - qperpw*qperpy*s020 - qperpx*qperpz*s020 - qperpy*qperpy*s100 - qperpz*qperpz*s100 + q1y*q1y*(-s000 + s100) + q1z*q1z*(-s000 + s100) + qperpx*qperpy*s110 - qperpw*qperpz*s110 + q1y*(q1x*(s010 - s110) + q1w*(s020 - s120)) + qperpw*qperpy*s120 + qperpx*qperpz*s120 + q1z*(-(q1w*s010) + q1x*s020 + q1w*s110 - q1x*s120))*theta, 2*(qperpy*qperpy*s001 + qperpz*qperpz*s001 - qperpx*qperpy*s011 + qperpw*qperpz*s011 - qperpw*qperpy*s021 - qperpx*qperpz*s021 - qperpy*qperpy*s101 - qperpz*qperpz*s101 + q1y*q1y*(-s001 + s101) + q1z*q1z*(-s001 + s101) + qperpx*qperpy*s111 - qperpw*qperpz*s111 + q1y*(q1x*(s011 - s111) + q1w*(s021 - s121)) + qperpw*qperpy*s121 + qperpx*qperpz*s121 + q1z*(-(q1w*s011) + q1x*s021 + q1w*s111 - q1x*s121))*theta, 2*(qperpy*qperpy*s002 + qperpz*qperpz*s002 - qperpx*qperpy*s012 + qperpw*qperpz*s012 - qperpw*qperpy*s022 - qperpx*qperpz*s022 - qperpy*qperpy*s102 - qperpz*qperpz*s102 + q1y*q1y*(-s002 + s102) + q1z*q1z*(-s002 + s102) + qperpx*qperpy*s112 - qperpw*qperpz*s112 + q1y*(q1x*(s012 - s112) + q1w*(s022 - s122)) + qperpw*qperpy*s122 + qperpx*qperpz*s122 + q1z*(-(q1w*s012) + q1x*s022 + q1w*s112 - q1x*s122))*theta); c1[1] = DerivativeTerm(-t0y + t1y, -(qperpx*qperpy*s000) - qperpw*qperpz*s000 - s010 + q1z*q1z*s010 + qperpx*qperpx*s010 + qperpz*qperpz*s010 - q1y*q1z*s020 + qperpw*qperpx*s020 - qperpy*qperpz*s020 + qperpx*qperpy*s100 + qperpw*qperpz*s100 + q1w*q1z*(-s000 + s100) + q1x*q1x*(s010 - s110) + s110 - q1z*q1z*s110 - qperpx*qperpx*s110 - qperpz*qperpz*s110 + q1x*(q1y*(-s000 + s100) + q1w*(s020 - s120)) + q1y*q1z*s120 - qperpw*qperpx*s120 + qperpy*qperpz*s120, -(qperpx*qperpy*s001) - qperpw*qperpz*s001 - s011 + q1z*q1z*s011 + qperpx*qperpx*s011 + qperpz*qperpz*s011 - q1y*q1z*s021 + qperpw*qperpx*s021 - qperpy*qperpz*s021 + qperpx*qperpy*s101 + qperpw*qperpz*s101 + q1w*q1z*(-s001 + s101) + q1x*q1x*(s011 - s111) + s111 - q1z*q1z*s111 - qperpx*qperpx*s111 - qperpz*qperpz*s111 + q1x*(q1y*(-s001 + s101) + q1w*(s021 - s121)) + q1y*q1z*s121 - qperpw*qperpx*s121 + qperpy*qperpz*s121, -(qperpx*qperpy*s002) - qperpw*qperpz*s002 - s012 + q1z*q1z*s012 + qperpx*qperpx*s012 + qperpz*qperpz*s012 - q1y*q1z*s022 + qperpw*qperpx*s022 - qperpy*qperpz*s022 + qperpx*qperpy*s102 + qperpw*qperpz*s102 + q1w*q1z*(-s002 + s102) + q1x*q1x*(s012 - s112) + s112 - q1z*q1z*s112 - qperpx*qperpx*s112 - qperpz*qperpz*s112 + q1x*(q1y*(-s002 + s102) + q1w*(s022 - s122)) + q1y*q1z*s122 - qperpw*qperpx*s122 + qperpy*qperpz*s122); c2[1] = DerivativeTerm(0., qperpx*qperpy*s000 + qperpw*qperpz*s000 + q1z*q1z*s010 - qperpx*qperpx*s010 - qperpz*qperpz*s010 - q1y*q1z*s020 - qperpw*qperpx*s020 + qperpy*qperpz*s020 - qperpx*qperpy*s100 - qperpw*qperpz*s100 + q1x*q1x*(s010 - s110) - q1z*q1z*s110 + qperpx*qperpx*s110 + qperpz*qperpz*s110 + q1y*q1z*s120 + qperpw*qperpx*s120 - qperpy*qperpz*s120 + 2*q1z*qperpw*s000*theta + 2*q1y*qperpx*s000*theta - 4*q1z*qperpz*s010*theta + 2*q1z*qperpy*s020*theta + 2*q1y*qperpz*s020*theta + q1x*(q1w*s020 + q1y*(-s000 + s100) - q1w*s120 + 2*qperpy*s000*theta - 4*qperpx*s010*theta - 2*qperpw*s020*theta) + q1w*(-(q1z*s000) + q1z*s100 + 2*qperpz*s000*theta - 2*qperpx*s020*theta), qperpx*qperpy*s001 + qperpw*qperpz*s001 + q1z*q1z*s011 - qperpx*qperpx*s011 - qperpz*qperpz*s011 - q1y*q1z*s021 - qperpw*qperpx*s021 + qperpy*qperpz*s021 - qperpx*qperpy*s101 - qperpw*qperpz*s101 + q1x*q1x*(s011 - s111) - q1z*q1z*s111 + qperpx*qperpx*s111 + qperpz*qperpz*s111 + q1y*q1z*s121 + qperpw*qperpx*s121 - qperpy*qperpz*s121 + 2*q1z*qperpw*s001*theta + 2*q1y*qperpx*s001*theta - 4*q1z*qperpz*s011*theta + 2*q1z*qperpy*s021*theta + 2*q1y*qperpz*s021*theta + q1x*(q1w*s021 + q1y*(-s001 + s101) - q1w*s121 + 2*qperpy*s001*theta - 4*qperpx*s011*theta - 2*qperpw*s021*theta) + q1w*(-(q1z*s001) + q1z*s101 + 2*qperpz*s001*theta - 2*qperpx*s021*theta), qperpx*qperpy*s002 + qperpw*qperpz*s002 + q1z*q1z*s012 - qperpx*qperpx*s012 - qperpz*qperpz*s012 - q1y*q1z*s022 - qperpw*qperpx*s022 + qperpy*qperpz*s022 - qperpx*qperpy*s102 - qperpw*qperpz*s102 + q1x*q1x*(s012 - s112) - q1z*q1z*s112 + qperpx*qperpx*s112 + qperpz*qperpz*s112 + q1y*q1z*s122 + qperpw*qperpx*s122 - qperpy*qperpz*s122 + 2*q1z*qperpw*s002*theta + 2*q1y*qperpx*s002*theta - 4*q1z*qperpz*s012*theta + 2*q1z*qperpy*s022*theta + 2*q1y*qperpz*s022*theta + q1x*(q1w*s022 + q1y*(-s002 + s102) - q1w*s122 + 2*qperpy*s002*theta - 4*qperpx*s012*theta - 2*qperpw*s022*theta) + q1w*(-(q1z*s002) + q1z*s102 + 2*qperpz*s002*theta - 2*qperpx*s022*theta)); c3[1] = DerivativeTerm(0., 2*(-(q1x*qperpy*s000) - q1w*qperpz*s000 + 2*q1x*qperpx*s010 + q1x*qperpw*s020 + q1w*qperpx*s020 + q1x*qperpy*s100 + q1w*qperpz*s100 - 2*q1x*qperpx*s110 - q1x*qperpw*s120 - q1w*qperpx*s120 + q1z*(2*qperpz*s010 - qperpy*s020 + qperpw*(-s000 + s100) - 2*qperpz*s110 + qperpy*s120) + q1y*(-(qperpx*s000) - qperpz*s020 + qperpx*s100 + qperpz*s120))* theta, 2*(-(q1x*qperpy*s001) - q1w*qperpz*s001 + 2*q1x*qperpx*s011 + q1x*qperpw*s021 + q1w*qperpx*s021 + q1x*qperpy*s101 + q1w*qperpz*s101 - 2*q1x*qperpx*s111 - q1x*qperpw*s121 - q1w*qperpx*s121 + q1z*(2*qperpz*s011 - qperpy*s021 + qperpw*(-s001 + s101) - 2*qperpz*s111 + qperpy*s121) + q1y*(-(qperpx*s001) - qperpz*s021 + qperpx*s101 + qperpz*s121))*theta, 2*(-(q1x*qperpy*s002) - q1w*qperpz*s002 + 2*q1x*qperpx*s012 + q1x*qperpw*s022 + q1w*qperpx*s022 + q1x*qperpy*s102 + q1w*qperpz*s102 - 2*q1x*qperpx*s112 - q1x*qperpw*s122 - q1w*qperpx*s122 + q1z*(2*qperpz*s012 - qperpy*s022 + qperpw*(-s002 + s102) - 2*qperpz*s112 + qperpy*s122) + q1y*(-(qperpx*s002) - qperpz*s022 + qperpx*s102 + qperpz*s122))*theta); c4[1] = DerivativeTerm(0., -(q1x*qperpy*s000) - q1w*qperpz*s000 + 2*q1x*qperpx*s010 + q1x*qperpw*s020 + q1w*qperpx*s020 + q1x*qperpy*s100 + q1w*qperpz*s100 - 2*q1x*qperpx*s110 - q1x*qperpw*s120 - q1w*qperpx*s120 + 2*qperpx*qperpy*s000*theta + 2*qperpw*qperpz*s000*theta + 2*q1x*q1x*s010*theta + 2*q1z*q1z*s010*theta - 2*qperpx*qperpx*s010*theta - 2*qperpz*qperpz*s010*theta + 2*q1w*q1x*s020*theta - 2*qperpw*qperpx*s020*theta + 2*qperpy*qperpz*s020*theta + q1y* (-(qperpx*s000) - qperpz*s020 + qperpx*s100 + qperpz*s120 - 2*q1x*s000*theta) + q1z*(2*qperpz*s010 - qperpy*s020 + qperpw*(-s000 + s100) - 2*qperpz*s110 + qperpy*s120 - 2*q1w*s000*theta - 2*q1y*s020*theta), -(q1x*qperpy*s001) - q1w*qperpz*s001 + 2*q1x*qperpx*s011 + q1x*qperpw*s021 + q1w*qperpx*s021 + q1x*qperpy*s101 + q1w*qperpz*s101 - 2*q1x*qperpx*s111 - q1x*qperpw*s121 - q1w*qperpx*s121 + 2*qperpx*qperpy*s001*theta + 2*qperpw*qperpz*s001*theta + 2*q1x*q1x*s011*theta + 2*q1z*q1z*s011*theta - 2*qperpx*qperpx*s011*theta - 2*qperpz*qperpz*s011*theta + 2*q1w*q1x*s021*theta - 2*qperpw*qperpx*s021*theta + 2*qperpy*qperpz*s021*theta + q1y* (-(qperpx*s001) - qperpz*s021 + qperpx*s101 + qperpz*s121 - 2*q1x*s001*theta) + q1z*(2*qperpz*s011 - qperpy*s021 + qperpw*(-s001 + s101) - 2*qperpz*s111 + qperpy*s121 - 2*q1w*s001*theta - 2*q1y*s021*theta), -(q1x*qperpy*s002) - q1w*qperpz*s002 + 2*q1x*qperpx*s012 + q1x*qperpw*s022 + q1w*qperpx*s022 + q1x*qperpy*s102 + q1w*qperpz*s102 - 2*q1x*qperpx*s112 - q1x*qperpw*s122 - q1w*qperpx*s122 + 2*qperpx*qperpy*s002*theta + 2*qperpw*qperpz*s002*theta + 2*q1x*q1x*s012*theta + 2*q1z*q1z*s012*theta - 2*qperpx*qperpx*s012*theta - 2*qperpz*qperpz*s012*theta + 2*q1w*q1x*s022*theta - 2*qperpw*qperpx*s022*theta + 2*qperpy*qperpz*s022*theta + q1y* (-(qperpx*s002) - qperpz*s022 + qperpx*s102 + qperpz*s122 - 2*q1x*s002*theta) + q1z*(2*qperpz*s012 - qperpy*s022 + qperpw*(-s002 + s102) - 2*qperpz*s112 + qperpy*s122 - 2*q1w*s002*theta - 2*q1y*s022*theta)); c5[1] = DerivativeTerm(0., -2*(qperpx*qperpy*s000 + qperpw*qperpz*s000 + q1z*q1z*s010 - qperpx*qperpx*s010 - qperpz*qperpz*s010 - q1y*q1z*s020 - qperpw*qperpx*s020 + qperpy*qperpz*s020 - qperpx*qperpy*s100 - qperpw*qperpz*s100 + q1w*q1z*(-s000 + s100) + q1x*q1x*(s010 - s110) - q1z*q1z*s110 + qperpx*qperpx*s110 + qperpz*qperpz*s110 + q1x*(q1y*(-s000 + s100) + q1w*(s020 - s120)) + q1y*q1z*s120 + qperpw*qperpx*s120 - qperpy*qperpz*s120)*theta, -2*(qperpx*qperpy*s001 + qperpw*qperpz*s001 + q1z*q1z*s011 - qperpx*qperpx*s011 - qperpz*qperpz*s011 - q1y*q1z*s021 - qperpw*qperpx*s021 + qperpy*qperpz*s021 - qperpx*qperpy*s101 - qperpw*qperpz*s101 + q1w*q1z*(-s001 + s101) + q1x*q1x*(s011 - s111) - q1z*q1z*s111 + qperpx*qperpx*s111 + qperpz*qperpz*s111 + q1x*(q1y*(-s001 + s101) + q1w*(s021 - s121)) + q1y*q1z*s121 + qperpw*qperpx*s121 - qperpy*qperpz*s121)*theta, -2*(qperpx*qperpy*s002 + qperpw*qperpz*s002 + q1z*q1z*s012 - qperpx*qperpx*s012 - qperpz*qperpz*s012 - q1y*q1z*s022 - qperpw*qperpx*s022 + qperpy*qperpz*s022 - qperpx*qperpy*s102 - qperpw*qperpz*s102 + q1w*q1z*(-s002 + s102) + q1x*q1x*(s012 - s112) - q1z*q1z*s112 + qperpx*qperpx*s112 + qperpz*qperpz*s112 + q1x*(q1y*(-s002 + s102) + q1w*(s022 - s122)) + q1y*q1z*s122 + qperpw*qperpx*s122 - qperpy*qperpz*s122)*theta); c1[2] = DerivativeTerm(-t0z + t1z, (qperpw*qperpy*s000 - qperpx*qperpz*s000 - q1y*q1z*s010 - qperpw*qperpx*s010 - qperpy*qperpz*s010 - s020 + q1y*q1y*s020 + qperpx*qperpx*s020 + qperpy*qperpy*s020 - qperpw*qperpy*s100 + qperpx*qperpz*s100 + q1x*q1z*(-s000 + s100) + q1y*q1z*s110 + qperpw*qperpx*s110 + qperpy*qperpz*s110 + q1w*(q1y*(s000 - s100) + q1x*(-s010 + s110)) + q1x*q1x*(s020 - s120) + s120 - q1y*q1y*s120 - qperpx*qperpx*s120 - qperpy*qperpy*s120), (qperpw*qperpy*s001 - qperpx*qperpz*s001 - q1y*q1z*s011 - qperpw*qperpx*s011 - qperpy*qperpz*s011 - s021 + q1y*q1y*s021 + qperpx*qperpx*s021 + qperpy*qperpy*s021 - qperpw*qperpy*s101 + qperpx*qperpz*s101 + q1x*q1z*(-s001 + s101) + q1y*q1z*s111 + qperpw*qperpx*s111 + qperpy*qperpz*s111 + q1w*(q1y*(s001 - s101) + q1x*(-s011 + s111)) + q1x*q1x*(s021 - s121) + s121 - q1y*q1y*s121 - qperpx*qperpx*s121 - qperpy*qperpy*s121), (qperpw*qperpy*s002 - qperpx*qperpz*s002 - q1y*q1z*s012 - qperpw*qperpx*s012 - qperpy*qperpz*s012 - s022 + q1y*q1y*s022 + qperpx*qperpx*s022 + qperpy*qperpy*s022 - qperpw*qperpy*s102 + qperpx*qperpz*s102 + q1x*q1z*(-s002 + s102) + q1y*q1z*s112 + qperpw*qperpx*s112 + qperpy*qperpz*s112 + q1w*(q1y*(s002 - s102) + q1x*(-s012 + s112)) + q1x*q1x*(s022 - s122) + s122 - q1y*q1y*s122 - qperpx*qperpx*s122 - qperpy*qperpy*s122)); c2[2] = DerivativeTerm(0., (q1w*q1y*s000 - q1x*q1z*s000 - qperpw*qperpy*s000 + qperpx*qperpz*s000 - q1w*q1x*s010 - q1y*q1z*s010 + qperpw*qperpx*s010 + qperpy*qperpz*s010 + q1x*q1x*s020 + q1y*q1y*s020 - qperpx*qperpx*s020 - qperpy*qperpy*s020 - q1w*q1y*s100 + q1x*q1z*s100 + qperpw*qperpy*s100 - qperpx*qperpz*s100 + q1w*q1x*s110 + q1y*q1z*s110 - qperpw*qperpx*s110 - qperpy*qperpz*s110 - q1x*q1x*s120 - q1y*q1y*s120 + qperpx*qperpx*s120 + qperpy*qperpy*s120 - 2*q1y*qperpw*s000*theta + 2*q1z*qperpx*s000*theta - 2*q1w*qperpy*s000*theta + 2*q1x*qperpz*s000*theta + 2*q1x*qperpw*s010*theta + 2*q1w*qperpx*s010*theta + 2*q1z*qperpy*s010*theta + 2*q1y*qperpz*s010*theta - 4*q1x*qperpx*s020*theta - 4*q1y*qperpy*s020*theta), (q1w*q1y*s001 - q1x*q1z*s001 - qperpw*qperpy*s001 + qperpx*qperpz*s001 - q1w*q1x*s011 - q1y*q1z*s011 + qperpw*qperpx*s011 + qperpy*qperpz*s011 + q1x*q1x*s021 + q1y*q1y*s021 - qperpx*qperpx*s021 - qperpy*qperpy*s021 - q1w*q1y*s101 + q1x*q1z*s101 + qperpw*qperpy*s101 - qperpx*qperpz*s101 + q1w*q1x*s111 + q1y*q1z*s111 - qperpw*qperpx*s111 - qperpy*qperpz*s111 - q1x*q1x*s121 - q1y*q1y*s121 + qperpx*qperpx*s121 + qperpy*qperpy*s121 - 2*q1y*qperpw*s001*theta + 2*q1z*qperpx*s001*theta - 2*q1w*qperpy*s001*theta + 2*q1x*qperpz*s001*theta + 2*q1x*qperpw*s011*theta + 2*q1w*qperpx*s011*theta + 2*q1z*qperpy*s011*theta + 2*q1y*qperpz*s011*theta - 4*q1x*qperpx*s021*theta - 4*q1y*qperpy*s021*theta), (q1w*q1y*s002 - q1x*q1z*s002 - qperpw*qperpy*s002 + qperpx*qperpz*s002 - q1w*q1x*s012 - q1y*q1z*s012 + qperpw*qperpx*s012 + qperpy*qperpz*s012 + q1x*q1x*s022 + q1y*q1y*s022 - qperpx*qperpx*s022 - qperpy*qperpy*s022 - q1w*q1y*s102 + q1x*q1z*s102 + qperpw*qperpy*s102 - qperpx*qperpz*s102 + q1w*q1x*s112 + q1y*q1z*s112 - qperpw*qperpx*s112 - qperpy*qperpz*s112 - q1x*q1x*s122 - q1y*q1y*s122 + qperpx*qperpx*s122 + qperpy*qperpy*s122 - 2*q1y*qperpw*s002*theta + 2*q1z*qperpx*s002*theta - 2*q1w*qperpy*s002*theta + 2*q1x*qperpz*s002*theta + 2*q1x*qperpw*s012*theta + 2*q1w*qperpx*s012*theta + 2*q1z*qperpy*s012*theta + 2*q1y*qperpz*s012*theta - 4*q1x*qperpx*s022*theta - 4*q1y*qperpy*s022*theta)); c3[2] = DerivativeTerm(0., -2*(-(q1w*qperpy*s000) + q1x*qperpz*s000 + q1x*qperpw*s010 + q1w*qperpx*s010 - 2*q1x*qperpx*s020 + q1w*qperpy*s100 - q1x*qperpz*s100 - q1x*qperpw*s110 - q1w*qperpx*s110 + q1z*(qperpx*s000 + qperpy*s010 - qperpx*s100 - qperpy*s110) + 2*q1x*qperpx*s120 + q1y*(qperpz*s010 - 2*qperpy*s020 + qperpw*(-s000 + s100) - qperpz*s110 + 2*qperpy*s120))*theta, -2*(-(q1w*qperpy*s001) + q1x*qperpz*s001 + q1x*qperpw*s011 + q1w*qperpx*s011 - 2*q1x*qperpx*s021 + q1w*qperpy*s101 - q1x*qperpz*s101 - q1x*qperpw*s111 - q1w*qperpx*s111 + q1z*(qperpx*s001 + qperpy*s011 - qperpx*s101 - qperpy*s111) + 2*q1x*qperpx*s121 + q1y*(qperpz*s011 - 2*qperpy*s021 + qperpw*(-s001 + s101) - qperpz*s111 + 2*qperpy*s121))*theta, -2*(-(q1w*qperpy*s002) + q1x*qperpz*s002 + q1x*qperpw*s012 + q1w*qperpx*s012 - 2*q1x*qperpx*s022 + q1w*qperpy*s102 - q1x*qperpz*s102 - q1x*qperpw*s112 - q1w*qperpx*s112 + q1z*(qperpx*s002 + qperpy*s012 - qperpx*s102 - qperpy*s112) + 2*q1x*qperpx*s122 + q1y*(qperpz*s012 - 2*qperpy*s022 + qperpw*(-s002 + s102) - qperpz*s112 + 2*qperpy*s122))*theta); c4[2] = DerivativeTerm(0., q1w*qperpy*s000 - q1x*qperpz*s000 - q1x*qperpw*s010 - q1w*qperpx*s010 + 2*q1x*qperpx*s020 - q1w*qperpy*s100 + q1x*qperpz*s100 + q1x*qperpw*s110 + q1w*qperpx*s110 - 2*q1x*qperpx*s120 - 2*qperpw*qperpy*s000*theta + 2*qperpx*qperpz*s000*theta - 2*q1w*q1x*s010*theta + 2*qperpw*qperpx*s010*theta + 2*qperpy*qperpz*s010*theta + 2*q1x*q1x*s020*theta + 2*q1y*q1y*s020*theta - 2*qperpx*qperpx*s020*theta - 2*qperpy*qperpy*s020*theta + q1z* (-(qperpx*s000) - qperpy*s010 + qperpx*s100 + qperpy*s110 - 2*q1x*s000*theta) + q1y*(-(qperpz*s010) + 2*qperpy*s020 + qperpw*(s000 - s100) + qperpz*s110 - 2*qperpy*s120 + 2*q1w*s000*theta - 2*q1z*s010*theta), q1w*qperpy*s001 - q1x*qperpz*s001 - q1x*qperpw*s011 - q1w*qperpx*s011 + 2*q1x*qperpx*s021 - q1w*qperpy*s101 + q1x*qperpz*s101 + q1x*qperpw*s111 + q1w*qperpx*s111 - 2*q1x*qperpx*s121 - 2*qperpw*qperpy*s001*theta + 2*qperpx*qperpz*s001*theta - 2*q1w*q1x*s011*theta + 2*qperpw*qperpx*s011*theta + 2*qperpy*qperpz*s011*theta + 2*q1x*q1x*s021*theta + 2*q1y*q1y*s021*theta - 2*qperpx*qperpx*s021*theta - 2*qperpy*qperpy*s021*theta + q1z* (-(qperpx*s001) - qperpy*s011 + qperpx*s101 + qperpy*s111 - 2*q1x*s001*theta) + q1y*(-(qperpz*s011) + 2*qperpy*s021 + qperpw*(s001 - s101) + qperpz*s111 - 2*qperpy*s121 + 2*q1w*s001*theta - 2*q1z*s011*theta), q1w*qperpy*s002 - q1x*qperpz*s002 - q1x*qperpw*s012 - q1w*qperpx*s012 + 2*q1x*qperpx*s022 - q1w*qperpy*s102 + q1x*qperpz*s102 + q1x*qperpw*s112 + q1w*qperpx*s112 - 2*q1x*qperpx*s122 - 2*qperpw*qperpy*s002*theta + 2*qperpx*qperpz*s002*theta - 2*q1w*q1x*s012*theta + 2*qperpw*qperpx*s012*theta + 2*qperpy*qperpz*s012*theta + 2*q1x*q1x*s022*theta + 2*q1y*q1y*s022*theta - 2*qperpx*qperpx*s022*theta - 2*qperpy*qperpy*s022*theta + q1z* (-(qperpx*s002) - qperpy*s012 + qperpx*s102 + qperpy*s112 - 2*q1x*s002*theta) + q1y*(-(qperpz*s012) + 2*qperpy*s022 + qperpw*(s002 - s102) + qperpz*s112 - 2*qperpy*s122 + 2*q1w*s002*theta - 2*q1z*s012*theta)); c5[2] = DerivativeTerm(0., 2*(qperpw*qperpy*s000 - qperpx*qperpz*s000 + q1y*q1z*s010 - qperpw*qperpx*s010 - qperpy*qperpz*s010 - q1y*q1y*s020 + qperpx*qperpx*s020 + qperpy*qperpy*s020 + q1x*q1z*(s000 - s100) - qperpw*qperpy*s100 + qperpx*qperpz*s100 + q1w*(q1y*(-s000 + s100) + q1x*(s010 - s110)) - q1y*q1z*s110 + qperpw*qperpx*s110 + qperpy*qperpz*s110 + q1y*q1y*s120 - qperpx*qperpx*s120 - qperpy*qperpy*s120 + q1x*q1x*(-s020 + s120))*theta, 2*(qperpw*qperpy*s001 - qperpx*qperpz*s001 + q1y*q1z*s011 - qperpw*qperpx*s011 - qperpy*qperpz*s011 - q1y*q1y*s021 + qperpx*qperpx*s021 + qperpy*qperpy*s021 + q1x*q1z*(s001 - s101) - qperpw*qperpy*s101 + qperpx*qperpz*s101 + q1w*(q1y*(-s001 + s101) + q1x*(s011 - s111)) - q1y*q1z*s111 + qperpw*qperpx*s111 + qperpy*qperpz*s111 + q1y*q1y*s121 - qperpx*qperpx*s121 - qperpy*qperpy*s121 + q1x*q1x*(-s021 + s121))*theta, 2*(qperpw*qperpy*s002 - qperpx*qperpz*s002 + q1y*q1z*s012 - qperpw*qperpx*s012 - qperpy*qperpz*s012 - q1y*q1y*s022 + qperpx*qperpx*s022 + qperpy*qperpy*s022 + q1x*q1z*(s002 - s102) - qperpw*qperpy*s102 + qperpx*qperpz*s102 + q1w*(q1y*(-s002 + s102) + q1x*(s012 - s112)) - q1y*q1z*s112 + qperpw*qperpx*s112 + qperpy*qperpz*s112 + q1y*q1y*s122 - qperpx*qperpx*s122 - qperpy*qperpy*s122 + q1x*q1x*(-s022 + s122))*theta); }
}

<<AnimatedTransform Private Data>>=
const Transform *startTransform, *endTransform; const Float startTime, endTime; const bool actuallyAnimated; Vector3f T[2]; Quaternion R[2]; Matrix4x4 S[2]; bool hasRotation;

Given the composite matrix for a transformation, information has been lost about any individual transformations that were composed to compute it. For example, given the matrix for the product of a translation and then a scale, an equal matrix could also be computed by first scaling and then translating (by different amounts). Thus, we need to choose a canonical sequence of transformations for the decomposition. For our needs here, the specific choice made isn’t significant. (It would be more important in an animation system that was decomposing composite transformations in order to make them editable by changing individual components, for example.)

We will handle only affine transformations here, which is what is needed for animating cameras and geometric primitives in a rendering system; perspective transformations aren’t generally relevant to animation of objects like these.

The transformation decomposition we will use is the following:

(2.9)

where is the given transformation, is a translation, is a rotation, and is a scale. is actually a generalized scale (Shoemake and Duff call it stretch) that represents a scale in some coordinate system, just not necessarily the current one. In any case, it can still be correctly interpolated with linear interpolation of its components. The Decompose() method computes the decomposition given a Matrix4x4.

<<AnimatedTransform Method Definitions>>+=
void AnimatedTransform::Decompose(const Matrix4x4 &m, Vector3f *T, Quaternion *Rquat, Matrix4x4 *S) { <<Extract translation T from transformation matrix>>
T->x = m.m[0][3]; T->y = m.m[1][3]; T->z = m.m[2][3];
<<Compute new transformation matrix M without translation>>
Matrix4x4 M = m; for (int i = 0; i < 3; ++i) M.m[i][3] = M.m[3][i] = 0.f; M.m[3][3] = 1.f;
<<Extract rotation R from transformation matrix>>
Float norm; int count = 0; Matrix4x4 R = M; do { <<Compute next matrix Rnext in series>>
Matrix4x4 Rnext; Matrix4x4 Rit = Inverse(Transpose(R)); for (int i = 0; i < 4; ++i) for (int j = 0; j < 4; ++j) Rnext.m[i][j] = 0.5f * (R.m[i][j] + Rit.m[i][j]);
<<Compute norm of difference between R and Rnext>>
norm = 0; for (int i = 0; i < 3; ++i) { Float n = std::abs(R.m[i][0] - Rnext.m[i][0]) + std::abs(R.m[i][1] - Rnext.m[i][1]) + std::abs(R.m[i][2] - Rnext.m[i][2]); norm = std::max(norm, n); }
R = Rnext; } while (++count < 100 && norm > .0001); *Rquat = Quaternion(R);
<<Compute scale S using rotation and original matrix>>
*S = Matrix4x4::Mul(Inverse(R), M);
}

Extracting the translation is easy; it can be found directly from the appropriate elements of the transformation matrix.

<<Extract translation T from transformation matrix>>=
T->x = m.m[0][3]; T->y = m.m[1][3]; T->z = m.m[2][3];

Since we are assuming an affine transformation (no projective components), after we remove the translation, what is left is the upper matrix that represents scaling and rotation together. This matrix is copied into a new matrix M for further processing.

<<Compute new transformation matrix M without translation>>=
Matrix4x4 M = m; for (int i = 0; i < 3; ++i) M.m[i][3] = M.m[3][i] = 0.f; M.m[3][3] = 1.f;

Next we’d like to extract the pure rotation component of . We’ll use a technique called polar decomposition to do this. It can be shown that the polar decomposition of a matrix into rotation and scale can be computed by successively averaging with its inverse transpose

until convergence, at which point . (It’s easy to see that if is a pure rotation, then averaging it with its inverse transpose will leave it unchanged, since its inverse is equal to its transpose. The “Further Reading” section has more references that discuss why this series converges to the rotation component of the original transformation.) Shoemake and Duff (1992) proved that the resulting matrix is the closest orthogonal matrix to —a desirable property.

To compute this series, we iteratively apply Equation (2.10) until either the difference between successive terms is small or a fixed number of iterations have been performed. In practice, this series generally converges quickly.

<<Extract rotation R from transformation matrix>>=
Float norm; int count = 0; Matrix4x4 R = M; do { <<Compute next matrix Rnext in series>>
Matrix4x4 Rnext; Matrix4x4 Rit = Inverse(Transpose(R)); for (int i = 0; i < 4; ++i) for (int j = 0; j < 4; ++j) Rnext.m[i][j] = 0.5f * (R.m[i][j] + Rit.m[i][j]);
<<Compute norm of difference between R and Rnext>>
norm = 0; for (int i = 0; i < 3; ++i) { Float n = std::abs(R.m[i][0] - Rnext.m[i][0]) + std::abs(R.m[i][1] - Rnext.m[i][1]) + std::abs(R.m[i][2] - Rnext.m[i][2]); norm = std::max(norm, n); }
R = Rnext; } while (++count < 100 && norm > .0001); *Rquat = Quaternion(R);

<<Compute next matrix Rnext in series>>=
Matrix4x4 Rnext; Matrix4x4 Rit = Inverse(Transpose(R)); for (int i = 0; i < 4; ++i) for (int j = 0; j < 4; ++j) Rnext.m[i][j] = 0.5f * (R.m[i][j] + Rit.m[i][j]);

<<Compute norm of difference between R and Rnext>>=
norm = 0; for (int i = 0; i < 3; ++i) { Float n = std::abs(R.m[i][0] - Rnext.m[i][0]) + std::abs(R.m[i][1] - Rnext.m[i][1]) + std::abs(R.m[i][2] - Rnext.m[i][2]); norm = std::max(norm, n); }

Once we’ve extracted the rotation from , the scale is all that’s left. We would like to find the matrix that satisfies . Now that we know both and , we just solve for .

<<Compute scale S using rotation and original matrix>>=
*S = Matrix4x4::Mul(Inverse(R), M);

For every rotation matrix, there are two unit quaternions that correspond to the matrix that only differ in sign. If the dot product of the two rotations that we have extracted is negative, then a slerp between them won’t take the shortest path between the two corresponding rotations. Negating one of them (here the second was chosen arbitrarily) causes the shorter path to be taken instead.

<<Flip R[1] if needed to select shortest path>>=
if (Dot(R[0], R[1]) < 0) R[1] = -R[1];

The Interpolate() method computes the interpolated transformation matrix at a given time. The matrix is found by interpolating the previously extracted translation, rotation, and scale and then multiplying them together to get a composite matrix that represents the effect of the three transformations together.

<<AnimatedTransform Method Definitions>>+=
void AnimatedTransform::Interpolate(Float time, Transform *t) const { <<Handle boundary conditions for matrix interpolation>>
if (!actuallyAnimated || time <= startTime) { *t = *startTransform; return; } if (time >= endTime) { *t = *endTransform; return; }
Float dt = (time - startTime) / (endTime - startTime); <<Interpolate translation at dt>>
Vector3f trans = (1 - dt) * T[0] + dt * T[1];
<<Interpolate rotation at dt>>
Quaternion rotate = Slerp(dt, R[0], R[1]);
<<Interpolate scale at dt>>
Matrix4x4 scale; for (int i = 0; i < 3; ++i) for (int j = 0; j < 3; ++j) scale.m[i][j] = Lerp(dt, S[0].m[i][j], S[1].m[i][j]);
<<Compute interpolated matrix as product of interpolated components>>
*t = Translate(trans) * rotate.ToTransform() * Transform(scale);
}

If the given time value is outside the time range of the two transformations stored in the AnimatedTransform, then the transformation at the start time or end time is returned, as appropriate. The AnimatedTransform constructor also checks whether the two Transforms stored are the same; if so, then no interpolation is necessary either. All of the classes in pbrt that support animation always store an AnimatedTransform for their transformation, rather than storing either a Transform or AnimatedTransform as appropriate. This simplifies their implementations, though it does make it worthwhile to check for this case here and not unnecessarily do the work to interpolate between two equal transformations.

<<Handle boundary conditions for matrix interpolation>>=
if (!actuallyAnimated || time <= startTime) { *t = *startTransform; return; } if (time >= endTime) { *t = *endTransform; return; }

The dt variable stores the offset in the range from startTime to endTime; it is zero at startTime and one at endTime. Given dt, interpolation of the translation is trivial.

<<Interpolate translation at dt>>=
Vector3f trans = (1 - dt) * T[0] + dt * T[1];

The rotation is interpolated between the start and end rotations using the Slerp() routine (Section 2.9.2).

<<Interpolate rotation at dt>>=
Quaternion rotate = Slerp(dt, R[0], R[1]);

Finally, the interpolated scale matrix is computed by interpolating the individual elements of the start and end scale matrices. Because the Matrix4x4 constructor sets the matrix to the identity matrix, we don’t need to initialize any of the other elements of scale.

<<Interpolate scale at dt>>=
Matrix4x4 scale; for (int i = 0; i < 3; ++i) for (int j = 0; j < 3; ++j) scale.m[i][j] = Lerp(dt, S[0].m[i][j], S[1].m[i][j]);

Given the three interpolated parts, the product of their three transformation matrices gives us the final result.

<<Compute interpolated matrix as product of interpolated components>>=
*t = Translate(trans) * rotate.ToTransform() * Transform(scale);

AnimatedTransform also provides a number of methods that apply interpolated transformations directly, using the provided time for Point3fs and Vector3fs and Ray::time for Rays. These methods are more efficient than calling AnimatedTransform::Interpolate() and then using the returned matrix when there is no actual animation since a copy of the transformation matrix doesn’t need to be made in that case.

<<AnimatedTransform Public Methods>>=
Ray operator()(const Ray &r) const; RayDifferential operator()(const RayDifferential &r) const; Point3f operator()(Float time, const Point3f &p) const; Vector3f operator()(Float time, const Vector3f &v) const;

2.9.4 Bounding Moving Bounding Boxes

Given a Bounds3f that is transformed by an animated transformation, it’s useful to be able to compute a bounding box that encompasses all of its motion over the animation time period. For example, if we can bound the motion of an animated geometric primitive, then we can intersect rays with this bound to determine if the ray might intersect the object before incurring the cost of interpolating the primitive’s bound to the ray’s time to check that intersection. The AnimatedTransform::MotionBounds() method performs this computation, taking a bounding box and returning the bounding box of its motion over the AnimatedTransform’s time range.

There are two easy cases: first, if the keyframe matrices are equal, then we can arbitrarily apply only the starting transformation to compute the full bounds. Second, if the transformation only includes scaling and/or translation, then the bounding box that encompasses the bounding box’s transformed positions at both the start time and the end time bounds all of its motion. To see why this is so, consider the position of a transformed point as a function of time; we’ll denote this function of two matrices, a point, and a time by .

Since in this case the rotation component of the decomposition is the identity, then with our matrix decomposition we have

where the translation and scale are both written as functions of time. Assuming for simplicity that is a regular scale, we can find expressions for the components of . For example, for the component, we have:

where is the corresponding element of the scale matrix for , is the same scale matrix element for , and the translation matrix elements are similarly denoted by . (We chose for “delta” here since is already claimed for time.) As a linear function of , the extrema of this function are at the start and end times. The other coordinates and the case for a generalized scale follow similarly.

<<AnimatedTransform Method Definitions>>+=
Bounds3f AnimatedTransform::MotionBounds(const Bounds3f &b) const { if (!actuallyAnimated) return (*startTransform)(b); if (hasRotation == false) return Union((*startTransform)(b), (*endTransform)(b)); <<Return motion bounds accounting for animated rotation>>
Bounds3f bounds; for (int corner = 0; corner < 8; ++corner) bounds = Union(bounds, BoundPointMotion(b.Corner(corner))); return bounds;
}

For the general case with animated rotations, the motion function may have extrema at points in the middle of the time range. We know of no simple way to find these points. Many renderers address this issue by sampling a large number of times in the time range, computing the interpolated transformation at each one, and taking the union of all of the corresponding transformed bounding boxes. Here, we will develop a more well-grounded method that lets us robustly compute these motion bounds.

We use a slightly simpler conservative bound that entails computing the motion of the eight corners of the bounding box individually and finding the union of those bounds.

<<Return motion bounds accounting for animated rotation>>=
Bounds3f bounds; for (int corner = 0; corner < 8; ++corner) bounds = Union(bounds, BoundPointMotion(b.Corner(corner))); return bounds;

For each bounding box corner , we need to find the extrema of over the animation time range. Recall from calculus that the extrema of a continuous function over some domain are either at the boundary points of the domain or at points where the function’s first derivative is zero. Thus, the overall bound is given by the union of the positions at the start and end of motion as well as the position at any extrema.

Figure 2.18 shows a plot of one coordinate of the motion function and its derivative for an interesting motion path of a point. Note that the maximum value of the function over the time range is reached at a point where the derivative has a zero.

To bound the motion of a single point, we start our derivation by following the approach used for the no-rotation case, expanding out the three , , and components of Equation (2.9) as functions of time and finding their product. We have:

The result is quite complex when expanded out, mostly due to the slerp and the conversion of the resulting quaternion to a matrix; a computer algebra system is a requirement for working with this function.

The derivative is also quite complex—in its full algebraic glory, over 2,000 operations are required to evaluate its value for a given pair of decomposed matrices, point and time. However, given specific transformation matrices and a specific point, is simplified substantially; we’ll denote the specialized function of alone as . Evaluating its derivative requires roughly 10 floating-point operations, a sine, and a cosine to evaluate for each coordinate:

where is the arc cosine of the dot product of the two quaternions and where the five coefficients are 3-vectors that depend on the two matrices and the position . This specialization works out well, since we will need to evaluate the function at many time values for a given point.

We now have two tasks: first, given a pair of keyframe matrices and a point , we first need to be able to efficiently compute the values of the coefficients . Then, given the relatively simple function defined by and , we need to find the zeros of Equation (2.12), which may represent the times at which motion extrema occur.

For the first task, we will first factor out the contributions to the coefficients that depend on the keyframe matrices from those that depend on the point , under the assumption that bounding boxes for multiple points’ motion will be computed for each pair of keyframe matrices (as is the case here). The result is fortunately quite simple—the vectors are linear functions of the point’s , , and components.

Thus, given the coefficients and a particular point we want to bound the motion of, we can efficiently compute the coefficients of the derivative function in Equation (2.12). The DerivativeTerm structure encapsulates these coefficients and this computation.

<<AnimatedTransform Private Data>>+=
struct DerivativeTerm { DerivativeTerm(Float c, Float x, Float y, Float z) : kc(c), kx(x), ky(y), kz(z) { } Float kc, kx, ky, kz; Float Eval(const Point3f &p) const { return kc + kx * p.x + ky * p.y + kz * p.z; } };

The attributes c1-c5 store derivative information corresponding to the five terms in Equation (2.12). The three array elements correspond to the three dimensions of space.

<<AnimatedTransform Private Data>>+=
DerivativeTerm c1[3], c2[3], c3[3], c4[3], c5[3];

The fragment <<Compute terms of motion derivative function>> in the AnimatedTransform constructor, not included here, initializes these terms, via automatically generated code. Given that it requires a few thousand floating-point operations, doing this work once and amortizing over the multiple bounding box corners is helpful. The coefficients are more easily computed if we assume a canonical time range ; later, we’ll have to remap the values of zeros of the motion function to the actual shutter time range.

Given the coefficients based on the keyframe matrices, BoundPointMotion() computes a robust bound of the motion of p.

<<AnimatedTransform Method Definitions>>+=
Bounds3f AnimatedTransform::BoundPointMotion(const Point3f &p) const { Bounds3f bounds((*startTransform)(p), (*endTransform)(p)); Float cosTheta = Dot(R[0], R[1]); Float theta = std::acos(Clamp(cosTheta, -1, 1)); for (int c = 0; c < 3; ++c) { <<Find any motion derivative zeros for the component c>>
Float zeros[4]; int nZeros = 0; IntervalFindZeros(c1[c].Eval(p), c2[c].Eval(p), c3[c].Eval(p), c4[c].Eval(p), c5[c].Eval(p), theta, Interval(0., 1.), zeros, &nZeros);
<<Expand bounding box for any motion derivative zeros found>>
for (int i = 0; i < nZeros; ++i) { Point3f pz = (*this)(Lerp(zeros[i], startTime, endTime), p); bounds = Union(bounds, pz); }
} return bounds; }

The IntervalFindZeros() function, to be introduced shortly, numerically finds zeros of Equation (2.12). Up to four are possible.

<<Find any motion derivative zeros for the component c>>=
Float zeros[4]; int nZeros = 0; IntervalFindZeros(c1[c].Eval(p), c2[c].Eval(p), c3[c].Eval(p), c4[c].Eval(p), c5[c].Eval(p), theta, Interval(0., 1.), zeros, &nZeros);

The zeros are found over , so we need to interpolate within the time range before calling the method to transform the point at the corresponding time. Note also that the extremum is only at one of the , , and  dimensions, and so the bounds only need to be updated in that one dimension. For convenience, here we just use the Union() function, which considers all dimensions, even though two could be ignored.

<<Expand bounding box for any motion derivative zeros found>>=
for (int i = 0; i < nZeros; ++i) { Point3f pz = (*this)(Lerp(zeros[i], startTime, endTime), p); bounds = Union(bounds, pz); }

Finding zeros of the motion derivative function, Equation (2.12), can’t be done algebraically; numerical methods are necessary. Fortunately, the function is well behaved—it’s fairly smooth and has a limited number of zeros. (Recall the plot in Figure 2.18, which was an unusually complex representative.)

While we could use a bisection-based search or Newton’s method, we’d risk missing zeros when the function only briefly crosses the axis. Therefore, we’ll use interval arithmetic, an extension of arithmetic that gives insight about the behavior of functions over ranges of values, which makes it possible to robustly find zeros of functions.

To understand the basic idea of interval arithmetic, consider, for example, the function . If we have an interval of values , then we can see that over the interval, the range of is the interval . In other words .

More generally, all of the basic operations of arithmetic have interval extensions that describe how they operate on intervals. For example, given two intervals and ,

In other words, if we add together two values where one is in the range and the second is in , then the result must be in the range .

Interval arithmetic has the important property that the intervals that it gives are conservative. In particular, if and if , then we know for sure that no value in causes to be negative. In the following, we will show how to compute Equation (2.12) over intervals and will take advantage of the conservative bounds of computed intervals to efficiently find small intervals with zero crossings where regular root finding methods can be reliably used.

First we will define an Interval class that represents intervals of real numbers.

<<Interval Definitions>>=
class Interval { public: <<Interval Public Methods>>
Interval(Float v) : low(v), high(v) { } Interval(Float v0, Float v1) : low(std::min(v0, v1)), high(std::max(v0, v1)) { } Interval operator+(const Interval &i) const { return Interval(low + i.low, high + i.high); } Interval operator-(const Interval &i) const { return Interval(low - i.high, high - i.low); } Interval operator*(const Interval &i) const { return Interval(std::min(std::min(low * i.low, high * i.low), std::min(low * i.high, high * i.high)), std::max(std::max(low * i.low, high * i.low), std::max(low * i.high, high * i.high))); }
Float low, high; };

An interval can be initialized with a single value, representing a single point on the real number line, or with two values that specify an interval with non-zero width.

<<Interval Public Methods>>=
Interval(Float v) : low(v), high(v) { } Interval(Float v0, Float v1) : low(std::min(v0, v1)), high(std::max(v0, v1)) { }

The class also provides overloads for the basic arithmetic operations. Note that for subtraction, the high value of the second interval is subtracted from the low value of the first.

<<Interval Public Methods>>+=
Interval operator+(const Interval &i) const { return Interval(low + i.low, high + i.high); } Interval operator-(const Interval &i) const { return Interval(low - i.high, high - i.low); }

For multiplication, which sides of each interval determine the minimum and maximum values of the result interval depend on the signs of the respective values. Multiplying the various possibilities and taking the overall minimum and maximum is easier than working through which ones to use and multiplying these.

<<Interval Public Methods>>+=
Interval operator*(const Interval &i) const { return Interval(std::min(std::min(low * i.low, high * i.low), std::min(low * i.high, high * i.high)), std::max(std::max(low * i.low, high * i.low), std::max(low * i.high, high * i.high))); }

We have also implemented Sin() and Cos() functions for Intervals. The implementations assume that the given interval is in , which is the case for our use of these functions. Here we only include the implementation of Sin(); Cos() is quite similar in basic structure.

<<Interval Definitions>>+=
inline Interval Sin(const Interval &i) { Float sinLow = std::sin(i.low), sinHigh = std::sin(i.high); if (sinLow > sinHigh) std::swap(sinLow, sinHigh); if (i.low < Pi / 2 && i.high > Pi / 2) sinHigh = 1.; if (i.low < (3.f / 2.f) * Pi && i.high > (3.f / 2.f) * Pi) sinLow = -1.; return Interval(sinLow, sinHigh); }

Given the interval machinery, we can now implement the IntervalFindZeros() function, which finds the values of any zero crossings of Equation (2.12) over the given interval tInterval.

<<Interval Definitions>>+=
void IntervalFindZeros(Float c1, Float c2, Float c3, Float c4, Float c5, Float theta, Interval tInterval, Float *zeros, int *zeroCount, int depth = 8) { <<Evaluate motion derivative in interval form, return if no zeros>>
Interval range = Interval(c1) + (Interval(c2) + Interval(c3) * tInterval) * Cos(Interval(2 * theta) * tInterval) + (Interval(c4) + Interval(c5) * tInterval) * Sin(Interval(2 * theta) * tInterval); if (range.low > 0. || range.high < 0. || range.low == range.high) return;
if (depth > 0) { <<Split tInterval and check both resulting intervals>>
Float mid = (tInterval.low + tInterval.high) * 0.5f; IntervalFindZeros(c1, c2, c3, c4, c5, theta, Interval(tInterval.low, mid), zeros, zeroCount, depth - 1); IntervalFindZeros(c1, c2, c3, c4, c5, theta, Interval(mid, tInterval.high), zeros, zeroCount, depth - 1);
} else { <<Use Newton’s method to refine zero>>
Float tNewton = (tInterval.low + tInterval.high) * 0.5f; for (int i = 0; i < 4; ++i) { Float fNewton = c1 + (c2 + c3 * tNewton) * std::cos(2.f * theta * tNewton) + (c4 + c5 * tNewton) * std::sin(2.f * theta * tNewton); Float fPrimeNewton = (c3 + 2 * (c4 + c5 * tNewton) * theta) * std::cos(2.f * tNewton * theta) + (c5 - 2 * (c2 + c3 * tNewton) * theta) * std::sin(2.f * tNewton * theta); if (fNewton == 0 || fPrimeNewton == 0) break; tNewton = tNewton - fNewton / fPrimeNewton; } zeros[*zeroCount] = tNewton; (*zeroCount)++;
} }

The function starts by computing the interval range over tInterval. If the range doesn’t span zero, then there are no zeros of the function over tInterval and the function can return.

<<Evaluate motion derivative in interval form, return if no zeros>>=
Interval range = Interval(c1) + (Interval(c2) + Interval(c3) * tInterval) * Cos(Interval(2 * theta) * tInterval) + (Interval(c4) + Interval(c5) * tInterval) * Sin(Interval(2 * theta) * tInterval); if (range.low > 0. || range.high < 0. || range.low == range.high) return;

If the interval range does span zero, then there may be one or more zeros in the interval tInterval, but it’s also possible that there actually aren’t any, since the interval bounds are conservative but not as tight as possible. The function splits tInterval into two parts and recursively checks the two sub-intervals. Reducing the size of the interval domain generally reduces the extent of the interval range, which may allow us to determine that there are no zeros in one or both of the new intervals.

<<Split tInterval and check both resulting intervals>>=
Float mid = (tInterval.low + tInterval.high) * 0.5f; IntervalFindZeros(c1, c2, c3, c4, c5, theta, Interval(tInterval.low, mid), zeros, zeroCount, depth - 1); IntervalFindZeros(c1, c2, c3, c4, c5, theta, Interval(mid, tInterval.high), zeros, zeroCount, depth - 1);

Once we have a narrow interval where the interval value of the motion derivative function spans zero, the implementation switches to a few iterations of Newton’s method to find the zero, starting at the midpoint of the interval. Newton’s method requires the derivative of the function; since we’re finding zeros of the motion derivative function, this is the second derivative of Equation (2.11):

<<Use Newton’s method to refine zero>>=
Float tNewton = (tInterval.low + tInterval.high) * 0.5f; for (int i = 0; i < 4; ++i) { Float fNewton = c1 + (c2 + c3 * tNewton) * std::cos(2.f * theta * tNewton) + (c4 + c5 * tNewton) * std::sin(2.f * theta * tNewton); Float fPrimeNewton = (c3 + 2 * (c4 + c5 * tNewton) * theta) * std::cos(2.f * tNewton * theta) + (c5 - 2 * (c2 + c3 * tNewton) * theta) * std::sin(2.f * tNewton * theta); if (fNewton == 0 || fPrimeNewton == 0) break; tNewton = tNewton - fNewton / fPrimeNewton; } zeros[*zeroCount] = tNewton; (*zeroCount)++;

Note that if there were multiple zeros of the function in tInterval when Newton’s method is used, then we will only find one of them here. However, because the interval is quite small at this point, the impact of this error should be minimal. In any case, we haven’t found this issue to be a problem in practice.