Skip to content
Snippets Groups Projects
Commit e4c548aa authored by Fabian Paus's avatar Fabian Paus
Browse files

ArViz: Extract functions for constraining scale and rotation

parent 0fd97c61
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
source/RobotAPI/components/ArViz/Coin/Visualizer.cpp
+ 94
− 41
View file @ e4c548aa
......@@ -7,6 +7,9 @@
#include <Inventor/SoPath.h>
#include <Eigen/Core>
#include <Eigen/Geometry>
namespace armarx::viz
{
namespace coin
......@@ -702,7 +705,35 @@ namespace coin
feedback.revision = pulledUpdates.revision;
}
#if 0
SbVec3f translationDueToScaling(SbRotation r_m, SbVec3f t_m, SbVec3f s_m, SbVec3f t_o)
{
SbVec3f t_o_scaled(s_m[0] * t_o[0], s_m[1] * t_o[1], s_m[2] * t_o[2]);
SbVec3f t_added_rotation_and_scale;
r_m.multVec(t_o_scaled, t_added_rotation_and_scale);
t_added_rotation_and_scale -= t_o;
SbVec3f t_added_rotation;
r_m.multVec(t_o, t_added_rotation);
t_added_rotation -= t_o;
SbVec3f t_added_scale = t_added_rotation_and_scale - t_added_rotation;
return t_added_scale;
}
SbVec3f translationDueToRotation(SbRotation r_m, SbVec3f t_m, SbVec3f s_m, SbVec3f t_o)
{
SbVec3f t_o_scaled(s_m[0] * t_o[0], s_m[1] * t_o[1], s_m[2] * t_o[2]);
SbVec3f t_added_rotation_and_scale;
r_m.multVec(t_o_scaled, t_added_rotation_and_scale);
t_added_rotation_and_scale -= t_o;
// Do we need to exclude
// SbVec3f t_added_rotation;
// r_m.multVec(t_o, t_added_rotation);
// t_added_rotation -= t_o;
// SbVec3f t_added_scale = t_added_rotation_and_scale - t_added_rotation;
return t_added_rotation_and_scale;
}
Eigen::Matrix4f toEigen(SbMat const& mat)
{
Eigen::Matrix4f result;
......@@ -717,27 +748,54 @@ namespace coin
return result;
}
Eigen::Affine3f toEigen(SbRotation const& rot, SbVec3f const& transl)
// This should constrain the rotation according to the enabled axes
static SbRotation constrainRotation(SbRotation input, int enableFlags)
{
Eigen::Affine3f result;
result.linear() = Eigen::Quaternion(rot[3], rot[0], rot[1], rot[2]).toRotationMatrix();
result.translation() = Eigen::Vector3f(transl[0], transl[1], transl[2]);
SbMatrix mat;
mat.setRotate(input);
Eigen::Matrix4f mat_eigen = toEigen(mat);
Eigen::Matrix3f mat_rot = mat_eigen.block<3, 3>(0, 0);
Eigen::Vector3f rpy = mat_rot.eulerAngles(0, 1, 2);
ARMARX_INFO << "rpy before: " << rpy.transpose();
if ((enableFlags & data::InteractionEnableFlags::ROTATION_X) == 0)
{
rpy(0) = 0.0f;
}
if ((enableFlags & data::InteractionEnableFlags::ROTATION_Y) == 0)
{
rpy(1) = 0.0f;
}
if ((enableFlags & data::InteractionEnableFlags::ROTATION_Z) == 0)
{
rpy(2) = 0.0f;
}
ARMARX_INFO << "rpy after: " << rpy.transpose();
mat_rot = Eigen::AngleAxisf(rpy(0), Eigen::Vector3f::UnitX())
* Eigen::AngleAxisf(rpy(1), Eigen::Vector3f::UnitY())
* Eigen::AngleAxisf(rpy(2), Eigen::Vector3f::UnitZ());
Eigen::Quaternionf q(mat_rot);
SbRotation result(q.x(), q.y(), q.z(), q.w());
return result;
}
#endif
SbVec3f translationDueToScaling(SbRotation r_m, SbVec3f t_m, SbVec3f s_m, SbVec3f t_o)
static SbVec3f constrainScaling(SbVec3f input, int enableFlags)
{
SbVec3f t_o_scaled(s_m[0] * t_o[0], s_m[1] * t_o[1], s_m[2] * t_o[2]);
SbVec3f t_added_rotation_and_scale;
r_m.multVec(t_o_scaled, t_added_rotation_and_scale);
t_added_rotation_and_scale -= t_o;
SbVec3f t_added_rotation;
r_m.multVec(t_o, t_added_rotation);
t_added_rotation -= t_o;
SbVec3f t_added_scale = t_added_rotation_and_scale - t_added_rotation;
return t_added_scale;
SbVec3f result = input;
if ((enableFlags & data::InteractionEnableFlags::SCALING_X) == 0)
{
result[0] = 1.0f;
}
if ((enableFlags & data::InteractionEnableFlags::SCALING_Y) == 0)
{
result[1] = 1.0f;
}
if ((enableFlags & data::InteractionEnableFlags::SCALING_Z) == 0)
{
result[2] = 1.0f;
}
return result;
}
void CoinVisualizer::onManipulation(SoDragger* dragger, int eventType)
......@@ -783,25 +841,14 @@ namespace coin
// Transformation applied by the manipulator
SbVec3f t_m = manipulator->translation.getValue();
SbRotation r_m = manipulator->rotation.getValue();
SbVec3f s_m = manipulator->scaleFactor.getValue();
SbRotation r_m_old = manipulator->rotation.getValue();
SbVec3f s_m_old = manipulator->scaleFactor.getValue();
int enableFlags = selectedElement->interaction.enableFlags;
// Allow scaling only along specified axes
SbVec3f s_m_old = s_m;
if ((enableFlags & data::InteractionEnableFlags::SCALING_X) == 0)
{
s_m[0] = 1.0f;
}
if ((enableFlags & data::InteractionEnableFlags::SCALING_Y) == 0)
{
s_m[1] = 1.0f;
}
if ((enableFlags & data::InteractionEnableFlags::SCALING_Z) == 0)
{
s_m[2] = 1.0f;
}
// TODO: This induces motion and needs to be corrected!
SbRotation r_m = constrainRotation(r_m_old, enableFlags);
SbVec3f s_m = constrainScaling(s_m_old, enableFlags);
// Transformation applied to the object
// Translation is in mm, but the manipulator works in m!
SbVec3f t_o = 0.001f * selectedElement->visu->transform->translation.getValue();
......@@ -809,12 +856,23 @@ namespace coin
if (s_m != s_m_old)
{
manipulator->scaleFactor.setValue(s_m);
// Remove motion induced by scaling here!
SbVec3f t_old = translationDueToScaling(r_m, t_m, s_m_old, t_o);
// Remove motion induced by scaling
SbVec3f t_old = translationDueToScaling(r_m_old, t_m, s_m_old, t_o);
SbVec3f t_new = translationDueToScaling(r_m, t_m, s_m, t_o);
SbVec3f t_diff = t_new - t_old;
t_m -= t_diff;
}
if (r_m != r_m_old)
{
manipulator->rotation.setValue(r_m);
// Remove motion induced by rotation
SbVec3f t_old = translationDueToRotation(r_m_old, t_m, s_m, t_o);
SbVec3f t_new = translationDueToRotation(r_m, t_m, s_m, t_o);
SbVec3f t_diff = t_new - t_old;
t_m -= t_diff;
}
// TODO: Should we use the rotation of the object?
// SbRotation r_o = selectedElement->visu->transform->rotation.getValue();
......@@ -834,8 +892,6 @@ namespace coin
r_m.multVec(t_o, t_added_rotation);
t_added_rotation -= t_o;
SbVec3f t_added_scale = t_added_rotation_and_scale - t_added_rotation;
//ARMARX_INFO << "delta_t: " << delta_t[0] << ", " << delta_t[1] << ", " << delta_t[2];
//ARMARX_INFO << "t_added_scale: " << t_added_scale[0] << ", " << t_added_scale[1] << ", " << t_added_scale[2];
// Prevent translation along disabled axes
if ((enableFlags & data::InteractionEnableFlags::TRANSLATION_X) == 0)
......@@ -854,9 +910,6 @@ namespace coin
SbVec3f t_m_projected = delta_t - t_added_rotation_and_scale;
manipulator->translation.setValue(t_m_projected);
//manipulator->rotation.setValue(newRotation.x(), newRotation.y(), newRotation.z(), newRotation.w());
// TODO: Prevent rotation along disabled axes
// t_m_projected is the correct value for the manipulator, but it still contains translation due to scaling!
// We should subtract the translation induced by scaling!
SbVec3f t_m_non_scaled = t_m_projected + t_added_scale;
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment