Implementing 3D Coordinate Transformations with Quaternions in C++ Using Eigen
Implementing 3D Coordinate Transformations with Quaternions in C++ Using Eigen
1. Declaring Quaternions
cpp
#include
#include
#include
using Eigen::Quaternionf;
using Eigen::Vector3f;
using namespace std;
int main()
{
// Create two quaternions with w, x, y, z components
Quaternionf rotation_quat{1, 2, 3, 4};
Quaternionf transform_quat{5, 6, 7, 8};
// Display quaternion values
cout << "First quaternion: " << rotation_quat << endl;
cout << "Second quaternion: " << transform_quat << endl;
return 0;
}
2. Adding Quaternions
cpp
#include
#include
using Eigen::Quaternionf;
using namespace std;
int main()
{
Quaternionf quat1{1, 2, 3, 4};
Quaternionf quat2{5, 6, 7, 8};
// Display original quaternions
cout << "Quaternion 1: " << quat1 << endl;
cout << "Quaternion 2: " << quat2 << endl;
// Create a new quaternion to store the result
Quaternionf result_quat;
// Add vector components and scalar parts separately
result_quat.vec() = quat1.vec() + quat2.vec();
result_quat.w() = quat1.w() + quat2.w();
cout << "Result of addition: " << result_quat << endl;
return 0;
}
3. Scalar Multiplication of Quaternions
cpp
#include
#include
using Eigen::Quaternionf;
using namespace std;
int main()
{
Quaternionf original_quat{1, 2, 3, 4};
float scale_factor = 5.0f;
// Display original quaternion
cout << "Original quaternion: " << original_quat << endl;
// Create a new quaternion for the result
Quaternionf scaled_quat;
// Apply scalar multiplication to both vector and scalar components
scaled_quat.vec() = scale_factor * original_quat.vec();
scaled_quat.w() = scale_factor * original_quat.w();
cout << "Scaled quaternion: " << scaled_quat << endl;
return 0;
}
4. Quaternion Multiplication: Method 1 (Using Built-in Operator)
cpp
#include
#include
using Eigen::Quaternionf;
using namespace std;
int main()
{
Quaternionf first_quat{1, 2, 3, 4};
Quaternionf second_quat{5, 6, 7, 8};
// Display original quaternions
cout << "First quaternion: " << first_quat << endl;
cout << "Second quaternion: " << second_quat << endl;
// Method 1: Use Eigen's built-in multiplication operator
Quaternionf product_quat = first_quat * second_quat;
cout << "Product using built-in operator: " << product_quat << endl;
return 0;
}
5. Quaternion Multiplication: Method 2 (Using Vector Formulas)
cpp
#include
#include
using Eigen::Quaternionf;
using Eigen::Vector3f;
using namespace std;
int main()
{
Quaternionf quat_a{1, 2, 3, 4};
Quaternionf quat_b{5, 6, 7, 8};
// Display original quaternions
cout << "Quaternion A: " << quat_a << endl;
cout << "Quaternion B: " << quat_b << endl;
// Method 2: Using vector-based multiplication formula
Quaternionf result_quat;
result_quat.vec() = quat_a.w() * quat_b.vec() +
quat_b.w() * quat_a.vec() +
quat_a.vec().cross(quat_b.vec());
result_quat.w() = quat_a.w() * quat_b.w() -
quat_a.vec().dot(quat_b.vec());
cout << "Product using vector formulas: " << result_quat << endl;
return 0;
}
6. Quaternion Multiplication: Method 3 (Component-wise Calculation)
cpp
#include
#include
using Eigen::Quaternionf;
using namespace std;
int main()
{
Quaternionf q1{1, 2, 3, 4};
Quaternionf q2{5, 6, 7, 8};
// Display original quaternions
cout << "Quaternion 1: " << q1 << endl;
cout << "Quaternion 2: " << q2 << endl;
// Method 3: Component-wise multiplication
Quaternionf result;
result.w() = q1.w() * q2.w() - q1.x() * q2.x() -
q1.y() * q2.y() - q1.z() * q2.z();
result.x() = q1.w() * q2.x() + q1.x() * q2.w() +
q1.y() * q2.z() - q1.z() * q2.y();
result.y() = q1.w() * q2.y() - q1.x() * q2.z() +
q1.y() * q2.w() + q1.z() * q2.x();
result.z() = q1.w() * q2.z() + q1.x() * q2.y() -
q1.y() * q2.x() + q1.z() * q2.w();
cout << "Product using component-wise calculation: " << result << endl;
return 0;
}
7. Complete Example: Rotating a 3D Point Using Quaternions
cpp
#include
#include
#include
using Eigen::Quaternionf;
using Eigen::Vector3f;
using namespace std;
// Function to add two quaternions
Quaternionf add_quaternions(Quaternionf q1, Quaternionf q2)
{
Quaternionf result;
result.vec() = q1.vec() + q2.vec();
result.w() = q1.w() + q2.w();
return result;
}
// Function to scale a quaternion
Quaternionf scale_quaternion(Quaternionf q, float scale)
{
Quaternionf result;
result.vec() = scale * q.vec();
result.w() = scale * q.w();
return result;
}
// Function to multiply quaternions using vector formulas
Quaternionf multiply_quaternions(Quaternionf q1, Quaternionf q2)
{
Quaternionf result;
result.vec() = q1.w() * q2.vec() +
q2.w() * q1.vec() +
q1.vec().cross(q2.vec());
result.w() = q1.w() * q2.w() -
q1.vec().dot(q2.vec());
return result;
}
// Alternative function to multiply quaternions component-wise
Quaternionf multiply_quaternions_component(Quaternionf q1, Quaternionf q2)
{
Quaternionf result;
result.w() = q1.w() * q2.w() - q1.x() * q2.x() -
q1.y() * q2.y() - q1.z() * q2.z();
result.x() = q1.w() * q2.x() + q1.x() * q2.w() +
q1.y() * q2.z() - q1.z() * q2.y();
result.y() = q1.w() * q2.y() - q1.x() * q2.z() +
q1.y() * q2.w() + q1.z() * q2.x();
result.z() = q1.w() * q2.z() + q1.x() * q2.y() -
q1.y() * q2.x() + q1.z() * q2.w();
return result;
}
// Function to calculate quaternion inverse
Quaternionf quaternion_inverse(Quaternionf q)
{
Quaternionf result;
result.vec() = -q.vec();
result.w() = q.w();
float norm_squared = q.norm() * q.norm();
result.vec() /= norm_squared;
result.w() /= norm_squared;
return result;
}
int main()
{
// Define two quaternions for demonstration
Quaternionf rotation1{1, 2, 3, 4};
Quaternionf rotation2{5, 6, 7, 8};
// Create a 3D point to rotate
Vector3f point{1.0f, 1.0f, 0.0f};
// Create a quaternion from the point (w=0)
Quaternionf point_quat;
point_quat.vec() = point;
point_quat.w() = 0.0f;
// Define rotation angle and axis
float angle_degrees = 45.0f;
float angle_radians = angle_degrees / 180.0f * 3.14159265358979323846f;
Vector3f rotation_axis{0.0f, 0.0f, 1.0f}; // Rotate around Z-axis
// Create rotation quaternion
Quaternionf rotation_quat{cos(angle_radians/2),
sin(angle_radians/2) * rotation_axis[0],
sin(angle_radians/2) * rotation_axis[1],
sin(angle_radians/2) * rotation_axis[2]};
// Normalize the rotation quaternion to ensure it's a unit quaternion
rotation_quat = rotation_quat.normalized();
cout << "Original point: " << point.transpose() << endl;
cout << "Rotation quaternion: " << rotation_quat << endl;
// Apply rotation: p' = q * p * q^-1
Quaternionf rotated_point_quat = rotation_quat * point_quat * quaternion_inverse(rotation_quat);
// Extract the rotated point from the quaternion
Vector3f rotated_point = rotated_point_quat.vec();
cout << "Rotated point: " << rotated_point.transpose() << endl;
return 0;
}
Tags:
C++
Eigen
Quaternions
3D Transformation
Rotation
Posted on Fri, 08 May 2026 04:39:47 +0000 by matthewd