text:
// ---------------------------------------------------------
//
//  dynamicsurface.cpp
//  Tyson Brochu 2008
//  Christopher Batty, Fang Da 2014
//
//  A triangle mesh with associated vertex locations and  masses.  Query functions for getting geometry info.
//
// ---------------------------------------------------------

// ---------------------------------------------------------
// Includes
// ---------------------------------------------------------

#include <dynamicsurface.h>
#include <Eigen/Core>
#include <Eigen/Eigenvalues>
#include <broadphasegrid.h>
#include <cassert>
#include <ccd_wrapper.h>
#include <collisionpipeline.h>
#include <collisionqueries.h>
#include <ctime>
#include <impactzonesolver.h>
#include <iomesh.h>
#include <mat.h>
#include <queue>
#include <runstats.h>
#include <vec.h>
#include <vector>
#include <wallclocktime.h>

// ---------------------------------------------------------
// Local constants, typedefs, macros
// ---------------------------------------------------------

// ---------------------------------------------------------
//  Extern globals
// ---------------------------------------------------------

namespace LosTopos {

extern RunStats g_stats;

// ---------------------------------------------------------
// Static function definitions
// ---------------------------------------------------------

// ---------------------------------------------------------
// Member function definitions
// ---------------------------------------------------------

// ---------------------------------------------------------
///
/// DynamicSurface constructor.  Copy triangles and vertex locations.
///
// ---------------------------------------------------------

DynamicSurface::DynamicSurface(const std::vector<Vec3d>& vertex_positions,
   const std::vector<Vec3st>& triangles,
   const std::vector<Vec2i>& labels,
   const std::vector<Vec3d>& masses,
   double in_proximity_epsilon,
   double in_friction_coefficient,
   bool in_collision_safety,
   bool in_collision_safety_asserts,
   bool in_verbose) :
   m_proximity_epsilon(in_proximity_epsilon),
   m_verbose(in_verbose),
   m_collision_safety(in_collision_safety),
   m_collision_safety_asserts( m_collision_safety_asserts ),
m_masses( masses ),
m_mesh(),
m_broad_phase( new BroadPhaseGrid() ),
m_collision_pipeline( NULL ),    // allocated and initialized in the constructor body
m_aabb_padding( max( in_proximity_epsilon, 1e-4 ) ),
pm_positions(vertex_positions),
pm_newpositions(vertex_positions),
pm_velocities(vertex_positions.size(),Vec3d(0,0,0)),
m_velocities(vertex_positions.size()),
m_feature_edge_angle_threshold(M_PI/6)
{

    if ( m_verbose )
    {
        std::cout << "constructing dynamic surface" << std::endl;
    }

    // if masses not provided, set all to 1.0
    if ( m_masses.size() == 0 )
    {
        m_masses.resize( get_num_vertices(), Vec3d(1.0, 1.0, 1.0) );
    }

    assert(triangles.size() == labels.size());

    m_mesh.set_num_vertices( get_num_vertices() );
    m_mesh.replace_all_triangles( triangles, labels );

    // Some compilers worry about using "this" in the initialization list, so initialize it here
    m_collision_pipeline = new CollisionPipeline( *this, *m_broad_phase, in_friction_coefficient );

    if ( m_verbose )
    {
        std::cout << "constructed dynamic surface" << std::endl;
    }

}

// ---------------------------------------------------------
///
/// Destructor. Frees memory allocated by DynamicSurface for the broad phase and collision pipeline objects.
///
// ---------------------------------------------------------

DynamicSurface::~DynamicSurface()
{
    delete m_broad_phase;
    delete m_collision_pipeline;
}

// ---------------------------------------------------------
///
/// Compute the unsigned distance to the surface.
///
// ---------------------------------------------------------

double DynamicSurface::distance_to_surface( const Vec3d& p, size_t& closest_triangle ) const
{

    double padding = m_aabb_padding;
    double min_distance = BIG_DOUBLE;

    while ( min_distance == BIG_DOUBLE )
    {

        Vec3d xmin( p - Vec3d( padding ) );
        Vec3d xmax( p + Vec3d( padding ) );

        std::vector<size_t> nearby_triangles;

        m_broad_phase->get_potential_triangle_collisions( xmin, xmax, true, true, nearby_triangles );

        for ( size_t j = 0; j < nearby_triangles.size(); ++j )
        {
            const Vec3st& tri = m_mesh.get_triangle( nearby_triangles[j] );

            if ( tri[0] == tri[1] || tri[1] == tri[2] || tri[0] == tri[2] ) { continue; }

            double curr_distance;
            check_point_triangle_proximity( p, get_position(tri[0]), get_position(tri[1]), get_position(tri[2]), curr_distance );
            if ( curr_distance < padding )
            {
                min_distance = min( min_distance, curr_distance );
                closest_triangle = nearby_triangles[j];
            }
        }

        padding *= 2.0;

    }

    return min_distance;

}

// --------------------------------------------------------
///
/// Break up the triangle mesh into connected components, determine surface IDs for all vertices.
///
// --------------------------------------------------------

void DynamicSurface::partition_surfaces( std::vector<size_t>& surface_ids, std::vector< std::vector< size_t> >& surfaces ) const
{

    static const size_t UNASSIGNED = (size_t) ~0;

    surfaces.clear();

    surface_ids.clear();
    surface_ids.resize( get_num_vertices(), UNASSIGNED );

    size_t curr_surface = 0;

    while ( true )
    {
        size_t next_unassigned_vertex;
        for ( next_unassigned_vertex = 0; next_unassigned_vertex < surface_ids.size(); ++next_unassigned_vertex )
        {
            if ( m_mesh.m_vertex_to_edge_map[next_unassigned_vertex].empty() ) { continue; }

            if ( surface_ids[next_unassigned_vertex] == UNASSIGNED )
            {
                break;
            }
        }

        if ( next_unassigned_vertex == surface_ids.size() )
        {
            break;
        }

        std::queue<size_t> open;
        open.push( next_unassigned_vertex );

        std::vector<size_t> surface_vertices;

        while ( false == open.empty() )
        {
            size_t vertex_index = open.front();
            open.pop();

            if ( m_mesh.m_vertex_to_edge_map[vertex_index].empty() ) { continue; }

            if ( surface_ids[vertex_index] != UNASSIGNED )
            {
                assert( surface_ids[vertex_index] == curr_surface );
                continue;
            }

            surface_ids[vertex_index] = curr_surface;
            surface_vertices.push_back( vertex_index );

            const std::vector<size_t>& incident_edges = m_mesh.m_vertex_to_edge_map[vertex_index];

            for( size_t i = 0; i < incident_edges.size(); ++i )
            {
                size_t adjacent_vertex = m_mesh.m_edges[ incident_edges[i] ][0];
                if ( adjacent_vertex == vertex_index ) { adjacent_vertex = m_mesh.m_edges[ incident_edges[i] ][1]; }

                if ( surface_ids[adjacent_vertex] == UNASSIGNED )
                {
                    open.push( adjacent_vertex );
                }
                else
                {
                    assert( surface_ids[adjacent_vertex] == curr_surface );
                }

            }
        }

        surfaces.push_back( surface_vertices );

        ++curr_surface;

    }

    //
    // assert all vertices are assigned and share volume IDs with their neighbours
    //

    for ( size_t i = 0; i < surface_ids.size(); ++i )
    {
        if ( m_mesh.m_vertex_to_edge_map[i].empty() ) { continue; }

        assert( surface_ids[i] != UNASSIGNED );

        const std::vector<size_t>& incident_edges = m_mesh.m_vertex_to_edge_map[i];
        for( size_t j = 0; j < incident_edges.size(); ++j )
        {
            size_t adjacent_vertex = m_mesh.m_edges[ incident_edges[j] ][0];
            if ( adjacent_vertex == i ) { adjacent_vertex = m_mesh.m_edges[ incident_edges[j] ][1]; }
            assert( surface_ids[adjacent_vertex] == surface_ids[i] );
        }

    }

}

// --------------------------------------------------------
///
/// Compute all vertex normals (unweighted average).
///
// --------------------------------------------------------

void DynamicSurface::get_all_vertex_normals( std::vector<Vec3d>& normals ) const
{
    normals.resize( get_num_vertices() );
    for ( size_t i = 0; i < get_num_vertices(); ++i )
    {
        normals[i] = get_vertex_normal(i);
    }
}

// ---------------------------------------------------------
///
/// Run intersection detection against all triangles
///
// ---------------------------------------------------------

void DynamicSurface::get_triangle_intersections( const Vec3d& segment_point_a,
                                                const Vec3d& segment_point_b,
                                                std::vector<double>& hit_ss,
                                                std::vector<size_t>& hit_triangles,
                                                bool verbose ) const
{
    Vec3d aabb_low, aabb_high;
    minmax( segment_point_a, segment_point_b, aabb_low, aabb_high );

    //TODO Use some kind of line-drawing algorithm to find a sequence of acceleration grid cells that overlap/enclose
    //the segment, to avoid having to search massive chunks of the mesh.

	static std::vector<size_t> overlapping_triangles(10);
	overlapping_triangles.clear();
    m_broad_phase->get_potential_triangle_collisions( aabb_low, aabb_high, true, true, overlapping_triangles );

    for ( size_t i = 0; i < overlapping_triangles.size(); ++i )
    {
        const Vec3st& tri = m_mesh.get_triangle( overlapping_triangles[i] );

        Vec3st t = tri;
        //Vec3st t = sort_triangle( tri );
        //assert( t[0] < t[1] && t[0] < t[2] && t[1] < t[2] );

        const Vec3d& v0 = get_position( t[0] );
        const Vec3d& v1 = get_position( t[1] );
        const Vec3d& v2 = get_position( t[2] );

        size_t dummy_index = get_num_vertices();

        double bary1, bary2, bary3;
        Vec3d normal;
        double sa, sb;

        bool hit = segment_triangle_intersection(segment_point_a, dummy_index,
                                                 segment_point_b, dummy_index+1,
                                                 v0, t[0],
                                                 v1, t[1],
                                                 v2, t[2],
                                                 sa, sb, bary1, bary2, bary3,
                                                 false, verbose );

        if ( hit )
        {
            hit_ss.push_back( sb );
            hit_triangles.push_back( overlapping_triangles[i] );
        }

    }

}

// ---------------------------------------------------------
///
/// Run intersection detection against all triangles and return the number of hits.
///
// ---------------------------------------------------------

size_t DynamicSurface::get_number_of_triangle_intersections( const Vec3d& segment_point_a,
                                                            const Vec3d& segment_point_b ) const
{
    int num_hits = 0;
    int num_misses = 0;
    Vec3d aabb_low, aabb_high;
    minmax( segment_point_a, segment_point_b, aabb_low, aabb_high );

    std::vector<size_t> overlapping_triangles;
    m_broad_phase->get_potential_triangle_collisions( aabb_low, aabb_high, true, true, overlapping_triangles );

    for ( size_t i = 0; i < overlapping_triangles.size(); ++i )
    {
        const Vec3st& tri = m_mesh.get_triangle( overlapping_triangles[i] );

        Vec3st t = tri;
        //Vec3st t = sort_triangle( tri );
        //assert( t[0] < t[1] && t[0] < t[2] && t[1] < t[2] );

        const Vec3d& v0 = get_position( t[0] );
        const Vec3d& v1 = get_position( t[1] );
        const Vec3d& v2 = get_position( t[2] );

        size_t dummy_index = get_num_vertices();
        static const bool degenerate_counts_as_hit = true;

        bool hit = segment_triangle_intersection( segment_point_a, dummy_index,
                                                 segment_point_b, dummy_index + 1,
                                                 v0, t[0],
                                                 v1, t[1],
                                                 v2, t[2],
                                                 degenerate_counts_as_hit );

        if ( hit )
        {
            ++num_hits;
        }
        else
        {
            ++num_misses;
        }
    }

    return num_hits;

}

// ---------------------------------------------------------
///
/// Compute rank of the quadric metric tensor at a vertex
///
// ---------------------------------------------------------

unsigned int DynamicSurface::vertex_primary_space_rank( size_t v, int region ) const
{
   if ( m_mesh.m_vertex_to_triangle_map[v].empty() )     { return 0; }

   const std::vector<size_t>& incident_triangles = m_mesh.m_vertex_to_triangle_map[v];

   //Check how many labels we have
   std::set<int> labelset;
   for(size_t i = 0; i < incident_triangles.size(); ++i) {
      Vec2i label = m_mesh.get_triangle_label(incident_triangles[i]);
      labelset.insert(label[0]);
      labelset.insert(label[1]);
   }

   //If manifold, just do the easy/cheap case.
   if(labelset.size() <= 2) {
      return compute_rank_from_triangles(incident_triangles);
   }
   else if(region != -1) {
      //if requesting rank for just a specific manifold component...

      //collect all the triangles with the relevant label
      std::vector<size_t> cur_tri_set;
      for(size_t i = 0; i < incident_triangles.size(); ++i) {
         Vec2i label = m_mesh.get_triangle_label(incident_triangles[i]);
         if(label[0] == region || label[1] == region)
            cur_tri_set.push_back(incident_triangles[i]);
      }
      return compute_rank_from_triangles(cur_tri_set);
   }
   else {
      //Otherwise, visit each set of the manifold regions separately, take the max.
      //This seems to work somewhat better than doing all at once in the non-manifold case.
      unsigned int max_rank = 0;
      for(std::set<int>::iterator it = labelset.begin(); it != labelset.end(); ++it) {
         int cur_label = *it;

         //collect all the triangles with the relevant label
         std::vector<size_t> cur_tri_set;
         for(size_t i = 0; i < incident_triangles.size(); ++i) {
            Vec2i label = m_mesh.get_triangle_label(incident_triangles[i]);
            if(label[0] == cur_label || label[1] == cur_label)
               cur_tri_set.push_back(incident_triangles[i]);
         }

         unsigned int rank = compute_rank_from_triangles(cur_tri_set);
         max_rank = max(rank, max_rank);
      }

      return max_rank;
   }
}

unsigned int DynamicSurface::compute_rank_from_triangles(const std::vector<size_t>& triangles) const {

   Eigen::Matrix3d A_Eigen;
   A_Eigen.setZero();

   for ( size_t i = 0; i < triangles.size(); ++i )
   {
      size_t triangle_index = triangles[i];
      Vec3d normal = get_triangle_normal(triangle_index);
      double w = get_triangle_area(triangle_index);

      A_Eigen(0, 0) += normal[0] * w * normal[0];
      A_Eigen(1, 0) += normal[1] * w * normal[0];
      A_Eigen(2, 0) += normal[2] * w * normal[0];

      A_Eigen(0, 1) += normal[0] * w * normal[1];
      A_Eigen(1, 1) += normal[1] * w * normal[1];
      A_Eigen(2, 1) += normal[2] * w * normal[1];

      A_Eigen(0, 2) += normal[0] * w * normal[2];
      A_Eigen(1, 2) += normal[1] * w * normal[2];
      A_Eigen(2, 2) += normal[2] * w * normal[2];
   }

   Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> es;
   es.compute(A_Eigen, false);
   if (es.info() != Eigen::Success) {
      if (m_verbose)
      {
         std::cout << "Eigen decomposition failed.  Incident triangles: " << std::endl;
         for (size_t i = 0; i < triangles.size(); ++i)
         {
            size_t triangle_index = triangles[i];
            Vec3d normal = get_triangle_normal(triangle_index);
            double w = get_triangle_area(triangle_index);

            std::cout << "normal: ( " << normal << " )    ";
            std::cout << "area: " << w << std::endl;
         }
      }
      return 4;
   }
   auto eigenvalues_Eigen = es.eigenvalues();
   double max_eig = std::max(eigenvalues_Eigen[0], std::max(eigenvalues_Eigen[1], eigenvalues_Eigen[2]));

   // compute rank of primary space
   unsigned int rank = 0;
   for ( unsigned int i = 0; i < 3; ++i )
   {
      if ( eigenvalues_Eigen[i] > G_EIGENVALUE_RANK_RATIO * max_eig )
      {
         ++rank;
      }
   }
   return rank;
}

/// Look at all triangle pairs and get the smallest angle, ignoring regions.
double DynamicSurface::get_largest_dihedral(size_t edge) const {
   const std::vector<size_t>& tri_list = m_mesh.m_edge_to_triangle_map[edge];

   //consider all triangle pairs
   size_t v0 = m_mesh.m_edges[edge][0];
   size_t v1 = m_mesh.m_edges[edge][1];

   double largest_angle = 0;
   for(size_t i = 0; i < tri_list.size(); ++i) {
      size_t tri_id0 = tri_list[i];
      Vec3d norm0 = get_triangle_normal(tri_id0);
      for(size_t j = i+1; j < tri_list.size(); ++j) {
         size_t tri_id1 = tri_list[j];
         Vec3d norm1 = get_triangle_normal(tri_id1);

         //possibly flip one normal so the tris are oriented in a matching way, to get the right dihedral angle.
         if (m_mesh.oriented(v0, v1, m_mesh.get_triangle(tri_id0)) != m_mesh.oriented(v1, v0, m_mesh.get_triangle(tri_id1))) {
            norm1 = -norm1;
         }

         double angle = acos(dot(norm0,norm1));
         largest_angle = std::max(largest_angle,angle);
      }
   }

   return largest_angle;

}

/// Look at all triangle pairs and get the smallest angle, ignoring regions.
double DynamicSurface::get_largest_dihedral(size_t edge, const std::vector<Vec3d> & cached_normals) const {
   const std::vector<size_t>& tri_list = m_mesh.m_edge_to_triangle_map[edge];

   //consider all triangle pairs
   size_t v0 = m_mesh.m_edges[edge][0];
   size_t v1 = m_mesh.m_edges[edge][1];

   double largest_angle = 0;
   for(size_t i = 0; i < tri_list.size(); ++i) {
      size_t tri_id0 = tri_list[i];
      Vec3d norm0 = cached_normals[tri_id0];
      for(size_t j = i+1; j < tri_list.size(); ++j) {
         size_t tri_id1 = tri_list[j];
         Vec3d norm1 = cached_normals[tri_id1];

         //possibly flip one normal so the tris are oriented in a matching way, to get the right dihedral angle.
         if (m_mesh.oriented(v0, v1, m_mesh.get_triangle(tri_id0)) != m_mesh.oriented(v1, v0, m_mesh.get_triangle(tri_id1))) {
            norm1 = -norm1;
         }
         double angle = acos(dot(norm0,norm1));
         largest_angle = std::max(largest_angle,angle);
      }
   }

   return largest_angle;

}

// ---------------------------------------------------------
///
/// Determine whether a point is inside the volume defined by the surface.
/// Uses raycasting to find the first intersection, and then the triangle normal
/// is used to determine inside/outside. (This can readily be extended to the multi-material case.)
/// Return -1 if we can't make a safe determination
// ---------------------------------------------------------
int DynamicSurface::test_region_via_ray_and_normal(const Vec3d& p, const Vec3d& ray_end) {

   std::vector<double> hit_ss;
   std::vector<size_t> hit_tris;
   get_triangle_intersections(p, ray_end, hit_ss, hit_tris);

   int first_hit = -1;
   double near_dist = 2; //result is between 0 and 1.

   bool good_hit = false;
   for(unsigned int i = 0; i < hit_ss.size(); ++i) {
      if(hit_ss[i] < near_dist && !m_mesh.triangle_is_deleted(hit_tris[i])) {
         first_hit = i;
         near_dist = hit_ss[i];
         good_hit = true;
      }
   }

   if(!good_hit) return 0; //assume no hits means outside (region 0).

   // get the normal of this triangle, check its orientation relative to the ray
   const Vec3st& t = m_mesh.m_tris[ hit_tris[first_hit] ];
   const Vec3d& v0 = pm_positions[ t[0] ];
   const Vec3d& v1 = pm_positions[ t[1] ];
   const Vec3d& v2 = pm_positions[ t[2] ];
   Vec3d tri_normal = -cross(v2-v0, v1-v0);

   const double eps = 1e-6;
   if(mag(tri_normal) > eps)
      normalize(tri_normal);
   else
      return -1;

   Vec2i labels = m_mesh.get_triangle_label(hit_tris[first_hit]);

   Vec3d vec_dir = ray_end-p;
   normalize(vec_dir);

   if(dot(tri_normal, vec_dir) > eps)
      return labels[1];
   else if(dot(tri_normal, vec_dir) < -eps)
      return labels[0];
   else
      return -1; //uncertain case...
}

int DynamicSurface::get_region_containing_point( const Vec3d& p )
{
   //TODO Take a distance as input? To avoid the hard-coded
   //ray distance of 1000.
   double raydist = 100;
   //
   // The point is inside if the dot product between the normal of the first
   // triangle intersection and the ray direction is positive.
   // We use voting (for the moment) to enhance robustness, in the absence of a geometrically
   // exact test.
   std::map<int,int> region_counts;

   // shoot a ray in the positive-x direction
   Vec3d ray_end( p + Vec3d( raydist, 0, 0 ) );
   int region = test_region_via_ray_and_normal(p, ray_end);
   if(region != -1)
      region_counts[region]++;

   // negative x
   ray_end = p - Vec3d( raydist, 0, 0 );
   region = test_region_via_ray_and_normal(p, ray_end);
   if(region != -1)
      region_counts[region]++;

   // positive y
   ray_end = p + Vec3d( 0, raydist, 0 );
   region = test_region_via_ray_and_normal(p, ray_end);
   if(region != -1)
      region_counts[region]++;

   // negative y
   ray_end = p - Vec3d( 0, raydist, 0 );
   region = test_region_via_ray_and_normal(p, ray_end);
   if(region != -1)
      region_counts[region]++;

   // positive z
   ray_end = p + Vec3d( 0, 0, raydist );
   region = test_region_via_ray_and_normal(p, ray_end);
   if(region != -1)
      region_counts[region]++;

   // negative z
   ray_end = p - Vec3d( 0, 0, raydist );
   region = test_region_via_ray_and_normal(p, ray_end);
   if(region != -1)
      region_counts[region]++;

   int max_val = 0;
   int max_ind = -1;
   int total = 0;
   std::map<int,int>::iterator it = region_counts.begin();
   for(;it != region_counts.end(); ++it) {
      if(it->second >= max_val) {
         max_ind = it->first;
         max_val = it->second;
      }
      total += it->second;
   }
   double consistency = (double)max_val / (double)total;
   if(consistency < 0.9 || total < 4) {
      //slightly dubious case, so cast a few more random rays for good measure.
      //std::cout << "Winning region score: " << max_val << " out of 6.\n";
      std::map<int,int>::iterator it = region_counts.begin();
      /*for(;it != region_counts.end(); ++it) {
         std::cout << "   " << it->first  << " got " << it->second << std::endl;
      }*/
      static int seed = 0;
      //throw another 7 rays in random directions
      for(int i = 0; i < 7; ++i) {
         float x_dir = randhashf(seed, -1, 1); ++seed;
         float y_dir = randhashf(seed, -1, 1); ++seed;
         float z_dir = randhashf(seed, -1, 1); ++seed;
         Vec3d direction(x_dir,y_dir,z_dir);
         if(mag(direction) > 0.00001)
            normalize(direction);
         ray_end = p + raydist*direction;
         region = test_region_via_ray_and_normal(p, ray_end);
         if(region != -1)
            region_counts[region]++;
      }

      it = region_counts.begin();
      int max_val = 0;
      int max_ind = -1;
      int total = 0;
      for(;it != region_counts.end(); ++it) {
         if(it->second > max_val) {
            max_ind = it->first;
            max_val = it->second;
         }
         total += it->second;
      }
      /*std::cout << "Updated region scores: " << (double)max_val << " out of 11.\n";
      it = region_counts.begin();
      for(;it != region_counts.end(); ++it) {
         std::cout << "   " << it->first  << " got " << it->second << std::endl;
      }*/
      consistency = (double)max_val / (double)total;
      if(consistency < 0.9) std::cout << "Warning: Consistency is only " << consistency << " out of " << total << " rays.\n";
      assert(consistency > 0.6); //if we didn't even hit 60% consistently the same, this labeling is pretty dubious
   }

   return max_ind;

}

// ---------------------------------------------------------
///
/// Advance mesh by one time step
///
// ---------------------------------------------------------

void DynamicSurface::integrate( double desired_dt, double& actual_dt )
{

    if ( m_collision_safety && m_collision_safety_asserts)
    {
      std::cout << "Checking collisions before integration.\n";
      assert_mesh_is_intersection_free( false );

    }
    std::cout << "Integrating\n";
    static const bool DEGEN_DOES_NOT_COUNT = false;
    static const bool USE_NEW_POSITIONS = true;

    if ( m_verbose )
    {
        std::cout << "---------------------- Los Topos: integration and collision handling --------------------" << std::endl;
    }

    double start_time = get_time_in_seconds();

    double curr_dt = desired_dt;
    bool success = false;

    const std::vector<Vec3d> saved_predicted_positions = get_newpositions();

    while ( !success )
    {

        m_velocities.resize( get_num_vertices() );
        for(size_t i = 0; i < get_num_vertices(); i++)
        {
            m_velocities[i] = ( get_newposition(i) - get_position(i) ) / curr_dt;
        }

        // Handle proximities

        if ( m_collision_safety )
        {
            m_collision_pipeline->handle_proximities( curr_dt );
            //check_continuous_broad_phase_is_up_to_date();
        }

        if ( m_collision_safety )
        {

            // Handle continuous collisions
            bool all_collisions_handled = false;

            all_collisions_handled = m_collision_pipeline->handle_collisions( curr_dt );

            // failsafe impact zones

            ImpactZoneSolver impactZoneSolver( *this );

            bool solver_ok = all_collisions_handled;

            if ( !solver_ok )
            {
                //if ( m_verbose )
                { std::cout << "IIZ" << std::endl; }
                solver_ok = impactZoneSolver.inelastic_impact_zones( curr_dt );
            }

            if ( !solver_ok )
            {
                //if ( m_verbose )
                { std::cout << "RIZ" << std::endl; }
                // punt to rigid impact zones
                solver_ok = impactZoneSolver.rigid_impact_zones( curr_dt );
            }

            if ( !solver_ok )
            {
                // back up and try again:

                curr_dt = 0.5 * curr_dt;
                for ( size_t i = 0; i < get_num_vertices(); ++i )
                {
                    set_newposition(i, get_position(i) + 0.5 * (saved_predicted_positions[i] - get_position(i)) ) ;
                }

                continue;
            }

            // verify intersection-free predicted mesh
            std::vector<Intersection> intersections;
            get_intersections( DEGEN_DOES_NOT_COUNT, USE_NEW_POSITIONS, intersections );

            if ( !intersections.empty() )
            {
                std::cout << "Intersection in predicted mesh." << std::endl;

                if ( all_collisions_handled )
                {
                    std::cout << "Intersection in predicted mesh but handle collisions returned ok." << std::endl;
                    assert( false );
                }

                if ( m_verbose )
                {
                    std::cout << "Intersection in predicted mesh, cutting timestep." << std::endl;
                }

                // back up and try again:

                curr_dt = 0.5 * curr_dt;
                for ( size_t i = 0; i < get_num_vertices(); ++i )
                {
                    set_newposition( i, get_position(i) + 0.5 * ( saved_predicted_positions[i] - get_position(i) ) );
                }

                continue;

            }

        }

        // Set m_positions
        set_positions_to_newpositions();

        if ( m_collision_safety && m_collision_safety_asserts)
        {
            assert_mesh_is_intersection_free( DEGEN_DOES_NOT_COUNT );
        }

        actual_dt = curr_dt;

        success = true;

    }

    double end_time = get_time_in_seconds();

    static unsigned int step = 0;
    g_stats.add_per_frame_double( "DynamicSurface:integration_time_per_timestep", step, end_time - start_time );
    ++step;
    std::cout << "Done integrating\n";

}

// ---------------------------------------------------------
///
/// Construct static acceleration structure
///
// ---------------------------------------------------------

void DynamicSurface::rebuild_static_broad_phase()
{
	std::cout << "Rebuild static\n";
    assert( m_collision_safety );
    if(m_verbose)
      std::cout << "Rebuilding broad phase\n";
    m_broad_phase->update_broad_phase( *this, false );

    if(m_verbose)
      std::cout << "Done rebuilding broad phase\n";
}

// ---------------------------------------------------------
///
/// Construct continuous acceleration structure
///
// ---------------------------------------------------------

void DynamicSurface::rebuild_continuous_broad_phase()
{
	std::cout << "Rebuild continous\n";
    assert( m_collision_safety );
    m_broad_phase->update_broad_phase( *this, true );
}

// ---------------------------------------------------------
///
/// Update the broadphase elements incident to the given vertex
///
// ---------------------------------------------------------

void DynamicSurface::update_static_broad_phase( size_t vertex_index )
{
    const std::vector<size_t>& incident_tris = m_mesh.m_vertex_to_triangle_map[ vertex_index ];
    const std::vector<size_t>& incident_edges = m_mesh.m_vertex_to_edge_map[ vertex_index ];

    Vec3d low, high;
    vertex_static_bounds( vertex_index, low, high );
    m_broad_phase->update_vertex( vertex_index, low, high, vertex_is_all_solid(vertex_index) );

    for ( size_t t = 0; t < incident_tris.size(); ++t )
    {
        triangle_static_bounds( incident_tris[t], low, high );
        m_broad_phase->update_triangle( incident_tris[t], low, high, triangle_is_all_solid(incident_tris[t]) );
    }

    for ( size_t e = 0; e < incident_edges.size(); ++e )
    {
        edge_static_bounds( incident_edges[e], low, high );
        m_broad_phase->update_edge( incident_edges[e], low, high, edge_is_all_solid(incident_edges[e]) );
    }

}

// ---------------------------------------------------------
///
/// Update the broadphase elements incident to the given vertex, using current and predicted vertex positions
///
// ---------------------------------------------------------

void DynamicSurface::update_continuous_broad_phase( size_t vertex_index )
{
    assert( m_collision_safety );

    const std::vector<size_t>& incident_tris = m_mesh.m_vertex_to_triangle_map[ vertex_index ];
    const std::vector<size_t>& incident_edges = m_mesh.m_vertex_to_edge_map[ vertex_index ];

    Vec3d low, high;
    vertex_continuous_bounds( vertex_index, low, high );
    m_broad_phase->update_vertex( vertex_index, low, high, vertex_is_all_solid(vertex_index) );

    for ( size_t t = 0; t < incident_tris.size(); ++t )
    {
        triangle_continuous_bounds( incident_tris[t], low, high );
        m_broad_phase->update_triangle( incident_tris[t], low, high, triangle_is_all_solid(incident_tris[t]) );
    }

    for ( size_t e = 0; e < incident_edges.size(); ++e )
    {
        edge_continuous_bounds( incident_edges[e], low, high );
        m_broad_phase->update_edge( incident_edges[e], low, high, edge_is_all_solid(incident_edges[e]) );
    }
}

// ---------------------------------------------------------
///
/// Compute the (padded) AABB of a vertex
///
// ---------------------------------------------------------

void DynamicSurface::vertex_static_bounds(size_t v, Vec3d &xmin, Vec3d &xmax) const
{
    if ( m_mesh.m_vertex_to_triangle_map[v].empty() )
    {
        xmin = Vec3d(m_aabb_padding);
        xmax = -Vec3d(m_aabb_padding);
    }
    else
    {
        xmin = get_position(v) - Vec3d(m_aabb_padding);
        xmax = get_position(v) + Vec3d(m_aabb_padding);
    }
}

// ---------------------------------------------------------
///
/// Compute the AABB of an edge
///
// ---------------------------------------------------------

void DynamicSurface::edge_static_bounds(size_t e, Vec3d &xmin, Vec3d &xmax) const
{
    const Vec2st& edge = m_mesh.m_edges[e];
    if ( edge[0] == edge[1] )
    {
        xmin = Vec3d(m_aabb_padding);
        xmax = -Vec3d(m_aabb_padding);
    }
    else
    {
        minmax( get_position(edge[0]), get_position(edge[1]), xmin, xmax);
        xmin -= Vec3d(m_aabb_padding);
        xmax += Vec3d(m_aabb_padding);
    }
}

// ---------------------------------------------------------
///
/// Compute the AABB of a triangle
///
// ---------------------------------------------------------

void DynamicSurface::triangle_static_bounds(size_t t, Vec3d &xmin, Vec3d &xmax) const
{
    const Vec3st& tri = m_mesh.get_triangle(t);
    if ( tri[0] == tri[1] )
    {
        xmin = Vec3d(m_aabb_padding);
        xmax = -Vec3d(m_aabb_padding);
    }
    else
    {
        minmax(get_position(tri[0]), get_position(tri[1]), get_position(tri[2]), xmin, xmax);
        xmin -= Vec3d(m_aabb_padding);
        xmax += Vec3d(m_aabb_padding);
    }
}

// ---------------------------------------------------------
///
/// Compute the AABB of a continuous vertex
///
// ---------------------------------------------------------

void DynamicSurface::vertex_continuous_bounds(size_t v, Vec3d &xmin, Vec3d &xmax) const
{
    if ( m_mesh.m_vertex_to_triangle_map[v].empty() )
    {
        xmin = Vec3d(m_aabb_padding);
        xmax = -Vec3d(m_aabb_padding);
    }
    else
    {
        minmax( get_position(v), get_newposition(v), xmin, xmax);
        xmin -= Vec3d(m_aabb_padding);
        xmax += Vec3d(m_aabb_padding);
    }
}

// ---------------------------------------------------------
///
/// Compute the AABB of a continuous edge
///
// ---------------------------------------------------------

void DynamicSurface::edge_continuous_bounds(size_t e, Vec3d &xmin, Vec3d &xmax) const
{
    const Vec2st& edge = m_mesh.m_edges[e];
    if ( edge[0] == edge[1] )
    {
        xmin = Vec3d(m_aabb_padding);
        xmax = -Vec3d(m_aabb_padding);
    }
    else
    {
        minmax(get_position(edge[0]), get_newposition(edge[0]),
               get_position(edge[1]), get_newposition(edge[1]),
               xmin, xmax);
        xmin -= Vec3d(m_aabb_padding);
        xmax += Vec3d(m_aabb_padding);
    }
}

// ---------------------------------------------------------
///
/// Compute the AABB of a continuous triangle
///
// ---------------------------------------------------------

void DynamicSurface::triangle_continuous_bounds(size_t t, Vec3d &xmin, Vec3d &xmax) const
{
    const Vec3st& tri = m_mesh.get_triangle(t);
    if ( tri[0] == tri[1] )
    {
        xmin = Vec3d(m_aabb_padding);
        xmax = -Vec3d(m_aabb_padding);
    }
    else
    {
        minmax(get_position(tri[0]), get_newposition(tri[0]),
               get_position(tri[1]), get_newposition(tri[1]),
               get_position(tri[2]), get_newposition(tri[2]),
               xmin, xmax);

        xmin -= Vec3d(m_aabb_padding);
        xmax += Vec3d(m_aabb_padding);
    }
}

// ---------------------------------------------------------
///
/// Check two axis-aligned bounding boxes for intersection
///
// ---------------------------------------------------------

static bool aabbs_intersect( const Vec3d& a_xmin, const Vec3d& a_xmax, const Vec3d& b_xmin, const Vec3d& b_xmax )
{
    if ( (a_xmin[0] <= b_xmax[0] && a_xmin[1] <= b_xmax[1] && a_xmin[2] <= b_xmax[2]) &&
        (a_xmax[0] >= b_xmin[0] && a_xmax[1] >= b_xmin[1] && a_xmax[2] >= b_xmin[2]) )
    {
        return true;
    }

    return false;
}

// ---------------------------------------------------------
///
/// Caution: slow!
/// Check the consistency of the broad phase by comparing against the N^2 broadphase.
///
// ---------------------------------------------------------

void DynamicSurface::check_static_broad_phase_is_up_to_date() const
{

    // Verify by running against the n^2 broad phase

    //
    // vertex vs triangle
    //

    for ( size_t i = 0; i < get_num_vertices(); ++i )
    {
        if ( m_mesh.vertex_is_deleted(i) ) { continue; }

        // First, accelerated broad phase

        Vec3d aabb_low, aabb_high;
        vertex_static_bounds( i, aabb_low, aabb_high );

        std::vector<size_t> overlapping_triangles;
        m_broad_phase->get_potential_triangle_collisions(aabb_low, aabb_high, true, true, overlapping_triangles);

        // filter deleted triangles
        for ( int k = 0; k < (int)overlapping_triangles.size(); ++k )
        {
            if ( m_mesh.triangle_is_deleted( overlapping_triangles[k] ) )
            {
                overlapping_triangles.erase( overlapping_triangles.begin() + k );
                --k;
            }
        }

        // Second, brute force check

        std::vector<size_t> brute_force_overlapping_triangles;

        for ( size_t j = 0; j < m_mesh.num_triangles(); ++j )
        {
            if ( m_mesh.triangle_is_deleted(j) ) { continue; }

            Vec3d tri_aabb_low, tri_aabb_high;
            triangle_static_bounds( j, tri_aabb_low, tri_aabb_high );

            if ( aabbs_intersect( aabb_low, aabb_high, tri_aabb_low, tri_aabb_high ) )
            {
                brute_force_overlapping_triangles.push_back( j );
            }
        }

        assert( overlapping_triangles.size() == brute_force_overlapping_triangles.size() );

        std::sort(overlapping_triangles.begin(), overlapping_triangles.end());
        std::sort(brute_force_overlapping_triangles.begin(), brute_force_overlapping_triangles.end());

        for ( size_t k = 0; k < overlapping_triangles.size(); ++k )
        {
            assert( overlapping_triangles[k] == brute_force_overlapping_triangles[k] );
        }

    }

    //
    // edge vs edge
    //

    for ( size_t i = 0; i < m_mesh.m_edges.size(); ++i )
    {
        if ( m_mesh.edge_is_deleted(i) ) { continue; }

        // Accelerated broad phase

        Vec3d aabb_low, aabb_high;
        edge_static_bounds( i, aabb_low, aabb_high );

        std::vector<size_t> overlapping_edges;
        m_broad_phase->get_potential_edge_collisions( aabb_low, aabb_high, true, true, overlapping_edges );

        // filter deleted edges
        for ( int k = 0; k < (int)overlapping_edges.size(); ++k )
        {
            if ( m_mesh.edge_is_deleted( overlapping_edges[k] ) )
            {
                overlapping_edges.erase( overlapping_edges.begin() + k );
                --k;
            }
        }

        // Brute force
        std::vector<size_t> brute_force_overlapping_edges;
        for ( size_t j = 0; j < m_mesh.m_edges.size(); ++j )
        {
            if ( m_mesh.edge_is_deleted(j) ) { continue; }

            Vec3d edge_aabb_low, edge_aabb_high;
            edge_static_bounds( j, edge_aabb_low, edge_aabb_high );

            if ( aabbs_intersect( aabb_low, aabb_high, edge_aabb_low, edge_aabb_high ) )
            {
                brute_force_overlapping_edges.push_back( j );
            }
        }

        if ( overlapping_edges.size() != brute_force_overlapping_edges.size() )
        {

            std::cout << "edge " << i << ": " << m_mesh.m_edges[i] << std::endl;
            std::cout << "overlapping_edges.size(): " << overlapping_edges.size() << std::endl;
            for ( size_t k = 0; k < overlapping_edges.size(); ++k )
            {
                std::cout << k << ": " << overlapping_edges[k] << std::endl;
            }

            std::cout << "brute_force_overlapping_edges.size(): " << brute_force_overlapping_edges.size() << std::endl;
            for ( size_t k = 0; k < brute_force_overlapping_edges.size(); ++k )
            {
                std::cout << k << ": " << brute_force_overlapping_edges[k] << std::endl;
            }

        }

        assert( overlapping_edges.size() == brute_force_overlapping_edges.size() );

        std::sort( overlapping_edges.begin(), overlapping_edges.end() );
        std::sort( brute_force_overlapping_edges.begin(), brute_force_overlapping_edges.end() );

        for ( size_t k = 0; k < overlapping_edges.size(); ++k )
        {
            assert( overlapping_edges[k] == brute_force_overlapping_edges[k] );
        }
    }

    //
    // triangle vs vertex
    //

    for ( size_t i = 0; i < m_mesh.num_triangles(); ++i )
    {
        if ( m_mesh.triangle_is_deleted(i) ) { continue; }

        // Accelerated broad phase

        Vec3d aabb_low, aabb_high;
        triangle_static_bounds( i, aabb_low, aabb_high );

        std::vector<size_t> overlapping_vertices;
        m_broad_phase->get_potential_vertex_collisions( aabb_low, aabb_high, true, true, overlapping_vertices );

        // filter deleted vertices
        for ( int k = 0; k < (int)overlapping_vertices.size(); ++k )
        {
            if ( m_mesh.vertex_is_deleted( overlapping_vertices[k] ) )
            {
                overlapping_vertices.erase( overlapping_vertices.begin() + k );
                --k;
            }
        }

        // Brute force
        std::vector<size_t> brute_force_overlapping_vertices;
        for ( size_t j = 0; j < get_num_vertices(); ++j )
        {
            if ( m_mesh.vertex_is_deleted(j) ) { continue; }

            Vec3d vertex_aabb_low, vertex_aabb_high;
            vertex_static_bounds( j, vertex_aabb_low, vertex_aabb_high );

            if ( aabbs_intersect( aabb_low, aabb_high, vertex_aabb_low, vertex_aabb_high ) )
            {
                brute_force_overlapping_vertices.push_back( j );
            }
        }

        if ( overlapping_vertices.size() != brute_force_overlapping_vertices.size() )
        {
            std::cout << "triangle " << i << ": " << m_mesh.get_triangle(i) << std::endl;
            std::cout << "overlapping_vertices.size(): " << overlapping_vertices.size() << std::endl;
            for ( size_t k = 0; k < overlapping_vertices.size(); ++k )
            {
                std::cout << k << ": " << overlapping_vertices[k] << " --- ";
                std::cout << "is deleted: " << m_mesh.vertex_is_deleted( overlapping_vertices[k] ) << std::endl;
            }

            std::cout << "brute_force_overlapping_vertices.size(): " << brute_force_overlapping_vertices.size() << std::endl;
            for ( size_t k = 0; k < brute_force_overlapping_vertices.size(); ++k )
            {
                std::cout << k << ": " << brute_force_overlapping_vertices[k] << " --- ";
                std::cout << "is deleted: " << m_mesh.vertex_is_deleted( brute_force_overlapping_vertices[k] ) << std::endl;
            }
        }

        assert( overlapping_vertices.size() == brute_force_overlapping_vertices.size() );

        std::sort( overlapping_vertices.begin(), overlapping_vertices.end() );
        std::sort( brute_force_overlapping_vertices.begin(), brute_force_overlapping_vertices.end() );

        for ( size_t k = 0; k < overlapping_vertices.size(); ++k )
        {
            assert( overlapping_vertices[k] == brute_force_overlapping_vertices[k] );
        }

    }

}

// ---------------------------------------------------------
///
/// Caution: slow!
/// Check the consistency of the broad phase by comparing against the N^2 broadphase.  Checks using current and predicted vertex
/// positions.
///
// ---------------------------------------------------------

void DynamicSurface::check_continuous_broad_phase_is_up_to_date() const
{

    // Verify by running against the n^2 broad phase

    //
    // vertex vs triangle
    //

    for ( size_t i = 0; i < get_num_vertices(); ++i )
    {
        if ( m_mesh.vertex_is_deleted(i) ) { continue; }

        // First, accelerated broad phase

        Vec3d aabb_low, aabb_high;
        vertex_continuous_bounds( i, aabb_low, aabb_high );

        std::vector<size_t> overlapping_triangles;
        m_broad_phase->get_potential_triangle_collisions(aabb_low, aabb_high, true, true, overlapping_triangles);

        // filter deleted triangles
        for ( int k = 0; k < (int)overlapping_triangles.size(); ++k )
        {
            if ( m_mesh.triangle_is_deleted( overlapping_triangles[k] ) )
            {
                overlapping_triangles.erase( overlapping_triangles.begin() + k );
                --k;
            }
        }

        // Second, brute force check

        std::vector<size_t> brute_force_overlapping_triangles;

        for ( size_t j = 0; j < m_mesh.num_triangles(); ++j )
        {
            if ( m_mesh.triangle_is_deleted(j) ) { continue; }

            Vec3d tri_aabb_low, tri_aabb_high;
            triangle_continuous_bounds( j, tri_aabb_low, tri_aabb_high );

            if ( aabbs_intersect( aabb_low, aabb_high, tri_aabb_low, tri_aabb_high ) )
            {
                brute_force_overlapping_triangles.push_back( j );
            }
        }

        assert( overlapping_triangles.size() == brute_force_overlapping_triangles.size() );

        std::sort(overlapping_triangles.begin(), overlapping_triangles.end());
        std::sort(brute_force_overlapping_triangles.begin(), brute_force_overlapping_triangles.end());

        for ( size_t k = 0; k < overlapping_triangles.size(); ++k )
        {
            assert( overlapping_triangles[k] == brute_force_overlapping_triangles[k] );
        }

    }

    //
    // edge vs edge
    //

    for ( size_t i = 0; i < m_mesh.m_edges.size(); ++i )
    {
        if ( m_mesh.edge_is_deleted(i) ) { continue; }

        // Accelerated broad phase

        Vec3d aabb_low, aabb_high;
        edge_continuous_bounds( i, aabb_low, aabb_high );

        std::vector<size_t> overlapping_edges;
        m_broad_phase->get_potential_edge_collisions( aabb_low, aabb_high, true, true, overlapping_edges );

        // filter deleted edges
        for ( int k = 0; k < (int)overlapping_edges.size(); ++k )
        {
            if ( m_mesh.edge_is_deleted( overlapping_edges[k] ) )
            {
                overlapping_edges.erase( overlapping_edges.begin() + k );
                --k;
            }
        }

        // Brute force
        std::vector<size_t> brute_force_overlapping_edges;
        for ( size_t j = 0; j < m_mesh.m_edges.size(); ++j )
        {
            if ( m_mesh.edge_is_deleted(j) ) { continue; }

            Vec3d edge_aabb_low, edge_aabb_high;
            edge_continuous_bounds( j, edge_aabb_low, edge_aabb_high );

            if ( aabbs_intersect( aabb_low, aabb_high, edge_aabb_low, edge_aabb_high ) )
            {
                brute_force_overlapping_edges.push_back( j );
            }
        }

        if ( overlapping_edges.size() != brute_force_overlapping_edges.size() )
        {

            std::cout << "edge " << i << ": " << m_mesh.m_edges[i] << std::endl;
            std::cout << "overlapping_edges.size(): " << overlapping_edges.size() << std::endl;
            for ( size_t k = 0; k < overlapping_edges.size(); ++k )
            {
                std::cout << k << ": " << overlapping_edges[k] << std::endl;
            }

            std::cout << "brute_force_overlapping_edges.size(): " << brute_force_overlapping_edges.size() << std::endl;
            for ( size_t k = 0; k < brute_force_overlapping_edges.size(); ++k )
            {
                std::cout << k << ": " << brute_force_overlapping_edges[k] << std::endl;
            }

        }

        assert( overlapping_edges.size() == brute_force_overlapping_edges.size() );

        std::sort( overlapping_edges.begin(), overlapping_edges.end() );
        std::sort( brute_force_overlapping_edges.begin(), brute_force_overlapping_edges.end() );

        for ( size_t k = 0; k < overlapping_edges.size(); ++k )
        {
            assert( overlapping_edges[k] == brute_force_overlapping_edges[k] );
        }
    }

    //
    // triangle vs vertex
    //

    for ( size_t i = 0; i < m_mesh.num_triangles(); ++i )
    {
        if ( m_mesh.triangle_is_deleted(i) ) { continue; }

        // Accelerated broad phase

        Vec3d aabb_low, aabb_high;
        triangle_continuous_bounds( i, aabb_low, aabb_high );

        std::vector<size_t> overlapping_vertices;
        m_broad_phase->get_potential_vertex_collisions( aabb_low, aabb_high, true, true, overlapping_vertices );

        // filter deleted vertices
        for ( int k = 0; k < (int)overlapping_vertices.size(); ++k )
        {
            if ( m_mesh.vertex_is_deleted( overlapping_vertices[k] ) )
            {
                overlapping_vertices.erase( overlapping_vertices.begin() + k );
                --k;
            }
        }

        // Brute force
        std::vector<size_t> brute_force_overlapping_vertices;
        for ( size_t j = 0; j < get_num_vertices(); ++j )
        {
            if ( m_mesh.vertex_is_deleted(j) ) { continue; }

            Vec3d vertex_aabb_low, vertex_aabb_high;
            vertex_continuous_bounds( j, vertex_aabb_low, vertex_aabb_high );

            if ( aabbs_intersect( aabb_low, aabb_high, vertex_aabb_low, vertex_aabb_high ) )
            {
                brute_force_overlapping_vertices.push_back( j );
            }
        }

        if ( overlapping_vertices.size() != brute_force_overlapping_vertices.size() )
        {
            std::cout << "triangle " << i << ": " << m_mesh.get_triangle(i) << std::endl;
            std::cout << "overlapping_vertices.size(): " << overlapping_vertices.size() << std::endl;
            for ( size_t k = 0; k < overlapping_vertices.size(); ++k )
            {
                std::cout << k << ": " << overlapping_vertices[k] << " --- ";
                std::cout << "is deleted: " << m_mesh.vertex_is_deleted( overlapping_vertices[k] ) << std::endl;
            }

            std::cout << "brute_force_overlapping_vertices.size(): " << brute_force_overlapping_vertices.size() << std::endl;
            for ( size_t k = 0; k < brute_force_overlapping_vertices.size(); ++k )
            {
                std::cout << k << ": " << brute_force_overlapping_vertices[k] << " --- ";
                std::cout << "is deleted: " << m_mesh.vertex_is_deleted( brute_force_overlapping_vertices[k] ) << std::endl;

                Vec3d lo, hi;
                bool is_solid = vertex_is_all_solid( brute_force_overlapping_vertices[k] );
                m_broad_phase->get_vertex_aabb( brute_force_overlapping_vertices[k], is_solid, lo, hi );
                std::cout << "AABB: " << lo << " - " << hi << std::endl;
                std::cout << "x: " << pm_positions[brute_force_overlapping_vertices[k]] << ", new_x: " << pm_newpositions[brute_force_overlapping_vertices[k]] << std::endl;

                bool query_overlaps_broadphase_aabb = aabbs_intersect( aabb_low, aabb_high, lo, hi );
                std::cout << "query_overlaps_broadphase_aabb: " << query_overlaps_broadphase_aabb << std::endl;

                BroadPhaseGrid* grid_bf = static_cast<BroadPhaseGrid*>(m_broad_phase);

                const std::vector<Vec3st>& cells = grid_bf->m_dynamic_vertex_grid.m_elementidxs[ brute_force_overlapping_vertices[k] ];
                std::cout << "cells: " << std::endl;
                for ( size_t m = 0; m < cells.size(); ++m )
                {
                    std::cout << cells[m] << std::endl;
                }

            }
        }

        assert( overlapping_vertices.size() == brute_force_overlapping_vertices.size() );

        std::sort( overlapping_vertices.begin(), overlapping_vertices.end() );
        std::sort( brute_force_overlapping_vertices.begin(), brute_force_overlapping_vertices.end() );

        for ( size_t k = 0; k < overlapping_vertices.size(); ++k )
        {
            assert( overlapping_vertices[k] == brute_force_overlapping_vertices[k] );
        }

    }

}

// --------------------------------------------------------
///
/// Check a triangle (by index) vs all other triangles for any kind of intersection
///
// --------------------------------------------------------

bool DynamicSurface::check_triangle_vs_all_triangles_for_intersection( size_t tri_index  )
{
    return check_triangle_vs_all_triangles_for_intersection( m_mesh.get_triangle(tri_index) );
}

// --------------------------------------------------------
///
/// Check a triangle vs all other triangles for any kind of intersection
///
// --------------------------------------------------------

bool DynamicSurface::check_triangle_vs_all_triangles_for_intersection( const Vec3st& tri )
{
    bool any_intersection = false;

    static std::vector<size_t> overlapping_triangles(20);
    overlapping_triangles.clear();

    Vec3d low, high;

    minmax( get_position(tri[0]), get_position(tri[1]), low, high );
    low -= Vec3d(m_aabb_padding);
    high += Vec3d(m_aabb_padding);

    m_broad_phase->get_potential_triangle_collisions( low, high, true, true, overlapping_triangles );

    for ( size_t i = 0; i < overlapping_triangles.size(); ++i )
    {

        const Vec3st& curr_tri = m_mesh.get_triangle( overlapping_triangles[i] );
        bool result = check_edge_triangle_intersection_by_index( tri[0], tri[1],
                                                                curr_tri[0], curr_tri[1], curr_tri[2],
                                                                get_positions(),
                                                                false );

        if ( result )
        {
            check_edge_triangle_intersection_by_index( tri[0], tri[1],
                                                      curr_tri[0], curr_tri[1], curr_tri[2],
                                                      get_positions(),
                                                      true );

            any_intersection = true;
        }
    }

    minmax( get_position(tri[1]), get_position(tri[2]), low, high );
    low -= Vec3d(m_aabb_padding);
    high += Vec3d(m_aabb_padding);

    overlapping_triangles.clear();
    m_broad_phase->get_potential_triangle_collisions( low, high, true, true,  overlapping_triangles );

    for ( size_t i = 0; i < overlapping_triangles.size(); ++i )
    {
        const Vec3st& curr_tri = m_mesh.get_triangle( overlapping_triangles[i] );

        bool result = check_edge_triangle_intersection_by_index( tri[1], tri[2],
                                                                curr_tri[0], curr_tri[1], curr_tri[2],
                                                                get_positions(),
                                                                false );

        if ( result )
        {
            check_edge_triangle_intersection_by_index( tri[1], tri[2],
                                                      curr_tri[0], curr_tri[1], curr_tri[2],
                                                      get_positions(),
                                                      true );

            any_intersection = true;
        }
    }

    minmax( get_position(tri[2]), get_position(tri[0]), low, high );
    low -= Vec3d(m_aabb_padding);
    high += Vec3d(m_aabb_padding);

    overlapping_triangles.clear();
    m_broad_phase->get_potential_triangle_collisions( low, high, true, true, overlapping_triangles );

    for ( size_t i = 0; i < overlapping_triangles.size(); ++i )
    {
        const Vec3st& curr_tri = m_mesh.get_triangle( overlapping_triangles[i] );

        bool result = check_edge_triangle_intersection_by_index( tri[2], tri[0],
                                                                curr_tri[0], curr_tri[1], curr_tri[2],
                                                                get_positions(),
                                                                false );

        if ( result )
        {
            check_edge_triangle_intersection_by_index( tri[2], tri[0],
                                                      curr_tri[0], curr_tri[1], curr_tri[2],
                                                      get_positions(),
                                                      true );

            any_intersection = true;
        }
    }

    //
    // edges
    //

    minmax( get_position(tri[0]), get_position(tri[1]), get_position(tri[2]), low, high );
    low -= Vec3d(m_aabb_padding);
    high += Vec3d(m_aabb_padding);

    static std::vector<size_t> overlapping_edges(10);
    overlapping_edges.clear();
    m_broad_phase->get_potential_edge_collisions( low, high, true, true, overlapping_edges );

    for ( size_t i = 0; i < overlapping_edges.size(); ++i )
    {

        bool result = check_edge_triangle_intersection_by_index(m_mesh.m_edges[overlapping_edges[i]][0],
                                                                m_mesh.m_edges[overlapping_edges[i]][1],
                                                                tri[0], tri[1], tri[2],
                                                                get_positions(),
                                                                false );

        if ( result )
        {
            check_edge_triangle_intersection_by_index(m_mesh.m_edges[overlapping_edges[i]][0],
                                                      m_mesh.m_edges[overlapping_edges[i]][1],
                                                      tri[0], tri[1], tri[2],
                                                      get_positions(),
                                                      true );

            any_intersection = true;
        }
    }

    return any_intersection;
}

// ---------------------------------------------------------
///
/// Detect all edge-triangle intersections.
///
// ---------------------------------------------------------

void DynamicSurface::get_intersections( bool degeneracy_counts_as_intersection,
                                       bool use_new_positions,
                                       std::vector<Intersection>& intersections )
{

    //#pragma omp parallel for schedule(guided)
    for ( int i = 0; i < (int)m_mesh.num_triangles(); ++i )
    {

        static std::vector<size_t> edge_candidates(20);

        bool get_solid_edges = !triangle_is_all_solid(i);
        edge_candidates.clear();
        Vec3d low, high;
        triangle_static_bounds( i, low, high );
        m_broad_phase->get_potential_edge_collisions( low, high, get_solid_edges, true, edge_candidates );

        const Vec3st& triangle = m_mesh.get_triangle(i);

        //skip deleted triangles
        if(m_mesh.triangle_is_deleted(i)) continue;

        for ( size_t j = 0; j < edge_candidates.size(); ++j )
        {

            const Vec2st& edge = m_mesh.m_edges[ edge_candidates[j] ];

            if(m_mesh.edge_is_deleted( edge_candidates[j] ) ) continue;

            if (    edge[0] == triangle[0] || edge[0] == triangle[1] || edge[0] == triangle[2]
                || edge[1] == triangle[0] || edge[1] == triangle[1] || edge[1] == triangle[2] )
            {
                continue;
            }

            assert( !triangle_is_all_solid( i ) || !edge_is_all_solid( edge_candidates[j] ) );

            const Vec3d& e0 = use_new_positions ? get_newposition(edge[0]) : get_position(edge[0]);
            const Vec3d& e1 = use_new_positions ? get_newposition(edge[1]) : get_position(edge[1]);
            const Vec3d& t0 = use_new_positions ? get_newposition(triangle[0]) : get_position(triangle[0]);
            const Vec3d& t1 = use_new_positions ? get_newposition(triangle[1]) : get_position(triangle[1]);
            const Vec3d& t2 = use_new_positions ? get_newposition(triangle[2]) : get_position(triangle[2]);

            if ( segment_triangle_intersection( e0, edge[0],
                                               e1, edge[1],
                                               t0, triangle[0],
                                               t1, triangle[1],
                                               t2, triangle[2],
                                               degeneracy_counts_as_intersection, m_verbose ) )
            {
                std::cout << "intersection: " << edge << " vs " << triangle << std::endl;
                std::cout << "e0: " << e0 << std::endl;
                std::cout << "e1: " << e1 << std::endl;
                std::cout << "t0: " << t0 << std::endl;
                std::cout << "t1: " << t1 << std::endl;
                std::cout << "t2: " << t2 << std::endl;
                //#pragma omp critical
                {
                  intersections.push_back( Intersection( edge_candidates[j], i ) );
                }
            }

        }

    }

}

// ---------------------------------------------------------
///
/// Fire an assert if any edge is intersecting any triangles
///
// ---------------------------------------------------------

void DynamicSurface::assert_mesh_is_intersection_free( bool degeneracy_counts_as_intersection )
{

    std::vector<Intersection> intersections;
    get_intersections( degeneracy_counts_as_intersection, false, intersections );

    for ( size_t i = 0; i < intersections.size(); ++i )
    {

        const Vec3st& triangle = m_mesh.get_triangle( intersections[i].m_triangle_index );
        const Vec2st& edge = m_mesh.m_edges[ intersections[i].m_edge_index ];

        std::cout << "Intersection!  Triangle " << triangle << " vs edge " << edge << std::endl;

        segment_triangle_intersection( get_position(edge[0]), edge[0],
                                      get_position(edge[1]), edge[1],
                                      get_position(triangle[0]), triangle[0],
                                      get_position(triangle[1]), triangle[1],
                                      get_position(triangle[2]), triangle[2],
                                      true, true );

        assert( false );

    }

}

// ---------------------------------------------------------
///
/// Using m_newpositions as the geometry, fire an assert if any edge is intersecting any triangles.
/// This is a useful debugging tool, as it will detect any missed collisions before the mesh is advected
/// into an intersecting state.
///
// ---------------------------------------------------------

void DynamicSurface::assert_predicted_mesh_is_intersection_free( bool degeneracy_counts_as_intersection )
{

    std::vector<Intersection> intersections;
    get_intersections( degeneracy_counts_as_intersection, true, intersections );

    for ( size_t i = 0; i < intersections.size(); ++i )
    {

        const Vec3st& triangle = m_mesh.get_triangle( intersections[i].m_triangle_index );
        const Vec2st& edge = m_mesh.m_edges[ intersections[i].m_edge_index ];

        std::cout << "Intersection!  Triangle " << triangle << " vs edge " << edge << std::endl;

        segment_triangle_intersection(get_position(edge[0]), edge[0],
                                      get_position(edge[1]), edge[1],
                                      get_position(triangle[0]), triangle[0],
                                      get_position(triangle[1]), triangle[1],
                                      get_position(triangle[2]), triangle[2],
                                      true, true );

        const Vec3d& ea = get_position(edge[0]);
        const Vec3d& eb = get_position(edge[1]);
        const Vec3d& ta = get_position(triangle[0]);
        const Vec3d& tb = get_position(triangle[1]);
        const Vec3d& tc = get_position(triangle[2]);

        std::vector<Collision> check_collisions;
        m_collision_pipeline->detect_collisions( check_collisions );
        std::cout << "number of collisions detected: " << check_collisions.size() << std::endl;

        for ( size_t c = 0; c < check_collisions.size(); ++c )
        {
            const Collision& collision = check_collisions[c];
            std::cout << "Collision " << c << ": " << std::endl;
            if ( collision.m_is_edge_edge )
            {
                std::cout << "edge-edge: ";
            }
            else
            {
                std::cout << "point-triangle: ";
            }
            std::cout << collision.m_vertex_indices << std::endl;

        }

        std::cout << "-----\n edge-triangle check using m_positions:" << std::endl;

        bool result = segment_triangle_intersection( get_position(edge[0]), edge[0],
                                                    get_position(edge[1]), edge[1],
                                                    get_position(triangle[0]), triangle[0],
                                                    get_position(triangle[1]), triangle[1],
                                                    get_position(triangle[2]), triangle[2],
                                                    degeneracy_counts_as_intersection,
                                                    m_verbose );

        std::cout << "result: " << result << std::endl;

        std::cout << "-----\n edge-triangle check using new m_positions" << std::endl;

        result = segment_triangle_intersection( get_newposition(edge[0]), edge[0],
                                               get_newposition(edge[1]), edge[1],
                                               get_newposition(triangle[0]), triangle[0],
                                               get_newposition(triangle[1]), triangle[1],
                                               get_newposition(triangle[2]), triangle[2],
                                               degeneracy_counts_as_intersection,
                                               m_verbose );

        std::cout << "result: " << result << std::endl;

        const Vec3d& ea_new = get_newposition(edge[0]);
        const Vec3d& eb_new = get_newposition(edge[1]);
        const Vec3d& ta_new = get_newposition(triangle[0]);
        const Vec3d& tb_new = get_newposition(triangle[1]);
        const Vec3d& tc_new = get_newposition(triangle[2]);

        std::cout.precision(20);

        std::cout << "old: (edge0 edge1 tri0 tri1 tri2 )" << std::endl;

        std::cout << "Vec3d ea( " << ea[0] << ", " << ea[1] << ", " << ea[2] << ");" << std::endl;
        std::cout << "Vec3d eb( " << eb[0] << ", " << eb[1] << ", " << eb[2] << ");" << std::endl;
        std::cout << "Vec3d ta( " << ta[0] << ", " << ta[1] << ", " << ta[2] << ");" << std::endl;
        std::cout << "Vec3d tb( " << tb[0] << ", " << tb[1] << ", " << tb[2] << ");" << std::endl;
        std::cout << "Vec3d tc( " << tc[0] << ", " << tc[1] << ", " << tc[2] << ");" << std::endl;

        std::cout << "Vec3d ea_new( " << ea_new[0] << ", " << ea_new[1] << ", " << ea_new[2] << ");" << std::endl;
        std::cout << "Vec3d eb_new( " << eb_new[0] << ", " << eb_new[1] << ", " << eb_new[2] << ");" << std::endl;
        std::cout << "Vec3d ta_new( " << ta_new[0] << ", " << ta_new[1] << ", " << ta_new[2] << ");" << std::endl;
        std::cout << "Vec3d tb_new( " << tb_new[0] << ", " << tb_new[1] << ", " << tb_new[2] << ");" << std::endl;
        std::cout << "Vec3d tc_new( " << tc_new[0] << ", " << tc_new[1] << ", " << tc_new[2] << ");" << std::endl;

        std::vector<double> possible_times;

        Vec3d normal;

        std::cout << "-----" << std::endl;

        assert( !segment_segment_collision(ea, ea_new, edge[0], eb, eb_new, edge[1],
                                           ta, ta_new, triangle[0], tb, tb_new, triangle[1] ) );

        std::cout << "-----" << std::endl;

        assert( !segment_segment_collision(ea, ea_new, edge[0], eb, eb_new, edge[1],
                                           tb, tb_new, triangle[1], tc, tc_new, triangle[2] ) );

        std::cout << "-----" << std::endl;

        assert( !segment_segment_collision(ea, ea_new, edge[0], eb, eb_new, edge[1],
                                           ta, ta_new, triangle[0], tc, tc_new, triangle[2] ) );

        std::cout << "-----" << std::endl;

        assert( !point_triangle_collision(ea, ea_new, edge[0], ta, ta_new, triangle[0],
                                          tb, tb_new, triangle[1], tc, tc_new, triangle[2] ) );

        std::cout << "-----" << std::endl;

        assert( !point_triangle_collision(eb, eb_new, edge[1], ta, ta_new, triangle[0],
                                          tb, tb_new, triangle[1], tc, tc_new, triangle[2] ) );

        m_verbose = false;

        std::cout << "no collisions detected" << std::endl;

        assert( false );

    }

}

}

max_stars_repo_name: xchern/MultiTracker

max_issues_repo_name: xchern/MultiTracker

max_forks_repo_name: xchern/MultiTracker


text:
#include <Eigen/Core>

#include <numpy_eigen/boost_python_headers.hpp>
Eigen::Matrix<float, 2, Eigen::Dynamic> test_float_2_D(const Eigen::Matrix<float, 2, Eigen::Dynamic>& M) { return M; }
void export_float_2_D() { boost::python::def("test_float_2_D", test_float_2_D); }

ext: cpp

max_stars_repo_name: chengfzy/kalibr

max_issues_repo_name: chengfzy/kalibr

max_forks_repo_name: chengfzy/kalibr


text:
///////////////////////////////////////////////////////////////////////////////
// vector_space::functional::l1_distance.hpp                         //
//                                                                           //
//  Copyright 2009 Erwann Rogard. Distributed under the Boost                //
//  Software License, Version 1.0. (See accompanying file                    //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_VECTOR_SPACE_FUNCTIONAL_l1_DISTANCE_SQUARED_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_VECTOR_SPACE_FUNCTIONAL_l1_DISTANCE_SQUARED_HPP_ER_2009
#include <cmath>
#include <numeric>
#include <functional>
#include <boost/mpl/assert.hpp>
#include <boost/type_traits.hpp>
#include <boost/function.hpp>
#include <boost/call_traits.hpp>
#include <boost/range.hpp>
#include <boost/lambda/lambda.hpp>
#include <boost/lambda/bind.hpp>
#include <boost/math/tools/precision.hpp>

