Fixed scaled rotation matrix for Pose (and derived) nodes (cyberbotics#6246)

* Fixed scaled rotation matrix

* Cleanup: horizontal drag only used for top nodes

* Cleanup unused method

Author: stefaniapedrazzi

src/webots/gui/WbDragPoseEvent.cpp

@@ -93,10 +93,7 @@ WbDragHorizontalEvent::WbDragHorizontalEvent(const QPoint &initialPosition, WbVi
   normalizedMouseRay.normalize();
   if (abs(normalizedMouseRay.direction().dot(mUpWorldVector)) > DRAG_HORIZONTAL_MIN_COS) {
     mIsMouseRayValid = true;
-
-    // in case mSelectedPose is not child of root (ex: Root --> Transform(s) --> mSelectedPose = uppermostSolid)
-    if (!mSelectedPose->isTopPose())
-      mCoordinateTransform = WbRotation(mSelectedPose->rotationMatrix()).toQuaternion().conjugated();
+    assert(mSelectedPose->baseNode()->upperPose() == NULL);  // is top Pose node
   } else {
     mIsMouseRayValid = false;
     WbLog::warning(tr("To drag this element, first rotate the view so that the horizontal plane is clearly visible."));
@@ -117,10 +114,6 @@ void WbDragHorizontalEvent::apply(const QPoint &currentMousePosition) {
     displacementFromInitialPosition.setX(displacementFromInitialPosition.x() / mScaleFromParents.x());
     displacementFromInitialPosition.setZ(displacementFromInitialPosition.z() / mScaleFromParents.z());
 
-    // express the displacement in the coordinate frame of the Solid (in case it has some parent Transform(s)).
-    if (!mSelectedPose->isTopPose())
-      displacementFromInitialPosition = mCoordinateTransform * displacementFromInitialPosition;
-
     mSelectedPose->setTranslation((mInitialPosition + displacementFromInitialPosition).rounded(WbPrecision::GUI_MEDIUM));
     mSelectedPose->emitTranslationOrRotationChangedByUser();
   }

src/webots/nodes/WbAbstractPose.cpp

@@ -39,8 +39,6 @@ void WbAbstractPose::init(WbBaseNode *node) {
   mMatrix = NULL;
   mMatrixNeedUpdate = true;
   mVrmlMatrixNeedUpdate = true;
-  mIsTopPose = false;
-  mHasSearchedTopPose = false;
   mTranslateRotateManipulator = NULL;
 
   mTranslateRotateManipulatorInitialized = false;
@@ -223,15 +221,6 @@ const WbMatrix4 &WbAbstractPose::vrmlMatrix() const {
   return mVrmlMatrix;
 }
 
-bool WbAbstractPose::isTopPose() const {
-  if (!mHasSearchedTopPose) {
-    mIsTopPose = mBaseNode->upperPose() == NULL;
-    mHasSearchedTopPose = true;
-  }
-
-  return mIsTopPose;
-}
-
 // Position and orientation setters
 
 void WbAbstractPose::setTranslationAndRotation(double tx, double ty, double tz, double rx, double ry, double rz, double angle) {

src/webots/nodes/WbAbstractPose.hpp

@@ -72,10 +72,14 @@ class WbAbstractPose {
   WbVector3 yAxis() const { return matrix().yAxis(); }
   WbVector3 zAxis() const { return matrix().zAxis(); }
 
-  bool isTopPose() const;
-
   // 3x3 absolute rotation matrix
-  virtual WbMatrix3 rotationMatrix() const { return matrix().extracted3x3Matrix(); }
+  virtual WbMatrix3 rotationMatrix() const {
+    const WbMatrix4 &m = matrix();
+    const WbVector3 &s = m.scale();
+    WbMatrix3 rm = m.extracted3x3Matrix();
+    rm.scale(1.0 / s.x(), 1.0 / s.y(), 1.0 / s.z());
+    return rm;
+  }
 
   // position in 'world' coordinates
   WbVector3 position() const { return matrix().translation(); }
@@ -139,8 +143,6 @@ class WbAbstractPose {
   void updateRotationFieldVisibility() const;
 
   virtual void updateMatrix() const;
-  mutable bool mHasSearchedTopPose;
-  mutable bool mIsTopPose;
 };
 
 void inline WbAbstractPose::setTranslationAndRotationFromOde(double tx, double ty, double tz, double rx, double ry, double rz,

src/webots/nodes/WbTransform.cpp

@@ -127,14 +127,6 @@ const WbVector3 &WbTransform::absoluteScale() const {
   return mAbsoluteScale;
 }
 
-WbMatrix3 WbTransform::rotationMatrix() const {
-  const WbMatrix4 &m = matrix();
-  const WbVector3 &s = m.scale();
-  WbMatrix3 rm = m.extracted3x3Matrix();
-  rm.scale(1.0 / s.x(), 1.0 / s.y(), 1.0 / s.z());
-  return rm;
-}
-
 const WbMatrix4 &WbTransform::vrmlMatrix() const {
   if (mVrmlMatrixNeedUpdate) {
     mVrmlMatrix.fromVrml(translation(), rotation(), scale());

src/webots/nodes/WbTransform.hpp

@@ -57,7 +57,6 @@ class WbTransform : public WbPose {
   mutable bool mAbsoluteScaleNeedUpdate;
 
   // 3x3 absolute rotation matrix
-  WbMatrix3 rotationMatrix() const override;
   const WbMatrix4 &vrmlMatrix() const override;
 
 protected: