Collision detection

CMakeLists.txt

@@ -179,6 +179,18 @@ if(NOT DEFINED Autoscoper_EXECUTABLE_LINK_FLAGS)
 endif()
 mark_as_superbuild(Autoscoper_EXECUTABLE_LINK_FLAGS:STRING)
 
+#-----------------------------------------------------------------------------
+# Collision Detection
+#-----------------------------------------------------------------------------
+if(DEFINED VTK_DIR AND EXISTS ${VTK_DIR})
+  message(STATUS "Configuring with collision detection")
+  set(Autoscoper_COLLISION_DETECTION 1)
+else()
+  message(STATUS "Configuring without collision detection. If you want to enable collision detection, please set VTK_DIR.")
+  set(Autoscoper_COLLISION_DETECTION 0)
+endif()
+mark_as_superbuild(Autoscoper_COLLISION_DETECTION)
+
 #-----------------------------------------------------------------------------
 # Dependencies
 #-----------------------------------------------------------------------------
@@ -252,6 +264,9 @@ if(Autoscoper_CONFIGURE_LAUCHER_SCRIPT)
       endif()
     endif()
   endforeach()
+  if(Autoscoper_COLLISION_DETECTION)
+    list(APPEND _library_paths "${VTK_DIR}/bin/$<CONFIG>/")
+  endif()
 
   string(REPLACE ";" "${_pathsep}" PATHS_CONFIG "${_library_paths}")
 
@@ -283,6 +298,9 @@ if(Autoscoper_INSTALL_DEPENDENCIES)
   foreach(dependency IN LISTS Autoscoper_DEPENDENCIES)
     install(SCRIPT "${Autoscoper_SUPERBUILD_DIR}/${dependency}-build/cmake_install.cmake")
   endforeach()
+  if(Autoscoper_COLLISION_DETECTION)
+    install(SCRIPT "${VTK_DIR}/cmake_install.cmake")
+  endif()
 endif()
 
 #-----------------------------------------------------------------------------

SuperBuild.cmake

@@ -34,6 +34,7 @@ ExternalProject_Add(${proj}
     -DCMAKE_CXX_STANDARD:STRING=${CMAKE_CXX_STANDARD}
     -DCMAKE_CXX_STANDARD_REQUIRED:BOOL=${CMAKE_CXX_STANDARD_REQUIRED}
     -DQt5_DIR:PATH=${Qt5_DIR}
+    -DVTK_DIR:PATH=${VTK_DIR}
     # Options
     -DAutoscoper_SUPERBUILD:BOOL=OFF
     -DAutoscoper_SUPERBUILD_DIR:PATH=${CMAKE_BINARY_DIR}

libautoscoper/CMakeLists.txt

@@ -60,6 +60,15 @@ list(APPEND libautoscoper_SOURCES
   src/filesystem_compat.cpp
   )
 
+if(Autoscoper_COLLISION_DETECTION) # Add collision detection sources
+  list(APPEND libautoscoper_SOURCES
+    src/Mesh.cpp
+  )
+  list(APPEND libautoscoper_HEADERS
+    src/Mesh.hpp
+  )
+endif()
+
 if(Autoscoper_RENDERING_BACKEND STREQUAL "CUDA")
   # CUDA 10.2 supports C++ up to version 14
   # See https://docs.nvidia.com/cuda/archive/10.2/cuda-c-programming-guide/index.html#c-cplusplus-language-support
@@ -125,6 +134,27 @@ target_compile_definitions(libautoscoper PUBLIC
   Autoscoper_RENDERING_USE_${Autoscoper_RENDERING_BACKEND}_BACKEND
 )
 
+if (Autoscoper_COLLISION_DETECTION) # Add definitions for collision detection
+  target_compile_definitions(libautoscoper PUBLIC
+    Autoscoper_COLLISION_DETECTION
+  )
+  find_package(VTK COMPONENTS
+    CommonCore
+    CommonSystem
+    FiltersSources
+    FiltersModeling
+    IOGeometry
+  )
+  if(NOT VTK_FOUND)
+    message(FATAL_ERROR "VTK was not found")
+  endif()
+  target_link_libraries(libautoscoper PUBLIC ${VTK_LIBRARIES})
+  vtk_module_autoinit(
+    TARGETS libautoscoper
+    MODULES ${VTK_LIBRARIES}
+  )
+endif()
+
 set_target_properties(libautoscoper PROPERTIES
   RUNTIME_OUTPUT_NAME "libautoscoper${Autoscoper_ARTIFACT_SUFFIX}"
   LIBRARY_OUTPUT_NAME "libautoscoper${Autoscoper_ARTIFACT_SUFFIX}"

libautoscoper/src/Mesh.cpp

@@ -0,0 +1,88 @@
+#include "Mesh.hpp"
+#include <vtkSTLReader.h>
+#include <vtkSTLWriter.h>
+#include <vtkTransform.h>
+#include <vtkTransformPolyDataFilter.h>
+#include <vtkCenterOfMass.h>
+
+Mesh::Mesh(const std::string& filename)
+{
+  this->filename = filename;
+
+  vtkSTLReader* reader = vtkSTLReader::New();
+  this->polyData = vtkPolyData::New();
+
+  reader->SetFileName(filename.c_str());
+  reader->Update();
+  this->polyData = reader->GetOutput();
+
+  // Create OBB Tree
+  this->meshOBB = vtkOBBTree::New();
+  // this->meshOBB->SetDataSet(this->polyData);
+  this->meshOBB->SetMaxLevel(1);
+
+  double corner[3];
+  double maxAxis[3];
+  double midAxis[3];
+  double minAxis[3];
+  double size[3];
+
+  this->meshOBB->ComputeOBB(this->polyData, corner, maxAxis, midAxis, minAxis, size);
+
+  /*std::cout << "corner = " << corner[0] << ", " << corner[1] << ", " << corner[2] << std::endl;
+  std::cout << "maxAxis = " << maxAxis[0] << ", " << maxAxis[1] << ", " << maxAxis[2] << std::endl;
+  std::cout << "midAxis = " << midAxis[0] << ", " << midAxis[1] << ", " << midAxis[2] << std::endl;
+  std::cout << "minAxis = " << minAxis[0] << ", " << minAxis[1] << ", " << minAxis[2] << std::endl;
+  std::cout << "size = " << size[0] << ", " << size[1] << ", " << size[2] << std::endl;*/
+
+  /*this->meshOBB->Update();
+  this->meshOBB->PrintSelf(std::cout, vtkIndent(2));*/
+
+  // Register so it exists after deleting the reader
+  this->polyData->Register(nullptr);
+
+  // Compute bounding radius
+  double centerA[3];
+  this->polyData->GetCenter(centerA);
+
+  double bounds[6];
+  this->polyData->GetBounds(bounds);
+
+  boundingRadius = (bounds[1] - centerA[0]) * (bounds[1] - centerA[0])
+                   + (bounds[3] - centerA[1]) * (bounds[3] - centerA[1])
+                   + (bounds[5] - centerA[2]) * (bounds[5] - centerA[2]);
+
+  boundingRadius = sqrt(boundingRadius);
+
+  std::cout << "Mesh " << filename << ", has a bounding radius of: " << boundingRadius << std::endl;
+
+  reader->Delete();
+}
+
+// Helper function to transform mesh after reading it in.  Physically moves the mesh to the new transformed position.
+void Mesh::Transform(double xAngle, double yAngle, double zAngle, double shiftX, double shiftY, double shiftZ)
+{
+  vtkNew<vtkTransform> transform;
+  // Shift in Y to overlay tiff (double bounding box center
+  transform->Translate(shiftX, shiftY, shiftZ);
+  transform->RotateX(xAngle);
+  transform->RotateY(yAngle);
+  transform->RotateZ(zAngle);
+
+  vtkNew<vtkTransformPolyDataFilter> transformFilter;
+  transformFilter->SetInputData(this->polyData);
+  transformFilter->SetTransform(transform);
+  transformFilter->Update();
+
+  this->polyData = transformFilter->GetOutput();
+  this->polyData->Register(nullptr);
+}
+
+void Mesh::Write(const std::string& filename) const
+{
+  vtkSTLWriter* writer = vtkSTLWriter::New();
+  writer->SetFileName(filename.c_str());
+  writer->SetInputData(this->polyData);
+  writer->Write();
+  writer->Delete();
+}

libautoscoper/src/Mesh.hpp

@@ -0,0 +1,25 @@
+#pragma once
+#include <string>
+#include <vtkPolyData.h>
+#include <vtkOBBTree.h>
+
+class Mesh
+{
+public:
+  Mesh(const std::string& filename);
+
+  vtkPolyData* GetPolyData() const { return this->polyData; }
+
+  void Write(const std::string& filename) const;
+
+  void Transform(double xAngle, double yAngle, double zAngle, double shiftX, double shiftY, double shiftZ);
+
+  double getBoundingRadius() const { return boundingRadius; }
+
+private:
+  double boundingRadius = 0.0;
+  std::string filename;
+
+  vtkPolyData* polyData;
+  vtkOBBTree* meshOBB;
+};

libautoscoper/src/PSO.cpp

@@ -3,26 +3,40 @@
 #include <cfloat> // For FLT_MAX
 #include <string>
 
+#define VELOCITY_FILTER 0
+#define COLLISION_RESPONSE 0
+
 // Particle Struct Function Definitions
 Particle::Particle(const Particle& p)
 {
   this->NCC = p.NCC;
   this->Position = p.Position;
   this->Velocity = p.Velocity;
+#ifdef Autoscoper_COLLISION_DETECTION
+  this->collided = p.collided;
+#endif // Autoscoper_COLLISION_DETECTION
 }
 
 Particle::Particle()
 {
   this->NCC = FLT_MAX;
   this->Position = std::vector<float>(NUM_OF_DIMENSIONS, 0.f);
   this->Velocity = std::vector<float>(NUM_OF_DIMENSIONS, 0.f);
+
+#ifdef Autoscoper_COLLISION_DETECTION
+  this->collided = false;
+#endif // Autoscoper_COLLISION_DETECTION
 }
 
 Particle::Particle(const std::vector<float>& pos)
 {
   this->NCC = FLT_MAX;
   this->Position = pos;
   this->Velocity = std::vector<float>(NUM_OF_DIMENSIONS, 0.f);
+
+#ifdef Autoscoper_COLLISION_DETECTION
+  this->collided = false;
+#endif // Autoscoper_COLLISION_DETECTION
 }
 
 Particle::Particle(float start_range_min, float start_range_max)
@@ -31,13 +45,22 @@ Particle::Particle(float start_range_min, float start_range_max)
   this->Position = std::vector<float>(NUM_OF_DIMENSIONS, 0.f);
   this->Velocity = std::vector<float>(NUM_OF_DIMENSIONS, 0.f);
   this->initializePosition(start_range_min, start_range_max);
+
+#ifdef Autoscoper_COLLISION_DETECTION
+  this->collided = false;
+#endif // Autoscoper_COLLISION_DETECTION
 }
 
 Particle& Particle::operator=(const Particle& p)
 {
   this->NCC = p.NCC;
   this->Position = p.Position;
   this->Velocity = p.Velocity;
+
+#ifdef Autoscoper_COLLISION_DETECTION
+  this->collided = p.collided;
+#endif // Autoscoper_COLLISION_DETECTION
+
   return *this;
 }
 
@@ -69,6 +92,21 @@ void Particle::updateVelocityAndPosition(const Particle& pBest, const Particle&
     this->Velocity[dim] = omega * this->Velocity[dim] + c1 * rp * (pBest.Position[dim] - this->Position[dim])
                           + c2 * rg * (gBest.Position[dim] - this->Position[dim]);
 
+#ifdef VELOCITY_FILTER
+    float velClamp = 0.2;
+    float speed = 0.0;
+    for (int i = 0; i < NUM_OF_DIMENSIONS; i++) {
+      speed += this->Velocity[i] * this->Velocity[i];
+    }
+    speed = sqrt(speed);
+    if (speed > velClamp) {
+      for (int i = 0; i < NUM_OF_DIMENSIONS; i++) {
+        this->Velocity[i] /= speed;
+        this->Velocity[i] *= velClamp;
+      }
+    }
+#endif // VELOCITY_FILTER
+
     this->Position[dim] += this->Velocity[dim];
   }
 }
@@ -123,6 +161,40 @@ float getRandomClamped()
   return (float)rand() / (float)RAND_MAX;
 }
 
+#ifdef Autoscoper_COLLISION_DETECTION
+void checkCollision(Particle& p,
+                    std::vector<float>& avgNonCollidedPosition,
+                    std::vector<float>& avgCollidedPosition,
+                    unsigned int& collidedCount)
+{
+  if (p.NCC > 1.0E4) {
+    collidedCount++;
+    p.collided = true;
+
+    for (int i = 0; i < NUM_OF_DIMENSIONS; ++i) {
+      avgCollidedPosition[i] += p.Position[i];
+    }
+  } else {
+    for (int i = 0; i < NUM_OF_DIMENSIONS; ++i) {
+      avgNonCollidedPosition[i] += p.Position[i];
+    }
+  }
+}
+
+void computeCorrectionVector(std::vector<float>& avgNonCollidedPosition,
+                             std::vector<float>& avgCollidedPosition,
+                             std::vector<float>& correctionVector,
+                             const unsigned int& collidedCount)
+{
+  if (collidedCount != 0 && collidedCount != NUM_OF_PARTICLES) {
+    for (int i = 0; i < NUM_OF_DIMENSIONS; ++i) {
+      avgNonCollidedPosition[i] /= ((float)NUM_OF_PARTICLES - (float)collidedCount);
+      avgCollidedPosition[i] /= (float)collidedCount;
+    }
+  }
+}
+#endif // Autoscoper_COLLISION_DETECTION
+
 Particle pso(float start_range_min, float start_range_max, unsigned int MAX_EPOCHS, unsigned int MAX_STALL)
 {
   int stall_iter = 0;
@@ -154,6 +226,12 @@ Particle pso(float start_range_min, float start_range_max, unsigned int MAX_EPOC
     }
 
     currentBest = gBest;
+#ifdef Autoscoper_COLLISION_DETECTION
+    unsigned int collidedCount = 0;
+    std::vector<float> avgNonCollidedPosition = std::vector<float>(NUM_OF_DIMENSIONS, 0.f);
+    std::vector<float> avgCollidedPosition = std::vector<float>(NUM_OF_DIMENSIONS, 0.f);
+    std::vector<float> correctionVector = std::vector<float>(NUM_OF_DIMENSIONS, 0.f);
+#endif // Autoscoper_COLLISION_DETECTION
 
     for (int idx = 0; idx < NUM_OF_PARTICLES; idx++) {
 
@@ -172,7 +250,23 @@ Particle pso(float start_range_min, float start_range_max, unsigned int MAX_EPOC
       if (particles[idx].NCC < gBest.NCC) {
         gBest = particles[idx];
       }
+#ifdef Autoscoper_COLLISION_DETECTION
+      checkCollision(particles[idx], avgNonCollidedPosition, avgCollidedPosition, collidedCount);
+#endif // Autoscoper_COLLISION_DETECTION
+    }
+
+#if defined(Autoscoper_COLLISION_DETECTION) and defined(COLLISION_RESPONSE)
+    computeCorrectionVector(avgNonCollidedPosition, avgCollidedPosition, correctionVector, collidedCount);
+    for (int idx = 0; idx < NUM_OF_PARTICLES; ++idx) {
+      if (particles[idx].collided) {
+        for (int j = 0; j < NUM_OF_DIMENSIONS; ++j) {
+          particles[idx].Position[j] += ((float)collidedCount / (float)NUM_OF_PARTICLES) * correctionVector[j];
+          pBest[idx] = particles[idx];
+          // Why always update the pBest here?
+        }
+      }
     }
+#endif // Autoscoper_COLLISION_DETECTION and COLLISION_RESPONSE
 
     OMEGA = OMEGA * 0.9f;
 

libautoscoper/src/PSO.hpp

@@ -34,6 +34,10 @@ struct Particle
   std::vector<float> Position;
   std::vector<float> Velocity;
 
+#ifdef Autoscoper_COLLISION_DETECTION
+  bool collided;
+#endif // Autoscoper_COLLISION_DETECTION
+
   // Copy constructor
   Particle(const Particle& p);
   // Default constructor
@@ -52,3 +56,15 @@ extern std::ostream& operator<<(std::ostream& os, const std::vector<float>& valu
 extern std::ostream& operator<<(std::ostream& os, const Particle& p);
 
 Particle pso(float start_range_min, float start_range_max, unsigned int MAX_EPOCHS, unsigned int MAX_STALL);
+
+#ifdef Autoscoper_COLLISION_DETECTION
+void checkCollision(Particle& p,
+                    std::vector<float>& avgNonCollidedPosition,
+                    std::vector<float>& avgCollidedPosition,
+                    unsigned int& collidedCount);
+
+void computeCorrectionVector(std::vector<float>& avgNonCollidedPosition,
+                             std::vector<float>& avgCollidedPosition,
+                             std::vector<float>& correctionVector,
+                             const unsigned int& collidedCount);
+#endif // Autoscoper_COLLISION_DETECTION