namespace boost{
namespace statistics{
namespace detail{
namespace vector_space{

    // See l2_distance_squared
    template<typename R>
    class l1_distance{
        typedef typename is_reference<R>::type is_ref_;
    public:
        typedef typename remove_reference<R>::type const_range_type;
        typedef typename remove_const<const_range_type>::type range_type;
        typedef typename range_value<range_type>::type result_type;

        // Constructor
        l1_distance(typename call_traits<R>::param_type x);
        l1_distance(const l1_distance& that);
        l1_distance& operator=(const l1_distance& that);

        // Call
        template<typename R1>
        result_type operator()(const R1& y)const;

        private:
        l1_distance();
        typename call_traits<R>::value_type x_;
    };

    template<typename R>
    l1_distance<R>
    make_l1_distance(const R& x){
        return l1_distance<R>(x);
    }

    // Definitions
    template<typename R>
    l1_distance<R>::l1_distance(
        typename call_traits<R>::param_type x
    ):x_(x){
        BOOST_MPL_ASSERT((mpl::not_<is_ref_>));
    }

    template<typename R>
    l1_distance<R>::l1_distance(
        const l1_distance& that
    ):x_(that.x_){}

    template<typename R>
    l1_distance<R>&
    l1_distance<R>::operator=(const l1_distance& that){
        if(&that != this){
            BOOST_MPL_ASSERT((mpl::not_<is_ref_>));
            x_ = that.x_;
        }
        return *this;
    }

    template<typename R>
    template<typename R1>
    typename l1_distance<R>::result_type
    l1_distance<R>::operator()(const R1& y)const{
        BOOST_ASSERT(
            size(x_) == size(y)
        );

        typedef typename range_value<R1>::type y_;

        function<result_type(const result_type&)> fun = fabs;

        return std::inner_product(
            begin(y),
            end(y),
            begin(x_),
            static_cast<result_type>(0),
            std::plus<result_type>(),
            boost::lambda::bind(
                fun,
                (lambda::_1 - lambda::_2)
            )
        );
    };

}// vector_space
}// detail
}// statistics
}// boost

#endif

ext: hpp


text:
//
// Created by byrdofafeather on 2/7/19.
//

#include <utility>
#include <map>
#include <cassert>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <iostream>
#include <opencv2/imgproc/imgproc.hpp>
#include <boost/python.hpp>
#include <Python.h>
#include <string>
#include <deque>
#include <future>

using namespace std;
using namespace cv;
using namespace boost::python;

typedef std::map<std::string, std::deque<std::string>> DescriptorToTemplatesMap;
typedef std::map<std::string, double> TemplateToThresholdMap;

class Timestamp {
public:
    double time;
    std::string descriptor;
    Timestamp() = default;

    Timestamp (std::string initDescriptor, double initTime) {
        time = initTime;
        descriptor = std::move(initDescriptor);
    }
};

