Fixing ArUcoMarkersGroup.to_obj method.

1 files changed, 10 insertions, 12 deletions

diff --git a/src/argaze/ArUcoMarkers/ArUcoMarkersGroup.py b/src/argaze/ArUcoMarkers/ArUcoMarkersGroup.py
index df390b4..bedd408 100644
--- a/src/argaze/ArUcoMarkers/ArUcoMarkersGroup.py
+++ b/src/argaze/ArUcoMarkers/ArUcoMarkersGroup.py
@@ -497,26 +497,24 @@ class ArUcoMarkersGroup():
 
             v_count = 0
 
-            for identifier, place in self.places.items():
+            for p, (identifier, place) in enumerate(self.places.items()):
 
                 file.write(f'o {self.dictionary.name}#{identifier}_Marker\n')
 
                 vertices = ''
 
-                T = place.translation
-                R = place.rotation
-
-                points = (T + numpy.float32([R.dot(place.marker.points[0]), R.dot(place.marker.points[1]), R.dot(place.marker.points[2]), R.dot(place.marker.points[3])])).reshape(-1, 3)
-
-                print(points)
-
                 # Write vertices in reverse order
-                for i in [3, 2, 1, 0]:
+                for v in [3, 2, 1, 0]:
 
-                    file.write(f'v {" ".join(map(str, points[i]))}\n')
+                    file.write(f'v {" ".join(map(str, place.corners[v]))}\n')
                     v_count += 1
 
-                    vertices += f' {v_count}'
+                    vertices += f' {v_count}//{p+1}'
+
+                # Write normal vector
+                nvec = numpy.cross(place.corners[-1] - place.corners[0], place.corners[1] - place.corners[0])
+                nvec = nvec / numpy.linalg.norm(nvec)
+                file.write(f'vn {nvec[0]} {nvec[1]} {nvec[2]}\n')
 
-                file.write('s off\n')
+                #file.write('s off\n')
                 file.write(f'f{vertices}\n')