Rodrigues’ rotation

This is an example of how to use the Symbol class to validate a function. The function we are using is Rodrigues’ rotation formula which takes a rotation in Angle-Axis form \(~(\theta, \mathbf{v})\) and transforms a vector \(\mathbf{k} \in \mathbb{R}^3\).

\[\mathbf{v}_\mathrm{rot} = \mathbf{v} \cos\theta + (\mathbf{k} \times \mathbf{v}) \sin\theta + \mathbf{k} ~(\mathbf{k} \cdot \mathbf{v}) (1 - \cos\theta)\]

We use the matrix library Eigen to implement the formula in a way which is efficient while remaining almost as expressive as the mathematical notation.

template <class T>
void rotate_rodrigues(const Vec<T, 3>& k, const T& theta, const Vec<T, 3>& v, Vec<T, 3>& v_rot)
{
    v_rot = v*cos(theta) + k.cross(v)*sin(theta) + k*k.dot(v)*(1-cos(theta));
}

This function can now be used to transform 3D vectors efficiently, but right now we are interested in testing its mathematical correctness. For reference we will use the class Eigen::AngleAxis<T>(), with T=Symbol.

C++

Output

int main()
{
    Symbol angle("theta");
    Vec3 axis = namedVector<3>("k"), point = namedVector<3>("v");
    
    Vec3 r1;

    // Use Rodrigues' rotation formula to calculate the rotation of the point:
    rotate_rodrigues(axis, angle, point, r1);
    std::cout << "Rodrigues:\n" << r1 << std::endl;

    // Use Eigen to calculate the rotation of the point:
    Vec3 r = Eigen::AngleAxis<Symbol>(angle, axis)*point;
    std::cout << "\nEigen:\n"  << r << std::endl;
    

Rodrigues:
v_1*cos(theta) + (-v_2*k_3 + v_3*k_2)*sin(theta) + k_1*(1 - cos(theta))*(v_1*k_1 + v_2*k_2 + v_3*k_3)
 v_2*cos(theta) + (v_1*k_3 - v_3*k_1)*sin(theta) + k_2*(1 - cos(theta))*(v_1*k_1 + v_2*k_2 + v_3*k_3)
v_3*cos(theta) + (-v_1*k_2 + v_2*k_1)*sin(theta) + k_3*(1 - cos(theta))*(v_1*k_1 + v_2*k_2 + v_3*k_3)

Eigen:
v_1*(pow(k_1, 2)*(1 - cos(theta)) + cos(theta)) + v_2*(-k_3*sin(theta) + k_2*k_1*(1 - cos(theta))) + v_3*(k_2*sin(theta) + k_3*k_1*(1 - cos(theta)))
v_1*(k_3*sin(theta) + k_2*k_1*(1 - cos(theta))) + v_2*(pow(k_2, 2)*(1 - cos(theta)) + cos(theta)) + v_3*(-k_1*sin(theta) + k_3*k_2*(1 - cos(theta)))
v_1*(-k_2*sin(theta) + k_3*k_1*(1 - cos(theta))) + v_2*(k_1*sin(theta) + k_3*k_2*(1 - cos(theta))) + v_3*(pow(k_3, 2)*(1 - cos(theta)) + cos(theta))

It is not immediately obvious whether the two expressions are equivalent, but the function Symbol::equals(const Symbol&) tests for mathematical equivalency.

C++

Output

    
    // Check for mathematical equality:
    std::cout << "\nThe two expressions are " 
              << (equals(r1, r) ? "equal" : "not equal") 
              << "." << std::endl;

The two expressions are equal.

Note

Do not use code like a==b to test for mathematical equivalency. See Conditional statements.

Finally, we can use the function Symbol::expand() to transform the expressions to their expanded form, which is identical for both expressions.

C++

Output

    // If we convert all expressions to their expanded form, we can see that they are identical.
    std::cout << "\nExpanded:\n";

    std::cout << "\nRodrigues:\n" << expanded(r1) << std::endl;
    std::cout << "\nEigen:\n"  << expanded(r) << std::endl;

Expanded:

Rodrigues:
v_1*pow(k_1, 2) + v_1*cos(theta) - v_1*pow(k_1, 2)*cos(theta) + v_2*k_2*k_1 - v_2*k_3*sin(theta) + v_3*k_2*sin(theta) + v_3*k_3*k_1 - v_2*k_2*k_1*cos(theta) - v_3*k_3*k_1*cos(theta)
v_2*pow(k_2, 2) + v_2*cos(theta) + v_1*k_2*k_1 + v_1*k_3*sin(theta) - v_2*pow(k_2, 2)*cos(theta) - v_3*k_1*sin(theta) + v_3*k_3*k_2 - v_1*k_2*k_1*cos(theta) - v_3*k_3*k_2*cos(theta)
v_3*pow(k_3, 2) + v_3*cos(theta) - v_1*k_2*sin(theta) + v_1*k_3*k_1 + v_2*k_1*sin(theta) + v_2*k_3*k_2 - v_3*pow(k_3, 2)*cos(theta) - v_1*k_3*k_1*cos(theta) - v_2*k_3*k_2*cos(theta)

Eigen:
v_1*pow(k_1, 2) + v_1*cos(theta) - v_1*pow(k_1, 2)*cos(theta) + v_2*k_2*k_1 - v_2*k_3*sin(theta) + v_3*k_2*sin(theta) + v_3*k_3*k_1 - v_2*k_2*k_1*cos(theta) - v_3*k_3*k_1*cos(theta)
v_2*pow(k_2, 2) + v_2*cos(theta) + v_1*k_2*k_1 + v_1*k_3*sin(theta) - v_2*pow(k_2, 2)*cos(theta) - v_3*k_1*sin(theta) + v_3*k_3*k_2 - v_1*k_2*k_1*cos(theta) - v_3*k_3*k_2*cos(theta)
v_3*pow(k_3, 2) + v_3*cos(theta) - v_1*k_2*sin(theta) + v_1*k_3*k_1 + v_2*k_1*sin(theta) + v_2*k_3*k_2 - v_3*pow(k_3, 2)*cos(theta) - v_1*k_3*k_1*cos(theta) - v_2*k_3*k_2*cos(theta)

You can find the full source code of this example here.