class TemplateScanner {
public:
    std::map<std::string, std::deque<cv::Mat>> templateMats;
    TemplateToThresholdMap thresholdMap;
    TemplateToThresholdMap orgThresholdMap;
    double templateThreshold;

    TemplateScanner(const DescriptorToTemplatesMap &templatePaths) {
        templateMats = build_image_map(templatePaths, true);
    }

    TemplateScanner(const DescriptorToTemplatesMap &templatePaths, TemplateToThresholdMap initThresholdMap) {
        templateMats = build_image_map(templatePaths, true);
        thresholdMap = initThresholdMap;
        orgThresholdMap = initThresholdMap;
    }

    TemplateScanner(const boost::python::dict &templatePaths, double threshold) {
        DescriptorToTemplatesMap final;

        // Convert passed Python types to C++ map
        boost::python::list keys = templatePaths.keys();
        for (int i = 0; i < boost::python::len(keys); i++) {
            std::string currentKey = boost::python::extract<std::string>(keys[i]);
            std::deque<std::string> currentPaths;
            boost::python::list paths = boost::python::extract<boost::python::list>(templatePaths[currentKey]);
            for (int j = 0; j < boost::python::len(paths); j++) {
                currentPaths.push_front(boost::python::extract<std::string>(paths[j]));
            }
            final[currentKey] = currentPaths;
        }

        templateMats = build_image_map(final);
        templateThreshold = threshold;
    }

