From 7dc1dd21fbda9dae824f93b58a33cf942257d7e8 Mon Sep 17 00:00:00 2001
From: wmayer <wmayer@users.sourceforge.net>
Date: Wed, 25 Jul 2018 17:31:11 +0200
Subject: [PATCH] + implement a faster method to multiply vector with matrix +
 add convenience method to transform a point array

---
 src/Base/Matrix.h                  | 32 ++++++++++++++++++++++++++----
 src/Mod/Mesh/App/Core/Elements.cpp |  6 ++++++
 src/Mod/Mesh/App/Core/Elements.h   | 14 +++++++++++++
 3 files changed, 48 insertions(+), 4 deletions(-)

diff --git a/src/Base/Matrix.h b/src/Base/Matrix.h
index dc27d349a9..bbc4141b68 100644
--- a/src/Base/Matrix.h
+++ b/src/Base/Matrix.h
@@ -83,6 +83,8 @@ public:
   /// Multiplication matrix with vector 
   inline Vector3f  operator *  (const Vector3f& rclVct) const;
   inline Vector3d  operator *  (const Vector3d& rclVct) const;
+  inline void multVec(const Vector3d & src, Vector3d & dst) const;
+  inline void multVec(const Vector3f & src, Vector3f & dst) const;
   /// Comparison
   inline bool      operator != (const Matrix4D& rclMtrx) const;
   /// Comparison
@@ -278,9 +280,9 @@ inline Vector3f Matrix4D::operator* (const Vector3f& rclVct) const
 {
   return Vector3f((float)(dMtrx4D[0][0]*rclVct.x + dMtrx4D[0][1]*rclVct.y +
                           dMtrx4D[0][2]*rclVct.z + dMtrx4D[0][3]),
-                  (float)(dMtrx4D[1][0]*rclVct.x + dMtrx4D[1][1]*rclVct.y + 
+                  (float)(dMtrx4D[1][0]*rclVct.x + dMtrx4D[1][1]*rclVct.y +
                           dMtrx4D[1][2]*rclVct.z + dMtrx4D[1][3]),
-                  (float)(dMtrx4D[2][0]*rclVct.x + dMtrx4D[2][1]*rclVct.y + 
+                  (float)(dMtrx4D[2][0]*rclVct.x + dMtrx4D[2][1]*rclVct.y +
                           dMtrx4D[2][2]*rclVct.z + dMtrx4D[2][3]));
 }
 
@@ -288,12 +290,34 @@ inline Vector3d Matrix4D::operator* (const Vector3d& rclVct) const
 {
   return Vector3d((dMtrx4D[0][0]*rclVct.x + dMtrx4D[0][1]*rclVct.y +
                    dMtrx4D[0][2]*rclVct.z + dMtrx4D[0][3]),
-                  (dMtrx4D[1][0]*rclVct.x + dMtrx4D[1][1]*rclVct.y + 
+                  (dMtrx4D[1][0]*rclVct.x + dMtrx4D[1][1]*rclVct.y +
                    dMtrx4D[1][2]*rclVct.z + dMtrx4D[1][3]),
-                  (dMtrx4D[2][0]*rclVct.x + dMtrx4D[2][1]*rclVct.y + 
+                  (dMtrx4D[2][0]*rclVct.x + dMtrx4D[2][1]*rclVct.y +
                    dMtrx4D[2][2]*rclVct.z + dMtrx4D[2][3]));
 }
 
+inline void Matrix4D::multVec(const Vector3d & src, Vector3d & dst) const
+{
+  double x = (dMtrx4D[0][0]*src.x + dMtrx4D[0][1]*src.y +
+              dMtrx4D[0][2]*src.z + dMtrx4D[0][3]);
+  double y = (dMtrx4D[1][0]*src.x + dMtrx4D[1][1]*src.y +
+              dMtrx4D[1][2]*src.z + dMtrx4D[1][3]);
+  double z = (dMtrx4D[2][0]*src.x + dMtrx4D[2][1]*src.y +
+              dMtrx4D[2][2]*src.z + dMtrx4D[2][3]);
+  dst.Set(x,y,z);
+}
+
+inline void Matrix4D::multVec(const Vector3f & src, Vector3f & dst) const
+{
+  float x = (dMtrx4D[0][0]*src.x + dMtrx4D[0][1]*src.y +
+             dMtrx4D[0][2]*src.z + dMtrx4D[0][3]);
+  float y = (dMtrx4D[1][0]*src.x + dMtrx4D[1][1]*src.y +
+             dMtrx4D[1][2]*src.z + dMtrx4D[1][3]);
+  float z = (dMtrx4D[2][0]*src.x + dMtrx4D[2][1]*src.y +
+             dMtrx4D[2][2]*src.z + dMtrx4D[2][3]);
+  dst.Set(x,y,z);
+}
+
 inline bool Matrix4D::operator== (const Matrix4D& rclMtrx) const
 {
   short     iz, is;
diff --git a/src/Mod/Mesh/App/Core/Elements.cpp b/src/Mod/Mesh/App/Core/Elements.cpp
index 5fcbc6381c..016f283be5 100644
--- a/src/Mod/Mesh/App/Core/Elements.cpp
+++ b/src/Mod/Mesh/App/Core/Elements.cpp
@@ -90,6 +90,12 @@ MeshPointArray& MeshPointArray::operator = (const MeshPointArray &rclPAry)
   return *this;
 }
 
+void MeshPointArray::Transform(const Base::Matrix4D& mat)
+{
+  for (_TIterator pP = begin(); pP != end(); ++pP)
+    mat.multVec(*pP,*pP);
+}
+
 void MeshFacetArray::Erase (_TIterator pIter)
 {
   unsigned long i, *pulN;
diff --git a/src/Mod/Mesh/App/Core/Elements.h b/src/Mod/Mesh/App/Core/Elements.h
index 2d73229a59..f49eea9c10 100644
--- a/src/Mod/Mesh/App/Core/Elements.h
+++ b/src/Mod/Mesh/App/Core/Elements.h
@@ -33,6 +33,7 @@
 
 #include <Base/BoundBox.h>
 #include <Base/Vector3D.h>
+#include <Base/Matrix.h>
 
 // Cannot use namespace Base in constructors of MeshPoint
 #ifdef _MSC_VER
@@ -99,6 +100,7 @@ public:
   /** @name Construction */
   //@{
   MeshPoint (void) : _ucFlag(0), _ulProp(0) { }
+  inline MeshPoint (float x, float y, float z);
   inline MeshPoint (const Base::Vector3f &rclPt);
   inline MeshPoint (const MeshPoint &rclPt);
   ~MeshPoint (void) { }
@@ -523,6 +525,7 @@ public:
 
   // Assignment
   MeshPointArray& operator = (const MeshPointArray &rclPAry);
+  void Transform(const Base::Matrix4D&);
   /**
    * Searches for the first point index  Two points are equal if the distance is less
    * than EPSILON. If no such points is found ULONG_MAX is returned. 
@@ -643,6 +646,17 @@ private:
     MeshFacetArray& rFacets;
 };
 
+inline MeshPoint::MeshPoint (float x, float y, float z)
+#ifdef _MSC_VER
+: Vector3f(x, y, z),
+#else
+: Base::Vector3f(x, y, z),
+#endif
+  _ucFlag(0),
+  _ulProp(0)
+{
+}
+
 inline MeshPoint::MeshPoint (const Base::Vector3f &rclPt)
 #ifdef _MSC_VER
 : Vector3f(rclPt),