WIP: Support collision detection

BrownBiomechanics · Aug 15, 2024 · b06f8aa · b06f8aa
1 parent 2fdded0
commit b06f8aa
Show file tree

Hide file tree

Showing 8 changed files with 372 additions and 13 deletions.
diff --git a/libautoscoper/CMakeLists.txt b/libautoscoper/CMakeLists.txt
@@ -142,6 +142,7 @@ if (Autoscoper_COLLISION_DETECTION) # Add definitions for collision detection
     CommonCore
     CommonSystem
     FiltersSources
+    FiltersModeling
     IOGeometry
   )
   if(NOT VTK_FOUND)

diff --git a/libautoscoper/src/Mesh.cpp b/libautoscoper/src/Mesh.cpp
@@ -1,20 +1,81 @@
 #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();
 }
 
-Mesh::Mesh(const Mesh& other)
+// 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)
 {
-  this->polyData = vtkPolyData::New();
-  this->polyData->DeepCopy(other.polyData);
+  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

diff --git a/libautoscoper/src/Mesh.hpp b/libautoscoper/src/Mesh.hpp
@@ -1,17 +1,25 @@
 #pragma once
 #include <string>
 #include <vtkPolyData.h>
+#include <vtkOBBTree.h>
 
 class Mesh
 {
 public:
   Mesh(const std::string& filename);
-  Mesh(const Mesh&);
 
   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;
+};
diff --git a/libautoscoper/src/PSO.cpp b/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;
 

diff --git a/libautoscoper/src/PSO.hpp b/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