    std::map<std::string, std::deque<cv::Mat>> build_image_map(const DescriptorToTemplatesMap &templatePaths, bool readGrey = false) {
        std::map<std::string, std::deque<cv::Mat>> returnMap;
        for (auto const& currentItems: templatePaths) {
            std::string currentDescriptor = currentItems.first;
            std::deque<std::string> paths = currentItems.second;
            for (const std::string &currentPath: paths) {
                // Reads the image and make sure it exists
                cv::Mat loadedImage;
                if (readGrey) {
                    loadedImage = imread(currentPath, cv::IMREAD_GRAYSCALE);
                }

                else {
                    loadedImage = imread(currentPath, cv::IMREAD_COLOR);
                }

                if (loadedImage.empty()) { cout << "FAILED TO LOAD IMAGE "; cout << currentPath << endl;}

                // Gets the reversed image
                cv::Mat reversedImage;
                cv::flip(loadedImage, reversedImage, 1);

                std::deque<cv::Mat> images = returnMap[currentDescriptor];
                images.push_front(loadedImage);
                images.push_front(reversedImage);
                returnMap[currentDescriptor] = images;
            }
        }
        return returnMap;
    }

    void releaseTemplates() {
        for (auto const& currentItems: templateMats) {
            std::string currentDescriptor = currentItems.first;
            std::deque<cv::Mat> images = currentItems.second;
            for (cv::Mat &currentImage: images) {
                currentImage.release();
            }
        }
    }

