From 17ce026f577fc88c7c1765ae8332e5b126742ea2 Mon Sep 17 00:00:00 2001
From: wmayer <wmayer@users.sourceforge.net>
Date: Wed, 5 Sep 2018 22:18:55 +0200
Subject: [PATCH] consider global placement when exporting point cloud

---
 src/Mod/Points/App/AppPointsPy.cpp |   5 +-
 src/Mod/Points/App/PointsAlgos.cpp | 105 +++++++++++++++++++++++------
 src/Mod/Points/App/PointsAlgos.h   |   2 +
 3 files changed, 90 insertions(+), 22 deletions(-)

diff --git a/src/Mod/Points/App/AppPointsPy.cpp b/src/Mod/Points/App/AppPointsPy.cpp
index 7965a0ff8b..c486b7d9d5 100644
--- a/src/Mod/Points/App/AppPointsPy.cpp
+++ b/src/Mod/Points/App/AppPointsPy.cpp
@@ -307,8 +307,10 @@ private:
             if (PyObject_TypeCheck(item, &(App::DocumentObjectPy::Type))) {
                 App::DocumentObject* obj = static_cast<App::DocumentObjectPy*>(item)->getDocumentObjectPtr();
                 if (obj->getTypeId().isDerivedFrom(pointsId)) {
-
+                    // get relative placement
                     Points::Feature* fea = static_cast<Points::Feature*>(obj);
+                    Base::Placement globalPlacement = fea->globalPlacement();
+
                     const PointKernel& kernel = fea->Points.getValue();
                     std::unique_ptr<Writer> writer;
                     if (file.hasExtension("asc")) {
@@ -354,6 +356,7 @@ private:
                         writer->setNormals(nor->getValues());
                     }
 
+                    writer->setPlacement(globalPlacement);
                     writer->write(encodedName);
 
                     break;
diff --git a/src/Mod/Points/App/PointsAlgos.cpp b/src/Mod/Points/App/PointsAlgos.cpp
index 3e3048aade..e048a8c859 100644
--- a/src/Mod/Points/App/PointsAlgos.cpp
+++ b/src/Mod/Points/App/PointsAlgos.cpp
@@ -1334,6 +1334,11 @@ void Writer::setHeight(int h)
     height = h;
 }
 
+void Writer::setPlacement(const Base::Placement& p)
+{
+    placement = p;
+}
+
 // ----------------------------------------------------------------------------
 
 AscWriter::AscWriter(const PointKernel& p) : Writer(p)
@@ -1346,7 +1351,14 @@ AscWriter::~AscWriter()
 
 void AscWriter::write(const std::string& filename)
 {
-    points.save(filename.c_str());
+    if (placement.isIdentity()) {
+        points.save(filename.c_str());
+    }
+    else {
+        PointKernel copy = points;
+        copy.transformGeometry(placement.toMatrix());
+        copy.save(filename.c_str());
+    }
 }
 
 // ----------------------------------------------------------------------------
@@ -1416,10 +1428,22 @@ void PlyWriter::write(const std::string& filename)
 
     Eigen::MatrixXf data(numPoints, properties.size());
 
-    for (std::size_t i=0; i<numPoints; i++) {
-        data(i,0) = pts[i].x;
-        data(i,1) = pts[i].y;
-        data(i,2) = pts[i].z;
+    if (placement.isIdentity()) {
+        for (std::size_t i=0; i<numPoints; i++) {
+            data(i,0) = pts[i].x;
+            data(i,1) = pts[i].y;
+            data(i,2) = pts[i].z;
+        }
+    }
+    else {
+        Base::Vector3d tmp;
+        for (std::size_t i=0; i<numPoints; i++) {
+            tmp = Base::convertTo<Base::Vector3d>(pts[i]);
+            placement.multVec(tmp, tmp);
+            data(i,0) = static_cast<float>(tmp.x);
+            data(i,1) = static_cast<float>(tmp.y);
+            data(i,2) = static_cast<float>(tmp.z);
+        }
     }
 
     std::size_t col = 3;
@@ -1427,10 +1451,23 @@ void PlyWriter::write(const std::string& filename)
         int col0 = col;
         int col1 = col+1;
         int col2 = col+2;
-        for (std::size_t i=0; i<numPoints; i++) {
-            data(i,col0) = normals[i].x;
-            data(i,col1) = normals[i].y;
-            data(i,col2) = normals[i].z;
+        Base::Rotation rot = placement.getRotation();
+        if (rot.isIdentity()) {
+            for (std::size_t i=0; i<numPoints; i++) {
+                data(i,col0) = normals[i].x;
+                data(i,col1) = normals[i].y;
+                data(i,col2) = normals[i].z;
+            }
+        }
+        else {
+            Base::Vector3d tmp;
+            for (std::size_t i=0; i<numPoints; i++) {
+                tmp = Base::convertTo<Base::Vector3d>(normals[i]);
+                rot.multVec(tmp, tmp);
+                data(i,col0) = static_cast<float>(tmp.x);
+                data(i,col1) = static_cast<float>(tmp.y);
+                data(i,col2) = static_cast<float>(tmp.z);
+            }
         }
         col += 3;
     }
@@ -1542,13 +1579,26 @@ void PcdWriter::write(const std::string& filename)
     }
 
     std::size_t numPoints = points.size();
+    const std::vector<Base::Vector3f>& pts = points.getBasicPoints();
+
     Eigen::MatrixXf data(numPoints, fields.size());
 
-    const std::vector<Base::Vector3f>& pts = points.getBasicPoints();
-    for (std::size_t i=0; i<numPoints; i++) {
-        data(i,0) = pts[i].x;
-        data(i,1) = pts[i].y;
-        data(i,2) = pts[i].z;
+    if (placement.isIdentity()) {
+        for (std::size_t i=0; i<numPoints; i++) {
+            data(i,0) = pts[i].x;
+            data(i,1) = pts[i].y;
+            data(i,2) = pts[i].z;
+        }
+    }
+    else {
+        Base::Vector3d tmp;
+        for (std::size_t i=0; i<numPoints; i++) {
+            tmp = Base::convertTo<Base::Vector3d>(pts[i]);
+            placement.multVec(tmp, tmp);
+            data(i,0) = static_cast<float>(tmp.x);
+            data(i,1) = static_cast<float>(tmp.y);
+            data(i,2) = static_cast<float>(tmp.z);
+        }
     }
 
     std::size_t col = 3;
@@ -1556,10 +1606,23 @@ void PcdWriter::write(const std::string& filename)
         int col0 = col;
         int col1 = col+1;
         int col2 = col+2;
-        for (std::size_t i=0; i<numPoints; i++) {
-            data(i,col0) = normals[i].x;
-            data(i,col1) = normals[i].y;
-            data(i,col2) = normals[i].z;
+        Base::Rotation rot = placement.getRotation();
+        if (rot.isIdentity()) {
+            for (std::size_t i=0; i<numPoints; i++) {
+                data(i,col0) = normals[i].x;
+                data(i,col1) = normals[i].y;
+                data(i,col2) = normals[i].z;
+            }
+        }
+        else {
+            Base::Vector3d tmp;
+            for (std::size_t i=0; i<numPoints; i++) {
+                tmp = Base::convertTo<Base::Vector3d>(normals[i]);
+                rot.multVec(tmp, tmp);
+                data(i,col0) = static_cast<float>(tmp.x);
+                data(i,col1) = static_cast<float>(tmp.y);
+                data(i,col2) = static_cast<float>(tmp.z);
+            }
         }
         col += 3;
     }
@@ -1617,9 +1680,9 @@ void PcdWriter::write(const std::string& filename)
     out << "WIDTH " << width << std::endl;
     out << "HEIGHT " << height << std::endl;
 
-    Base::Placement placement;
-    Base::Vector3d p = placement.getPosition();
-    Base::Rotation o = placement.getRotation();
+    Base::Placement plm;
+    Base::Vector3d p = plm.getPosition();
+    Base::Rotation o = plm.getRotation();
     double x,y,z,w; o.getValue(x,y,z,w);
     out << "VIEWPOINT " << p.x << " " << p.y << " " << p.z
         << " " << w << " " << x << " " << y << " " << z << std::endl;
diff --git a/src/Mod/Points/App/PointsAlgos.h b/src/Mod/Points/App/PointsAlgos.h
index d248c07a06..da72e778c4 100644
--- a/src/Mod/Points/App/PointsAlgos.h
+++ b/src/Mod/Points/App/PointsAlgos.h
@@ -127,6 +127,7 @@ public:
     void setNormals(const std::vector<Base::Vector3f>&);
     void setWidth(int);
     void setHeight(int);
+    void setPlacement(const Base::Placement&);
 
 protected:
     const PointKernel& points;
@@ -134,6 +135,7 @@ protected:
     std::vector<App::Color> colors;
     std::vector<Base::Vector3f> normals;
     int width, height;
+    Base::Placement placement;
 };
 
 class AscWriter : public Writer