int hormodular::Orientation::getRelativeOrientation(int connector, hormodular::Orientation localOrient, hormodular::Orientation remoteOrient) { //-- Create rotation matrices for local orientation: Eigen::AngleAxisd rollAngle( deg2rad(localOrient.getRoll()), Eigen::Vector3d::UnitZ()); Eigen::AngleAxisd pitchAngle( deg2rad(localOrient.getPitch()), Eigen::Vector3d::UnitY()); Eigen::AngleAxisd yawAngle( deg2rad(localOrient.getYaw()), Eigen::Vector3d::UnitX()); Eigen::Quaterniond q0 = yawAngle * pitchAngle * rollAngle ; Eigen::Matrix3d rotationMatrix = q0.matrix(); //-- Create rotation matrices for the other orientation: Eigen::AngleAxisd otherRollAngle( deg2rad(remoteOrient.getRoll()), Eigen::Vector3d::UnitZ()); Eigen::AngleAxisd otherPitchAngle( deg2rad(remoteOrient.getPitch()), Eigen::Vector3d::UnitY()); Eigen::AngleAxisd otherYawAngle( deg2rad(remoteOrient.getYaw()), Eigen::Vector3d::UnitX()); Eigen::Quaterniond q1 = otherYawAngle * otherPitchAngle * otherRollAngle; Eigen::Matrix3d otherRotationMatrix = q1.matrix(); Eigen::Matrix3d relativeRotation = rotationMatrix.inverse() * otherRotationMatrix; Eigen::Vector3d new_z = relativeRotation * Eigen::Vector3d::UnitZ(); // std::cout << "New_z: " << std::endl << new_z << std::endl << std::endl; //-- Get connector base vector for the rotations: Eigen::Vector3d base_vector; if ( connector == 0 || connector == 2) { //-- Y axis base_vector = Eigen::Vector3d::UnitY(); } else if ( connector == 1 || connector == 3) { //-- X axis base_vector = Eigen::Vector3d::UnitX(); } //-- Try for rotations to fit the vector for ( int i = 0; i < 4; i++) { Eigen::AngleAxisd rotAngle( deg2rad(i*90), base_vector); Eigen::Matrix3d rotMatrix = rotAngle.matrix(); Eigen::Vector3d result_vector = rotMatrix * Eigen::Vector3d::UnitZ(); // std::cout << "i = " << i << std::endl << result_vector << std::endl << std::endl; if ( vector_equal(result_vector, new_z) ) return i; } return -1; }
Foam::scalar Foam::finiteRotation::rotIncrementAngle() const { return rotAngle(rotIncrementTensor_); }
Foam::HamiltonRodriguezRot Foam::finiteRotation::eCurrent() const { return HamiltonRodriguezRot(rotVector(), rotAngle()); }