    double match_template(cv::Mat image, cv::Mat templateToMatch, std::string descriptor,
            int templateIndex, int filterProcess) {
        cv::Mat result;
        int resultCols = image.cols - templateToMatch.cols + 1;
        int resultRows = image.rows - templateToMatch.rows + 1;
        result.create(resultRows, resultCols, CV_32FC1);

        cv::matchTemplate(image, templateToMatch, result, filterProcess);

        double minVal; double maxVal; cv::Point minLoc; cv::Point maxLoc; cv::Point matchLoc;

        cv::minMaxLoc(result, &minVal, &maxVal, &minLoc, &maxLoc, cv::Mat());
        cv::Mat outputImage = image.clone();
        if (maxVal >= thresholdMap[descriptor + std::to_string(templateIndex)]) {
            rectangle(outputImage, maxLoc, Point( maxLoc.x + templateToMatch.cols , maxLoc.y + templateToMatch.rows ), Scalar::all(0), 2, 8, 0);
            rectangle(result, maxLoc, Point( maxLoc.x + templateToMatch.cols , maxLoc.y + templateToMatch.rows ), Scalar::all(0), 2, 8, 0);
            cv::imwrite("results/" + descriptor + std::to_string(maxVal) + ".png", outputImage);
        }

        return maxVal;
    }

    std::string get_best_match(const cv::Mat &image) {
        std::map<std::string, double> positive;
        for (auto const& currentValues : templateMats) {
            std::string currentDescriptor = currentValues.first;
            std::deque<cv::Mat> currentImages = currentValues.second;

            // Gets all matches above the threshold
            double currentDescriptorMaxProbability = 0;
            int TemplateIndex = 0;
            std::string index = currentDescriptor + std::to_string(TemplateIndex);
            for (cv::Mat &currentImage: currentImages) {
                double currentMaxProbability = match_template(image, currentImage, currentDescriptor, TemplateIndex, cv::TM_CCOEFF_NORMED);
                if (currentMaxProbability >= thresholdMap[index]) {
                    if (currentDescriptorMaxProbability < currentMaxProbability) {
                        currentDescriptorMaxProbability = currentMaxProbability;
                        positive[currentDescriptor] = currentDescriptorMaxProbability;
                    }
                }
                else {
                    double currentDifference = thresholdMap[index] - currentMaxProbability;
                    if (currentDifference <= .03) {
                        if (thresholdMap[index] - currentDifference >= orgThresholdMap[index]/1.3 && thresholdMap[index] - currentDifference >= .6) {
                            thresholdMap[index] -= (currentDifference);
                            cout << "This is the new probablity for " + currentDescriptor + std::to_string(TemplateIndex) << endl;
                            cout << thresholdMap[index] << endl;
                        }
                    }
                }

                TemplateIndex += 1;
            }
        }

        // Finds best match above the threshold
        if (positive.size() > 1) {
            double mostProbableDescriptorValue = 0;
            std::string mostProbableDescriptor;
            for (auto const &currentMaxes : positive) {
                std::string currentDescriptor = currentMaxes.first;
                double currentDescriptorMaxValue = currentMaxes.second;
                if (mostProbableDescriptorValue < currentDescriptorMaxValue) {
                    mostProbableDescriptorValue = currentDescriptorMaxValue;
                    mostProbableDescriptor = currentDescriptor;
                }
            }
            return mostProbableDescriptor;
        }

        else if (positive.size() == 1) {
            for (auto const &finalValue : positive) { return finalValue.first; }
        }

        else { return ""; }
    }

    std::map<std::string, double> get_best_prob_for_templates(cv::Mat image, std::deque<cv::Mat> templatesToMatch,
            std::string forDescriptor) {
        std::deque<std::future<double>> scans;
        for (cv::Mat currentImage : templatesToMatch) {
            std::future<double> scan = std::async(&TemplateScanner::match_template, *this, image, currentImage,
                    cv::TM_CCOEFF_NORMED);
            scans.push_front(std::move(scan));
        }

        double maxScan = 0;
        for (std::future<double> &finishedScans : scans) {
            double currentScan = finishedScans.get();
            if (currentScan > templateThreshold) {
                if (currentScan > maxScan) {
                    maxScan = currentScan;
                }
            }
        }

        std::map<std::string, double> returnMap;
        returnMap[forDescriptor] = maxScan;
        return returnMap;
    }

    std::string get_best_match_multithread(cv::Mat image) {
        std::deque<std::future<std::map<std::string, double>>> matches;
        for (auto const& currentValues : templateMats) {
            std::string currentDescriptor = currentValues.first;
            std::deque<cv::Mat> currentImages = currentValues.second;

            std::future<std::map<std::string, double>> currentPromise = std::async(&TemplateScanner::get_best_prob_for_templates, *this,
                    image, currentImages, currentDescriptor);

            matches.push_front(std::move(currentPromise));
        }

        std::string bestMatch = "";
        double bestMatchValue = 0;
        for (std::future<std::map<std::string, double>> &match : matches) {
            std::map<std::string, double> currentMatch = match.get();
            for (auto const &currentVal : currentMatch) {
                if (currentVal.second > bestMatchValue) {
					bestMatchValue = currentVal.second;
                    bestMatch = currentVal.first;
                }
            }
        }
        return bestMatch;
    }

    std::string scan(const cv::Mat &image) {
        std::string currentMaxDescriptor = get_best_match(image);
        return currentMaxDescriptor;
    }

    std::string multithreaded_scan(const cv::Mat &image) {
        std::string currentMaxDescriptor = get_best_match_multithread(image);
        return currentMaxDescriptor;
    }

    boost::python::str multithreadedScanPython(const boost::python::str imagePath) {
        cv::Mat image;
        std::string cPath = boost::python::extract<std::string>(imagePath);
        image = cv::imread(cPath);
        assert(!image.empty());
        std::string currentMaxDescriptor = get_best_match_multithread(image);
        boost::python::str returnValue(currentMaxDescriptor);
        return returnValue;
    }

    boost::python::str scanPython(const boost::python::str imagePath) {
        cv::Mat image;
        std::string cPath = boost::python::extract<std::string>(imagePath);
        image = cv::imread(cPath);
        assert(!image.empty());
        std::string currentMaxDescriptor = get_best_match(image);
        boost::python::str returnValue(currentMaxDescriptor);
        return returnValue;
    }
};

class VideoScannerThreaded: public TemplateScanner {
public:
    int frameIndexes[2]{};
    std::string videoPath;

    VideoScannerThreaded(const DescriptorToTemplatesMap &templatePaths, std::string initVideoPath,
            int frameIndexStart, int frameIndexEnd, TemplateToThresholdMap thresholdMap);

    std::deque<Timestamp> get_timestamps() {
        // Load video and make sure it isn't empty
        cv::VideoCapture video(videoPath);
        assert(video.get(cv::CAP_PROP_FRAME_COUNT) != 0);

        std::deque<Timestamp> timestamps;

        // Set the video to the start of the frame index and get the FPS
        video.set(cv::CAP_PROP_POS_FRAMES, frameIndexes[0]);
        double fps = video.get(cv::CAP_PROP_FPS);

//        if (false) {
//            double windowNumber = video.get(cv::CAP_PROP_POS_FRAMES);
//            cv::namedWindow("Display window " + std::to_string(windowNumber),
//                            WINDOW_AUTOSIZE);// Create a window for display.
//        }

        while(true) {
            // Read the next frame
            cv::Mat currentFrame;
            video.read(currentFrame);

            if (video.get(cv::CAP_PROP_POS_FRAMES) < frameIndexes[1] && !currentFrame.empty()) {
                // Scan the frame with the current templates
                cv::Mat grayFrame;
                cv::cvtColor(currentFrame, grayFrame, cv::COLOR_BGR2GRAY);
                std::string exportDescriptor = scan(grayFrame);

                if (!exportDescriptor.empty()) {
                    // Testing code for running from C++
//                    if (false) {
//                        cv::imshow("Display window " + std::to_string(windowNumber), currentFrame);
//                        cv::waitKey(0);
//                    }
                    cout << "EXPORTING : ";
                    cout << video.get(cv::CAP_PROP_POS_MSEC) / 1000 << endl;
                    cout << "EXPORTING : ";
                    cout << exportDescriptor << endl;
                    Timestamp currentTime(exportDescriptor, video.get(cv::CAP_PROP_POS_MSEC) / 1000);
                    timestamps.push_front(currentTime);
                }

               //  // Move a second in the video
               // double currentFrameNumber = video.get(cv::CAP_PROP_POS_FRAMES);
               // video.set(cv::CAP_PROP_POS_FRAMES, currentFrameNumber + (fps / 10));
            }

            else {
                break;
            }
        }

        video.release();
        releaseTemplates();
        return timestamps;
    }
};

