From 17e6117b444bf3efcbda15970451f505ad3d7654 Mon Sep 17 00:00:00 2001
From: Jerome1434 <63642637+Jerome1434@users.noreply.github.com>
Date: Thu, 23 Jul 2020 12:07:56 +0200
Subject: [PATCH] Update align.py

---
 ampscan/align.py | 24 ++++++++++++++++++++++--
 1 file changed, 22 insertions(+), 2 deletions(-)

diff --git a/ampscan/align.py b/ampscan/align.py
index d1e3b44..68d54b2 100644
--- a/ampscan/align.py
+++ b/ampscan/align.py
@@ -53,13 +53,16 @@ class align(object):
     """    
 
     def __init__(self, moving, static, method = 'linPoint2Plane', 
-                *args, **kwargs):
+                inverse=False, *args, **kwargs):
         mData = dict(zip(['vert', 'faces', 'values'], 
                          [moving.vert, moving.faces, moving.values]))
         alData = copy.deepcopy(mData)
         self.m = AmpObject(alData, stype='reg')
         self.s = static
-        self.runICP(method=method, *args, **kwargs)
+        if inverse:
+            self.inverse(method=method, *args, **kwargs)
+        else:
+            self.runICP(method=method, *args, **kwargs)
 
     
     def runICP(self, method = 'linPoint2Plane', maxiter=20, inlier=1.0,
@@ -160,6 +163,23 @@ def runICP(self, method = 'linPoint2Plane', maxiter=20, inlier=1.0,
         self.T = Ts[:, -1]
         self.rmse = err[-1]  
             
+    def inverse(self, method = 'linPoint2Plane', *args, **kwargs):
+        #inverting the objects
+        self.temp = self.s
+        self.s = self.m
+        self.m = self.temp
+        self.runICP(method=method, *args, **kwargs)
+        #resetting the objects
+        self.temp = self.s
+        self.s = self.m
+        self.m = self.temp
+        del self.temp
+        #inverting the transformation on both objects
+        self.R = self.R.transpose()
+        self.T = -self.T
+        self.tForm = np.r_[np.c_[self.R, np.zeros(3)], np.append(self.T, 1)[:, None].T]
+        self.s.rigidTransform(self.R, self.T)
+        self.m.rigidTransform(self.R, self.T)
     
     @staticmethod
     def linPoint2Plane(mv, sv, sn):