From c4ffec6a906e217fabce3ea7ab30eed7543c6e10 Mon Sep 17 00:00:00 2001
From: fjandrad <ing.andrade.francisco@gmail.com>
Date: Sun, 5 Apr 2020 09:55:13 +0200
Subject: [PATCH] Use the scale in the externalMesh object

---
 bindings/matlab/+iDynTreeWrappers/getMeshes.m       | 10 +++++++---
 bindings/matlab/+iDynTreeWrappers/plotMeshInWorld.m | 11 ++++-------
 2 files changed, 11 insertions(+), 10 deletions(-)

diff --git a/bindings/matlab/+iDynTreeWrappers/getMeshes.m b/bindings/matlab/+iDynTreeWrappers/getMeshes.m
index 7c2a90b9962..f95838dfb6a 100644
--- a/bindings/matlab/+iDynTreeWrappers/getMeshes.m
+++ b/bindings/matlab/+iDynTreeWrappers/getMeshes.m
@@ -37,12 +37,16 @@
     meshInfo=struct('meshFile',{},'mesh_triangles',{},'link_H_geom',{});
     for solids=1:solids_number
         if solidarray{solids}.isExternalMesh
-            meshName=split(solidarray{solids}.asExternalMesh.filename,':');
+            externalMesh=solidarray{solids}.asExternalMesh;
+            scale=externalMesh.scale.toMatlab;
+            meshName=split(externalMesh.filename,':');
             meshFile=meshName{2};
             % Import an STL mesh, returning a PATCH-compatible face-vertex structure
-            mesh_triangles = stlread([meshFilePrefix meshFile]);
+            mesh_triangles = stlread([meshFilePrefix meshFile]);            
             meshInfo(solids).meshFile=meshFile;
+            meshInfo(solids).scale=scale';
         else
+            meshInfo(solids).scale=[1,1,1];  
             if solidarray{solids}.isCylinder
                 meshInfo(solids).meshFile='cylinder';
                 length=solidarray{solids}.asCylinder.length;
@@ -64,7 +68,7 @@
         end
         link_H_geom=solidarray{solids}.link_H_geometry.asHomogeneousTransform.toMatlab;
         meshInfo(solids).link_H_geom=link_H_geom;
-        meshInfo(solids).mesh_triangles=mesh_triangles;
+        meshInfo(solids).mesh_triangles=mesh_triangles;              
         map(count,:)=[{meshFile},{linkName}];
         count=count+1;
     end
diff --git a/bindings/matlab/+iDynTreeWrappers/plotMeshInWorld.m b/bindings/matlab/+iDynTreeWrappers/plotMeshInWorld.m
index b0bb9abdefd..0d3a5941afc 100644
--- a/bindings/matlab/+iDynTreeWrappers/plotMeshInWorld.m
+++ b/bindings/matlab/+iDynTreeWrappers/plotMeshInWorld.m
@@ -17,7 +17,6 @@
 % Default values
 meshColor= [0.1843    0.3098    0.3098];
 transparency=0.5;
-scaleRatio=1/1000;
 
 % create a transform object %
 transform = hgtransform('Parent',gca);
@@ -25,13 +24,11 @@
     
     mesh_triangles=linkMeshInfo.meshInfo(mesh_number).mesh_triangles;
     link_H_geom=linkMeshInfo.meshInfo(mesh_number).link_H_geom;
+    scale=linkMeshInfo.meshInfo(mesh_number).scale;
     
-    % Change scale only for stl meshes
-    if ~strcmp({'cylinder','box','sphere'},linkMeshInfo.meshInfo(mesh_number).meshFile)
-        corrected_vertices=mesh_triangles.Points*scaleRatio;
-    else
-        corrected_vertices=mesh_triangles.Points;
-    end
+    % Change scale
+    % Scale the STL mesh    
+    corrected_vertices=mesh_triangles.Points.*scale;    
     
     % Applying transform to link frame to stl file  :    
     corrected_vertices=link_H_geom*[corrected_vertices';ones(1,size(corrected_vertices,1))];