VideoScannerThreaded::VideoScannerThreaded(const DescriptorToTemplatesMap &templatePaths,
                                           std::string initVideoPath, int frameIndexStart, int frameIndexEnd,
                                           TemplateToThresholdMap thresholdMap)
                                           : TemplateScanner(templatePaths, thresholdMap) {
    frameIndexes[0] = frameIndexStart;
    frameIndexes[1] = frameIndexEnd;
    videoPath = std::move(initVideoPath);
}

class ThresholdFinder: public TemplateScanner {
public:
    ThresholdFinder(DescriptorToTemplatesMap templatePaths);

    using TemplateScanner::match_template;

    double static match_template(cv::Mat image, cv::Mat templateToMatch, int filterProcess) {
        cv::Mat result;
        int resultCols = image.cols - templateToMatch.cols + 1;
        int resultRows = image.rows - templateToMatch.rows + 1;
        result.create(resultRows, resultCols, CV_32FC1);

        cv::matchTemplate(image, templateToMatch, result, filterProcess);

        double minVal; double maxVal; cv::Point minLoc; cv::Point maxLoc; cv::Point matchLoc;

        cv::minMaxLoc(result, &minVal, &maxVal, &minLoc, &maxLoc, cv::Mat());

        return maxVal;
    }

    double static getThreshold(std::string descriptor, cv::Mat templateToMatch, std::string videoPath, long frameGuess, std::string specificDescriptor="Generic") {
        cv::VideoCapture video(videoPath);

        double bestMatch = 0;
        double frameCount = video.get(cv::CAP_PROP_FRAME_COUNT);

        std::random_device rd;
        std::default_random_engine generator(rd());
        std::uniform_int_distribution<long> distribution(0, (long)frameCount);

        std::deque<double> matches;
        for (int i = 0; i <= frameGuess; i++) {

            long frame = distribution(generator);

            video.set(cv::CAP_PROP_POS_FRAMES, frame);
            cv::Mat currentFrame;
            video.read(currentFrame);

            if (!currentFrame.empty()) {
                cv::Mat grayFrame;
                cv::cvtColor(currentFrame, grayFrame, cv::COLOR_BGR2GRAY);
                double currentMatch = match_template(grayFrame, templateToMatch, cv::TM_CCOEFF_NORMED);
                matches.push_front(currentMatch);
                if (bestMatch <= currentMatch) {
                    bestMatch = currentMatch;
                }
            }

            else {
                continue;
            }
        }
        cout << descriptor + specificDescriptor << endl;
        cout << std::to_string(bestMatch) << endl;
        for (double &match : matches) {
            cout << match << endl;
        }
        video.release();
        double thresh = bestMatch;
        return thresh;
    }

    TemplateToThresholdMap getThresholds(std::string videoPath) {
        cv::VideoCapture video(videoPath);

        double frameRate = video.get(cv::CAP_PROP_FPS);
        double frameCount = video.get(cv::CAP_PROP_FRAME_COUNT);
        video.release();

        double frameGuess = std::log(1 - .9) / std::log(1 - ((1 + frameRate) / frameCount));
        double frameGuessInt = std::floor(frameGuess);

        cout << std::to_string(frameGuessInt) << endl;

        if (frameGuessInt > frameCount) { frameGuessInt = frameCount; }

        std::map<std::string, std::future<double>> tempTemplateToThresholdMap;

        for (auto const &currentItems: templateMats) {
            std::string currentDescriptor = currentItems.first;
            std::deque<cv::Mat> currentImages = currentItems.second;
            int imageIndex = 0;
            for (cv::Mat &currentImage: currentImages) {
                std::promise<double> currentPromise;
                std::future<double> currentPromisedFuture = std::async(getThreshold, currentDescriptor, currentImage,
                                                                       videoPath, frameGuess,
                                                                       std::to_string(imageIndex));
                tempTemplateToThresholdMap[currentDescriptor + std::to_string(imageIndex)] = std::move(
                        currentPromisedFuture);
                imageIndex += 1;
                cout << "=== JUST STARTED " + currentDescriptor + " THREAD " + std::to_string(imageIndex) + " ===" << endl;
            }
            cout << "=== STARTED " + std::to_string(imageIndex) + " THREADS ===" << endl;
        }

        TemplateToThresholdMap thresholdMap;

        for (auto  &currentItems: tempTemplateToThresholdMap) {
            std::string currentDescriptorIndex = currentItems.first;
            double currentMax = currentItems.second.get();
            cout << "Adding " + currentDescriptorIndex + " with " + std::to_string(currentMax) << endl;
            thresholdMap[currentDescriptorIndex] = currentMax;
        }
        return thresholdMap;
    }
};

ThresholdFinder::ThresholdFinder(DescriptorToTemplatesMap templatePaths): TemplateScanner(templatePaths) { }

class VideoThreader {
public:
    DescriptorToTemplatesMap templates;

    VideoThreader(DescriptorToTemplatesMap initTemplatePaths) {
        templates = initTemplatePaths;
    }

    std::deque<Timestamp> thread_scanners(const std::string &videoPath, int divisor = 1800) {
        // Get the current video to get the frame count and then release
        cv::VideoCapture video(videoPath);
        double frame_count = video.get(cv::CAP_PROP_FRAME_COUNT);
        assert(frame_count != 0);
        video.release();

        ThresholdFinder finder(templates);
        TemplateToThresholdMap thresholdMap = finder.getThresholds(videoPath);

        // Build the list of threads
        std::deque<std::future<std::deque<Timestamp>>> futures;
        int frameSections = 0;
        int currentFrameStart = 0;
        int currentFrameEnd = divisor;
        while (frame_count > divisor * frameSections) {
            VideoScannerThreaded currentVideoScanner(templates, videoPath,
                currentFrameStart, currentFrameEnd, thresholdMap);
            std::future<std::deque<Timestamp>> currentPromisedFuture = std::async(&VideoScannerThreaded::get_timestamps,
                    currentVideoScanner);
            futures.push_front(std::move(currentPromisedFuture));
            currentFrameStart += divisor;
            currentFrameEnd += divisor;
            frameSections += 1;
        }

        cout << "===== STARTING " + std::to_string(frameSections) + " THREADS =====" << endl;

        // Start the threads
        std::deque<Timestamp> allTimestamps;
        for (std::future<std::deque<Timestamp>> &future : futures) {
            std::deque<Timestamp> currentTimestamps = future.get();
            for (Timestamp &timestamp : currentTimestamps) {
                allTimestamps.push_front(timestamp);
            }
        }
        return allTimestamps;
    }
};

struct ThreadedVideoScan {
public:
    ThreadedVideoScan() = default;

    boost::python::list run(boost::python::dict templates, boost::python::str pythonVideoPath) {
        DescriptorToTemplatesMap final;

        // Convert passed Python types to C++ map
        std::string videoPath = boost::python::extract<std::string>(pythonVideoPath);
        boost::python::list keys = templates.keys();
        for (int i = 0; i < boost::python::len(keys); i++) {
            std::string currentKey = boost::python::extract<std::string>(keys[i]);
            std::deque<std::string> currentPaths;
            boost::python::list paths = boost::python::extract<boost::python::list>(templates[currentKey]);
            for (int j = 0; j < boost::python::len(paths); j++) {
                currentPaths.push_front(boost::python::extract<std::string>(paths[j]));
            }
            final[currentKey] = currentPaths;
        }

        // Call the scanner to create the threads and run them
        VideoThreader scanner(final);

        // Convert scanner return values from C++ Types to Python types
        std::deque<Timestamp> cTimestamps = scanner.thread_scanners(videoPath);
        boost::python::list pythonTimestamps;
        for (Timestamp &time : cTimestamps) {
                pythonTimestamps.append(boost::python::object(Timestamp(time.descriptor, time.time)));
        }
        return pythonTimestamps;
    }

    void runTest(DescriptorToTemplatesMap templates, std::string videoPath) {
        VideoThreader scanner(templates);
        std::deque<Timestamp> times;
        times = scanner.thread_scanners(videoPath);
        for (Timestamp &time : times) {
            cout << time.time << endl;
            cout << time.descriptor << endl;
        }
    }
};

int main() {
    // Build Test Map
    DescriptorToTemplatesMap testMap;
    std::deque<std::string> testImagePaths;
    testImagePaths.push_front("0.png");
    testMap["Jump"] = testImagePaths;

    // Build Video Path
    std::string videoPath = "mm.mp4";

    // Start test
    ThreadedVideoScan testScanner;
    testScanner.runTest(testMap, videoPath);
}

BOOST_PYTHON_MODULE(templatescannersbeta) {
    class_<TemplateScanner>("TemplateScanner", init<boost::python::dict, double>())
            .def("scan", &TemplateScanner::scanPython)
            .def("multithreaded_scan", &TemplateScanner::multithreadedScanPython);

    class_<ThreadedVideoScan>("ThreadedVideoScan")
            .def("run", &ThreadedVideoScan::run);

    class_<Timestamp>("Timestamp")
            .def_readonly("time", &Timestamp::time)
            .def_readonly("marker", &Timestamp::descriptor);
}

max_stars_repo_name: ByrdOfAFeather/TemplateScanners

max_issues_repo_name: ByrdOfAFeather/TemplateScanners

max_forks_repo_name: ByrdOfAFeather/TemplateScanners


text:
//#include "threadpool.h"

#include "asio_tpool.hpp"

#include <iostream>
#include <functional>
#include <thread>
#include <chrono>

#include <boost/asio/system_timer.hpp>
#include <boost/exception/exception.hpp>

int some_long_op(int a, int b)
{
	std::cout << "Boom !" << std::endl;
	std::cout.flush();
	std::this_thread::sleep_for(std::chrono::seconds(1));
	BOOST_THROW_EXCEPTION(std::runtime_error("EXIT in run!"));
	return a + b;
}

void main_async(atpool::io& io, atpool::ctx ctx)
{
	atpool::thread_pool tpool(io);
	std::cout << "async start thread id = " << std::this_thread::get_id() << std::endl;
	boost::asio::spawn(io, [&io](atpool::ctx ctx)
	{
		int i = 0;

		for (;;)
		{
			boost::asio::system_timer timer(io);
			timer.expires_from_now(std::chrono::milliseconds(1000));
			timer.async_wait(ctx);
			std::cout << i++ << " secs. tick 1000 ms thread id = " << std::this_thread::get_id() << std::endl;
		}
	});
	auto t = tpool.run(&some_long_op, 11, 22);

	try
	{
		std::cout << t->result(ctx) << std::endl;
	}
	catch (const std::exception& e)
	{
		std::cout << e.what() << std::endl;
	}

	tpool.wait_all();
	std::cout << "main finished" << std::endl;
}

int main(int argc, char* argv[])
{
	try
	{
		atpool::io io;
		boost::asio::spawn(io, [&](atpool::ctx ctx)
		{
			main_async(io, ctx);
		});
		io.run();
	}
	catch (const std::exception& e)
	{
		std::cerr << boost::current_exception_diagnostic_information(true) << std::endl;
		exit(1);
	}
	catch (...)
	{
		std::cerr << "unknown exception, aborting" << std::endl;
		exit(1);
	}

	return 0;
}