1
// ---------------------------------------------------------
6
// A triangle mesh with associated vertex locations and
7
// velocities. Collision detection and solving.
9
// ---------------------------------------------------------
11
// ---------------------------------------------------------
13
// ---------------------------------------------------------
15
#include <dynamicsurface.h>
22
#include <OpenGL/gl.h>
33
#include <ccd_wrapper.h>
36
#include <nondestructivetrimesh.h>
37
#include <broadphasegrid.h>
38
#include <wallclocktime.h>
40
#include <sparse_matrix.h>
41
#include <krylov_solvers.h>
43
#include <lapack_wrapper.h>
46
#include <ccd_wrapper.h>
47
#include <collisionqueries.h>
49
//#define USE_EXACT_ARITHMETIC_RAY_CASTING
51
#ifdef USE_EXACT_ARITHMETIC_RAY_CASTING
55
// ---------------------------------------------------------
56
// Local constants, typedefs, macros
57
// ---------------------------------------------------------
59
// ---------------------------------------------------------
61
// ---------------------------------------------------------
63
const double G_EIGENVALUE_RANK_RATIO = 0.03;
65
// ---------------------------------------------------------
66
// Static function definitions
67
// ---------------------------------------------------------
69
// ---------------------------------------------------------
71
/// Add a collision to the list as long as it doesn't have the same vertices as any other collisions in the list.
73
// ---------------------------------------------------------
75
static void add_unique_collision( std::vector<Collision>& collisions, const Collision& new_collision )
77
for ( std::vector<Collision>::iterator iter = collisions.begin(); iter != collisions.end(); ++iter )
79
if ( iter->same_vertices( new_collision ) )
85
collisions.push_back( new_collision );
88
// ---------------------------------------------------------
90
/// Helper function: multiply transpose(A) * D * B
92
// ---------------------------------------------------------
94
static void AtDB(const SparseMatrixDynamicCSR &A, const double* diagD, const SparseMatrixDynamicCSR &B, SparseMatrixDynamicCSR &C)
99
for(int k=0; k<A.m; ++k)
101
const DynamicSparseVector& r = A.row[k];
103
for( DynamicSparseVector::const_iterator p=r.begin(); p != r.end(); ++p )
106
double multiplier = p->value * diagD[k];
107
C.add_sparse_row( i, B.row[k], multiplier );
112
// ---------------------------------------------------------
113
// Member function definitions
114
// ---------------------------------------------------------
116
// ---------------------------------------------------------
118
/// DynamicSurface constructor. Copy triangles and vertex locations.
120
// ---------------------------------------------------------
122
DynamicSurface::DynamicSurface( const std::vector<Vec3d>& vertex_positions,
123
const std::vector<Vec3ui>& triangles,
124
const std::vector<double>& masses,
125
double in_proximity_epsilon,
126
bool in_collision_safety,
128
m_proximity_epsilon( in_proximity_epsilon ),
129
m_verbose( in_verbose ),
130
m_collision_safety( in_collision_safety ),
131
m_positions(0), m_newpositions(0), m_velocities(0), m_masses(0),
133
m_num_collisions_this_step(0), m_total_num_collisions(0)
136
m_broad_phase = new BroadPhaseGrid();
138
std::cout << "constructing dynamic surface" << std::endl;
140
for(unsigned int i = 0; i < vertex_positions.size(); i++)
142
m_positions.push_back( vertex_positions[i] );
143
m_newpositions.push_back( vertex_positions[i] );
144
m_velocities.push_back( Vec3d(0,0,0) );
145
m_masses.push_back( masses[i] );
148
for(unsigned int i = 0; i < triangles.size(); i++)
150
m_mesh.m_tris.push_back( triangles[i] );
153
std::cout << "constructed dynamic surface" << std::endl;
157
// ---------------------------------------------------------
161
// ---------------------------------------------------------
163
DynamicSurface::~DynamicSurface()
165
delete m_broad_phase;
169
// ---------------------------------------------------------
171
/// Compute rank of the quadric metric tensor at a vertex
173
// ---------------------------------------------------------
175
unsigned int DynamicSurface::classify_vertex( unsigned int v )
177
if ( m_mesh.m_vtxtri[v].empty() ) { return 0; }
179
const std::vector<unsigned int>& incident_triangles = m_mesh.m_vtxtri[v];
181
std::vector< Vec3d > N;
182
std::vector< double > W;
184
for ( unsigned int i = 0; i < incident_triangles.size(); ++i )
186
unsigned int triangle_index = incident_triangles[i];
187
N.push_back( get_triangle_normal(triangle_index) );
188
W.push_back( get_triangle_area(triangle_index) );
191
Mat33d A(0,0,0,0,0,0,0,0,0);
193
for ( unsigned int i = 0; i < N.size(); ++i )
195
A(0,0) += N[i][0] * W[i] * N[i][0];
196
A(1,0) += N[i][1] * W[i] * N[i][0];
197
A(2,0) += N[i][2] * W[i] * N[i][0];
199
A(0,1) += N[i][0] * W[i] * N[i][1];
200
A(1,1) += N[i][1] * W[i] * N[i][1];
201
A(2,1) += N[i][2] * W[i] * N[i][1];
203
A(0,2) += N[i][0] * W[i] * N[i][2];
204
A(1,2) += N[i][1] * W[i] * N[i][2];
205
A(2,2) += N[i][2] * W[i] * N[i][2];
208
// get eigen decomposition
209
double eigenvalues[3];
211
int info = ~0, n = 3, lwork = 9;
212
LAPACK::get_eigen_decomposition( &n, A.a, &n, eigenvalues, work, &lwork, &info );
216
std::cout << "Eigen decomp failed. Incident triangles: " << std::endl;
217
for ( unsigned int i = 0; i < W.size(); ++i )
219
std::cout << "normal: ( " << N[i] << " ) ";
220
std::cout << "area: " << W[i] << std::endl;
225
// compute rank of primary space
226
unsigned int rank = 0;
227
for ( unsigned int i = 0; i < 3; ++i )
229
if ( eigenvalues[i] > G_EIGENVALUE_RANK_RATIO * eigenvalues[2] )
241
// ---------------------------------------------------------
243
/// Add a triangle to the surface. Update the underlying TriMesh and acceleration grid.
245
// ---------------------------------------------------------
247
unsigned int DynamicSurface::add_triangle( const Vec3ui& t )
249
unsigned int new_triangle_index = m_mesh.m_tris.size();
250
m_mesh.nondestructive_add_triangle( t );
252
if ( m_collision_safety )
254
// Add to the triangle grid
256
triangle_static_bounds( new_triangle_index, low, high );
257
m_broad_phase->add_triangle( new_triangle_index, low, high );
259
// Add edges to grid as well
260
unsigned int new_edge_index = m_mesh.get_edge( t[0], t[1] );
261
assert( new_edge_index != m_mesh.m_edges.size() );
262
edge_static_bounds( new_edge_index, low, high );
263
m_broad_phase->add_edge( new_edge_index, low, high );
265
new_edge_index = m_mesh.get_edge( t[1], t[2] );
266
assert( new_edge_index != m_mesh.m_edges.size() );
267
edge_static_bounds( new_edge_index, low, high );
268
m_broad_phase->add_edge( new_edge_index, low, high );
270
new_edge_index = m_mesh.get_edge( t[2], t[0] );
271
assert( new_edge_index != m_mesh.m_edges.size() );
272
edge_static_bounds( new_edge_index, low, high );
273
m_broad_phase->add_edge( new_edge_index, low, high );
276
return new_triangle_index;
280
// ---------------------------------------------------------
282
/// Remove a triangle from the surface. Update the underlying TriMesh and acceleration grid.
284
// ---------------------------------------------------------
286
void DynamicSurface::remove_triangle(unsigned int t)
288
m_mesh.nondestructive_remove_triangle( t );
289
if ( m_collision_safety )
291
m_broad_phase->remove_triangle( t );
296
// ---------------------------------------------------------
298
/// Add a vertex to the surface. Update the acceleration grid.
300
// ---------------------------------------------------------
302
unsigned int DynamicSurface::add_vertex( const Vec3d& new_vertex_position,
303
const Vec3d& new_vertex_velocity,
304
double new_vertex_mass )
306
m_positions.push_back( new_vertex_position );
307
m_newpositions.push_back( new_vertex_position );
308
m_velocities.push_back( new_vertex_velocity );
309
m_masses.push_back( new_vertex_mass );
311
unsigned int new_vertex_index = m_mesh.nondestructive_add_vertex( );
313
assert( new_vertex_index == m_positions.size() - 1 );
315
if ( m_collision_safety )
317
m_broad_phase->add_vertex( new_vertex_index, m_positions[new_vertex_index], m_positions[new_vertex_index] );
320
return new_vertex_index;
324
// ---------------------------------------------------------
326
/// Remove a vertex from the surface. Update the acceleration grid.
328
// ---------------------------------------------------------
330
void DynamicSurface::remove_vertex( unsigned int vertex_index )
332
m_mesh.nondestructive_remove_vertex( vertex_index );
334
if ( m_collision_safety )
336
m_broad_phase->remove_vertex( vertex_index );
339
m_positions[ vertex_index ] = Vec3d( 0.0, 0.0, 0.0 );
340
m_newpositions[ vertex_index ] = Vec3d( 0.0, 0.0, 0.0 );
344
// --------------------------------------------------------
346
/// Determine surface IDs for all vertices
348
// --------------------------------------------------------
350
void DynamicSurface::partition_surfaces( std::vector<unsigned int>& surface_ids, std::vector< std::vector< unsigned int> >& surfaces ) const
353
static const unsigned int UNASSIGNED = (unsigned int) ~0;
358
surface_ids.resize( m_positions.size(), UNASSIGNED );
360
unsigned int curr_surface = 0;
364
unsigned int next_unassigned_vertex;
365
for ( next_unassigned_vertex = 0; next_unassigned_vertex < surface_ids.size(); ++next_unassigned_vertex )
367
if ( m_mesh.m_vtxedge[next_unassigned_vertex].empty() ) { continue; }
369
if ( surface_ids[next_unassigned_vertex] == UNASSIGNED )
375
if ( next_unassigned_vertex == surface_ids.size() )
380
std::queue<unsigned int> open;
381
open.push( next_unassigned_vertex );
383
std::vector<unsigned int> surface_vertices;
385
while ( false == open.empty() )
387
unsigned int vertex_index = open.front();
390
if ( m_mesh.m_vtxedge[vertex_index].empty() ) { continue; }
392
if ( surface_ids[vertex_index] != UNASSIGNED )
394
assert( surface_ids[vertex_index] == curr_surface );
398
surface_ids[vertex_index] = curr_surface;
399
surface_vertices.push_back( vertex_index );
401
const std::vector<unsigned int>& incident_edges = m_mesh.m_vtxedge[vertex_index];
403
for( unsigned int i = 0; i < incident_edges.size(); ++i )
405
unsigned int adjacent_vertex = m_mesh.m_edges[ incident_edges[i] ][0];
406
if ( adjacent_vertex == vertex_index ) { adjacent_vertex = m_mesh.m_edges[ incident_edges[i] ][1]; }
408
if ( surface_ids[adjacent_vertex] == UNASSIGNED )
410
open.push( adjacent_vertex );
414
assert( surface_ids[adjacent_vertex] == curr_surface );
420
surfaces.push_back( surface_vertices );
426
std::cout << " %%%%%%%%%%%%%%%%%%%%%%% number of surfaces: " << surfaces.size() << std::endl;
429
// assert all vertices are assigned and share volume IDs with their neighbours
432
for ( unsigned int i = 0; i < surface_ids.size(); ++i )
434
if ( m_mesh.m_vtxedge[i].empty() ) { continue; }
436
assert( surface_ids[i] != UNASSIGNED );
438
const std::vector<unsigned int>& incident_edges = m_mesh.m_vtxedge[i];
439
for( unsigned int j = 0; j < incident_edges.size(); ++j )
441
unsigned int adjacent_vertex = m_mesh.m_edges[ incident_edges[j] ][0];
442
if ( adjacent_vertex == i ) { adjacent_vertex = m_mesh.m_edges[ incident_edges[j] ][1]; }
443
assert( surface_ids[adjacent_vertex] == surface_ids[i] );
451
// ---------------------------------------------------------
453
/// Compute the maximum timestep that will not invert any triangle normals, using a quadratic solve as in [Jiao 2007].
455
// ---------------------------------------------------------
457
double DynamicSurface::compute_max_timestep_quadratic_solve( const std::vector<Vec3ui>& tris,
458
const std::vector<Vec3d>& positions,
459
const std::vector<Vec3d>& displacements,
462
double max_beta = 1.0;
464
double min_area = 1e30;
466
for ( unsigned int i = 0; i < tris.size(); ++i )
468
if ( tris[i][0] == tris[i][1] ) { continue; }
470
const Vec3d& x1 = positions[tris[i][0]];
471
const Vec3d& x2 = positions[tris[i][1]];
472
const Vec3d& x3 = positions[tris[i][2]];
474
const Vec3d& u1 = displacements[tris[i][0]];
475
const Vec3d& u2 = displacements[tris[i][1]];
476
const Vec3d& u3 = displacements[tris[i][2]];
478
Vec3d new_x1 = x1 + u1;
479
Vec3d new_x2 = x2 + u2;
480
Vec3d new_x3 = x3 + u3;
482
const Vec3d c0 = cross( (x2-x1), (x3-x1) );
483
const Vec3d c1 = cross( (x2-x1), (u3-u1) ) - cross( (x3-x1), (u2-u1) );
484
const Vec3d c2 = cross( (u2-u1), (u3-u1) );
485
const double a = dot(c0, c2);
486
const double b = dot(c0, c1);
487
const double c = dot(c0, c0);
491
min_area = min( min_area, c );
497
printf( "super small triangle %d (%d %d %d)\n", i, tris[i][0], tris[i][1], tris[i][2] );
505
//printf( "triangle %d: ", i );
506
//printf( "a == 0 (%g)\n", a );
509
if ( ( fabs(b) > 1e-14 ) && ( -c / b >= 0.0 ) )
517
if ( fabs(b) < 1e-14 )
519
printf( "triangle %d: ", i );
520
printf( "b == 0 too (%g).\n", b );
527
double descriminant = b*b - 4.0*a*c;
529
if ( descriminant < 0.0 )
531
// Hmm, what does this mean?
534
printf( "triangle %d: descriminant == %g\n", i, descriminant );
544
q = -0.5 * ( b + sqrt( descriminant ) );
548
q = -0.5 * ( b - sqrt( descriminant ) );
551
double beta_1 = q / a;
552
double beta_2 = c / q;
558
assert( dot( triangle_normal(x1, x2, x3), triangle_normal(new_x1, new_x2, new_x3) ) > 0.0 );
571
else if ( beta_1 < beta_2 )
584
bool changed = false;
585
if ( beta < max_beta )
587
max_beta = 0.99 * beta;
592
printf( "changing beta --- triangle: %d\n", i );
593
printf( "new max beta: %g\n", max_beta );
594
printf( "a = %g, b = %g, c = %g\n", a, b, c );
597
if ( max_beta < 1e-4 )
604
new_x1 = x1 + max_beta * u1;
605
new_x2 = x2 + max_beta * u2;
606
new_x3 = x3 + max_beta * u3;
608
Vec3d old_normal = cross(x2-x1, x3-x1);
609
Vec3d new_normal = cross(new_x2-new_x1, new_x3-new_x1);
611
if ( dot( old_normal, new_normal ) < 0.0 )
613
printf( "triangle %d: (%d %d %d)\n", i, tris[i][0], tris[i][1], tris[i][2] );
614
printf( "old normal: %g %g %g\n", old_normal[0], old_normal[1], old_normal[2] );
615
printf( "new normal: %g %g %g\n", new_normal[0], new_normal[1], new_normal[2] );
616
printf( "dot product: %g\n", dot( triangle_normal(x1, x2, x3), triangle_normal(new_x1, new_x2, new_x3) ) );
617
printf( "%s\n", (changed ? "changed" : "not changed") );
618
printf( "beta: %g\n", beta );
619
printf( "max beta: %g\n", max_beta );
628
// ---------------------------------------------------------
630
/// Compute the unsigned distance to the surface.
632
// ---------------------------------------------------------
634
double DynamicSurface::distance_to_surface( const Vec3d& p, unsigned int& closest_triangle )
637
double padding = m_proximity_epsilon;
638
double min_distance = 1e30;
640
while ( min_distance == 1e30 )
643
Vec3d xmin( p - Vec3d( padding ) );
644
Vec3d xmax( p + Vec3d( padding ) );
646
std::vector<unsigned int> nearby_triangles;
648
m_broad_phase->get_potential_triangle_collisions( xmin, xmax, nearby_triangles );
650
for ( unsigned int j = 0; j < nearby_triangles.size(); ++j )
652
const Vec3ui& tri = m_mesh.m_tris[ nearby_triangles[j] ];
654
if ( tri[0] == tri[1] || tri[1] == tri[2] || tri[0] == tri[2] ) { continue; }
656
if ( m_masses[tri[0]] > 1.5 && m_masses[tri[1]] > 1.5 && m_masses[tri[2]] > 1.5 ) { continue; }
658
double curr_distance;
659
check_point_triangle_proximity( p, m_positions[tri[0]], m_positions[tri[1]], m_positions[tri[2]], curr_distance );
660
if ( curr_distance < padding )
662
min_distance = min( min_distance, curr_distance );
663
closest_triangle = nearby_triangles[j];
676
// ---------------------------------------------------------
678
/// Run intersection detection against all triangles
680
// ---------------------------------------------------------
682
void DynamicSurface::get_triangle_intersections( const Vec3d& segment_point_a,
683
const Vec3d& segment_point_b,
684
std::vector<double>& hit_ss,
685
std::vector<unsigned int>& hit_triangles ) const
687
Vec3d aabb_low, aabb_high;
688
minmax( segment_point_a, segment_point_b, aabb_low, aabb_high );
690
std::vector<unsigned int> overlapping_triangles;
691
m_broad_phase->get_potential_triangle_collisions( aabb_low, aabb_high, overlapping_triangles );
693
for ( unsigned int i = 0; i < overlapping_triangles.size(); ++i )
695
const Vec3ui& tri = m_mesh.m_tris[ overlapping_triangles[i] ];
697
Vec3ui t = sort_triangle( tri );
698
assert( t[0] < t[1] && t[0] < t[2] && t[1] < t[2] );
700
const Vec3d& v0 = m_positions[ t[0] ];
701
const Vec3d& v1 = m_positions[ t[1] ];
702
const Vec3d& v2 = m_positions[ t[2] ];
704
unsigned int dummy_index = m_positions.size();
706
double bary1, bary2, bary3;
709
double relative_normal_displacement;
711
bool hit = point_triangle_collision( segment_point_a, segment_point_b, dummy_index,
717
s, relative_normal_displacement );
721
hit_ss.push_back( s );
722
hit_triangles.push_back( overlapping_triangles[i] );
729
// ---------------------------------------------------------
731
/// Run intersection detection against all triangles and return the number of hits.
733
// ---------------------------------------------------------
735
unsigned int DynamicSurface::get_number_of_triangle_intersections( const Vec3d& segment_point_a,
736
const Vec3d& segment_point_b ) const
740
Vec3d aabb_low, aabb_high;
741
minmax( segment_point_a, segment_point_b, aabb_low, aabb_high );
743
std::vector<unsigned int> overlapping_triangles;
744
m_broad_phase->get_potential_triangle_collisions( aabb_low, aabb_high, overlapping_triangles );
746
for ( unsigned int i = 0; i < overlapping_triangles.size(); ++i )
748
const Vec3ui& tri = m_mesh.m_tris[ overlapping_triangles[i] ];
750
Vec3ui t = sort_triangle( tri );
751
assert( t[0] < t[1] && t[0] < t[2] && t[1] < t[2] );
753
const Vec3d& v0 = m_positions[ t[0] ];
754
const Vec3d& v1 = m_positions[ t[1] ];
755
const Vec3d& v2 = m_positions[ t[2] ];
757
unsigned int dummy_index = m_positions.size();
758
static const bool degenerate_counts_as_hit = true;
760
bool hit = segment_triangle_intersection( segment_point_a, dummy_index,
761
segment_point_b, dummy_index + 1,
765
degenerate_counts_as_hit );
782
#ifdef USE_EXACT_ARITHMETIC_RAY_CASTING
784
static Mat44d look_at(const Vec3d& from,
788
Vec3d c=-normalized(to-from);
789
Vec3d a=cross(Vec3d(0,1,0), c);
792
T(0,0)=a[0]; T(0,1)=a[1]; T(0,2)=a[2]; T(0,3) = -dot(a,from);
793
T(1,0)=b[0]; T(1,1)=b[1]; T(1,2)=b[2]; T(1,3) = -dot(b,from);
794
T(2,0)=c[0]; T(2,1)=c[1]; T(2,2)=c[2]; T(2,3) = -dot(c,from);
795
T(3,0)=0.0; T(3,1)=0.0; T(3,2)=0.0; T(3,3) = 1.0;
800
static Vec3d apply_to_point( const Mat44d& M, const Vec3d& x )
802
double w = M(3,0)*x[0] + M(3,1)*x[1] + M(3,2)*x[2] + M(3,3);
804
return Vec3d((M(0,0)*x[0] + M(0,1)*x[1] + M(0,2)*x[2] + M(0,3))/w,
805
(M(1,0)*x[0] + M(1,1)*x[1] + M(1,2)*x[2] + M(1,3))/w,
806
(M(2,0)*x[0] + M(2,1)*x[1] + M(2,2)*x[2] + M(2,3))/w );
809
// ---------------------------------------------------------
811
/// Run intersection detection against all triangles and return the number of hits. Use exact arithmetic.
813
// ---------------------------------------------------------
815
unsigned int DynamicSurface::get_number_of_triangle_intersections_exact( const Vec3d& segment_point_a,
816
const Vec3d& segment_point_b ) const
820
Vec3d aabb_low, aabb_high;
821
minmax( segment_point_a, segment_point_b, aabb_low, aabb_high );
823
std::vector<unsigned int> overlapping_triangles;
824
m_broad_phase->get_potential_triangle_collisions( aabb_low, aabb_high, overlapping_triangles );
826
for ( unsigned int i = 0; i < overlapping_triangles.size(); ++i )
828
const Vec3ui& tri = m_mesh.m_tris[ overlapping_triangles[i] ];
830
Vec3ui t = sort_triangle( tri );
831
assert( t[0] < t[1] && t[0] < t[2] && t[1] < t[2] );
833
const Vec3d v0 = (m_positions[ t[0] ]);
834
const Vec3d v1 = (m_positions[ t[1] ]);
835
const Vec3d v2 = (m_positions[ t[2] ]);
837
Vec3d ortho = cross( v1-v0, v2-v0 );
839
double ray_length = mag( segment_point_b - segment_point_a );
840
Vec3d ray_direction = ( segment_point_b - segment_point_a ) / ray_length;
841
Vec3d ray_origin( segment_point_a );
843
// find plane intersection with ray
844
double dn=dot( ray_direction, ortho );
852
double s0 = -dot( ray_origin - v0, ortho ) / dn;
854
if ( s0 < 0 || s0 > ray_length )
861
Mat44d transform = look_at( ray_direction, Vec3d(0,0,0) );
863
Vec3d a = apply_to_point( transform, v0 - ray_origin );
864
Vec3d b = apply_to_point( transform, v1 - ray_origin );
865
Vec3d c = apply_to_point( transform, v2 - ray_origin );
867
Vec2d a2( a[0], a[1] );
868
Vec2d b2( b[0], b[1] );
869
Vec2d c2( c[0], c[1] );
872
//std::cout << "2d triangle: " << a2 << ", " << b2 << ", " << c2 << std::endl;
875
int dummy_index = m_positions.size();
876
int result = sos_simplex_intersection2d( 3,
881
&(bary[0]), &(bary[1]), &(bary[2]), &(bary[3]) );
899
// ---------------------------------------------------------
901
/// Determine whether a point is inside the volume defined by the surface. Uses raycast-voting.
903
// ---------------------------------------------------------
905
bool DynamicSurface::point_is_inside( const Vec3d& p )
908
#ifdef USE_EXACT_ARITHMETIC_RAY_CASTING
910
// Exact arithmetic ray casting:
911
Vec3d ray_end( p + Vec3d( 1e+3, 1, 0 ) );
912
int hits = get_number_of_triangle_intersections_exact( p, ray_end );
914
if ( hits % 2 == 1 ) { return true; }
920
// The point is inside if there are an odd number of hits on a ray cast from the point.
921
// We'll cast six rays for numerical robustness.
925
unsigned int inside_votes = 0;
927
// shoot a ray in the positive-x direction
928
Vec3d ray_end( p + Vec3d( 1e+3, 1, 0 ) );
929
int hits = get_number_of_triangle_intersections( p, ray_end );
930
if ( hits % 2 == 1 ) { ++inside_votes; }
933
ray_end = p - Vec3d( 1e+3, 0, 1 );
934
hits = get_number_of_triangle_intersections( p, ray_end );
935
if ( hits % 2 == 1 ) { ++inside_votes; }
938
ray_end = p + Vec3d( 1, 1e+3, 0 );
939
hits = get_number_of_triangle_intersections( p, ray_end );
940
if ( hits % 2 == 1 ) { ++inside_votes; }
943
ray_end = p - Vec3d( 0, 1e+3, 1 );
944
hits = get_number_of_triangle_intersections( p, ray_end );
945
if ( hits % 2 == 1 ) { ++inside_votes; }
948
ray_end = p + Vec3d( 0, 1, 1e+3 );
949
hits = get_number_of_triangle_intersections( p, ray_end );
950
if ( hits % 2 == 1 ) { ++inside_votes; }
953
ray_end = p - Vec3d( 1, 0, 1e+3 );
954
hits = get_number_of_triangle_intersections( p, ray_end );
955
if ( hits % 2 == 1 ) { ++inside_votes; }
957
return ( inside_votes > 3 );
964
// ---------------------------------------------------------
966
/// Remove all vertices not incident on any triangles.
968
// ---------------------------------------------------------
970
void DynamicSurface::clear_deleted_vertices( )
975
for ( unsigned int i = 0; i < m_positions.size(); ++i )
977
std::vector<unsigned int>& inc_tris = m_mesh.m_vtxtri[i];
979
if ( inc_tris.size() != 0 )
981
m_positions[j] = m_positions[i];
982
m_newpositions[j] = m_newpositions[i];
983
m_velocities[j] = m_velocities[i];
984
m_masses[j] = m_masses[i];
986
for ( unsigned int t = 0; t < inc_tris.size(); ++t )
988
Vec3ui& triangle = m_mesh.m_tris[ inc_tris[t] ];
990
if ( triangle[0] == i ) { triangle[0] = j; }
991
if ( triangle[1] == i ) { triangle[1] = j; }
992
if ( triangle[2] == i ) { triangle[2] = j; }
1000
m_positions.resize(j);
1001
m_newpositions.resize(j);
1002
m_velocities.resize(j);
1008
// ---------------------------------------------------------
1010
/// Apply an impulse between two edges
1012
// ---------------------------------------------------------
1014
void DynamicSurface::apply_edge_edge_impulse( const Vec2ui& edge0,
1015
const Vec2ui& edge1,
1021
Vec3d& v0 = m_velocities[edge0[0]];
1022
Vec3d& v1 = m_velocities[edge0[1]];
1023
Vec3d& v2 = m_velocities[edge1[0]];
1024
Vec3d& v3 = m_velocities[edge1[1]];
1025
double inv_m0 = m_masses[edge0[0]] < 100 ? 1 / m_masses[edge0[0]] : 0.0;
1026
double inv_m1 = m_masses[edge0[1]] < 100 ? 1 / m_masses[edge0[1]] : 0.0;
1027
double inv_m2 = m_masses[edge1[0]] < 100 ? 1 / m_masses[edge1[0]] : 0.0;
1028
double inv_m3 = m_masses[edge1[1]] < 100 ? 1 / m_masses[edge1[1]] : 0.0;
1030
double s1 = 1.0 - s0;
1031
double s3 = 1.0 - s2;
1032
double i = magnitude/(s0*s0*inv_m0 + s1*s1*inv_m1 + s2*s2*inv_m2 + s3*s3*inv_m3);
1034
v0 += i*s0*inv_m0 * direction;
1035
v1 += i*s1*inv_m1 * direction;
1036
v2 -= i*s2*inv_m2 * direction;
1037
v3 -= i*s3*inv_m3 * direction;
1040
// ---------------------------------------------------------
1042
/// Apply an impulse between a point and a triangle
1044
// ---------------------------------------------------------
1046
void DynamicSurface::apply_triangle_point_impulse( const Vec3ui& tri,
1055
Vec3d& v0 = m_velocities[v];
1056
Vec3d& v1 = m_velocities[tri[0]];
1057
Vec3d& v2 = m_velocities[tri[1]];
1058
Vec3d& v3 = m_velocities[tri[2]];
1059
double inv_m0 = m_masses[v] < 100 ? 1 / m_masses[v] : 0.0;
1060
double inv_m1 = m_masses[tri[0]] < 100 ? 1 / m_masses[tri[0]] : 0.0;
1061
double inv_m2 = m_masses[tri[1]] < 100 ? 1 / m_masses[tri[1]] : 0.0;
1062
double inv_m3 = m_masses[tri[2]] < 100 ? 1 / m_masses[tri[2]] : 0.0;
1064
double i = magnitude / (inv_m0 + s1*s1*inv_m1 + s2*s2*inv_m2 + s3*s3*inv_m3);
1066
v0 += (i*inv_m0) * direction;
1067
v1 -= (i*s1*inv_m1) * direction;
1068
v2 -= (i*s2*inv_m2) * direction;
1069
v3 -= (i*s3*inv_m3) * direction;
1074
// ---------------------------------------------------------
1076
/// Detect all triangle-point proximities and apply repulsion impulses
1078
// ---------------------------------------------------------
1080
void DynamicSurface::handle_triangle_point_proximities( double dt )
1083
unsigned int broadphase_hits = 0;
1084
unsigned int point_triangle_proximities = 0;
1086
for ( unsigned int i = 0; i < m_mesh.m_tris.size(); ++i )
1088
const Vec3ui& tri = m_mesh.m_tris[i];
1090
if ( tri[0] == tri[1] ) { continue; }
1094
triangle_static_bounds( i, low, high );
1095
std::vector<unsigned int> potential_vertex_collisions;
1096
m_broad_phase->get_potential_vertex_collisions( low, high, potential_vertex_collisions );
1098
for ( unsigned int j = 0; j < potential_vertex_collisions.size(); ++j )
1100
unsigned int v = potential_vertex_collisions[j];
1102
if(tri[0] != v && tri[1] != v && tri[2] != v)
1105
double distance, s1, s2, s3;
1108
point_triangle_distance( m_positions[v], v,
1109
m_positions[tri[0]], tri[0],
1110
m_positions[tri[1]], tri[1],
1111
m_positions[tri[2]], tri[2],
1112
distance, s1, s2, s3, normal );
1114
if(distance < m_proximity_epsilon)
1117
double relvel = dot(normal, m_velocities[v] - (s1*m_velocities[tri[0]] + s2*m_velocities[tri[1]] + s3*m_velocities[tri[2]]));
1118
double desired_relative_velocity = ( m_proximity_epsilon - distance ) / dt;
1119
double impulse = (desired_relative_velocity - relvel);
1121
apply_triangle_point_impulse( tri, v, s1, s2, s3, normal, impulse);
1123
++point_triangle_proximities;
1133
// ---------------------------------------------------------
1135
/// Detect all edge-edge proximities and apply repulsion impulses
1137
// ---------------------------------------------------------
1139
void DynamicSurface::handle_edge_edge_proximities( double dt )
1142
unsigned int edge_edge_proximities = 0;
1144
for ( unsigned int i = 0; i < m_mesh.m_edges.size(); ++i )
1146
const Vec2ui& e0 = m_mesh.m_edges[ i ];
1148
if ( e0[0] == e0[1] ) { continue; }
1151
edge_static_bounds( i, low, high );
1152
std::vector<unsigned int> potential_collisions;
1153
m_broad_phase->get_potential_edge_collisions( low, high, potential_collisions );
1155
for ( unsigned int j = 0; j < potential_collisions.size(); ++j )
1157
if ( potential_collisions[j] <= i ) { continue; }
1159
const Vec2ui& e1 = m_mesh.m_edges[ potential_collisions[j] ];
1161
if ( e1[0] == e1[1] ) { continue; }
1163
if(e0[0] != e1[0] && e0[0] != e1[1] && e0[1] != e1[0] && e0[1] != e1[1])
1165
double distance, s0, s2;
1168
segment_segment_distance( m_positions[e0[0]], e0[0],
1169
m_positions[e0[1]], e0[1],
1170
m_positions[e1[0]], e1[0],
1171
m_positions[e1[1]], e0[1],
1172
distance, s0, s2, normal);
1174
if( distance < m_proximity_epsilon )
1176
++edge_edge_proximities;
1178
double relvel = dot(normal, s0*m_velocities[e0[0]] + (1.0-s0)*m_velocities[e0[1]] - (s2*m_velocities[e1[0]] + (1.0-s2)*m_velocities[e1[1]]));
1179
double desired_relative_velocity = ( m_proximity_epsilon - distance ) / dt;
1180
double impulse = ( desired_relative_velocity - relvel );
1182
apply_edge_edge_impulse( e0, e1, s0, s2, normal, impulse);
1191
// ---------------------------------------------------------
1193
/// Add point-triangle collision candidates for a specified triangle
1195
// ---------------------------------------------------------
1197
void DynamicSurface::add_triangle_candidates(unsigned int t, CollisionCandidateSet& collision_candidates)
1200
triangle_continuous_bounds(t, tmin, tmax);
1201
tmin -= Vec3d( m_proximity_epsilon, m_proximity_epsilon, m_proximity_epsilon );
1202
tmax += Vec3d( m_proximity_epsilon, m_proximity_epsilon, m_proximity_epsilon );
1204
std::vector<unsigned int> candidate_vertices;
1205
m_broad_phase->get_potential_vertex_collisions( tmin, tmax, candidate_vertices );
1207
for(unsigned int j = 0; j < candidate_vertices.size(); j++)
1209
add_to_collision_candidates( collision_candidates, Vec3ui(t, candidate_vertices[j], 0) );
1214
// ---------------------------------------------------------
1216
/// Add edge-edge collision candidates for a specified edge
1218
// ---------------------------------------------------------
1220
void DynamicSurface::add_edge_candidates(unsigned int e, CollisionCandidateSet& collision_candidates)
1223
edge_continuous_bounds(e, emin, emax);
1224
emin -= Vec3d( m_proximity_epsilon, m_proximity_epsilon, m_proximity_epsilon );
1225
emax += Vec3d( m_proximity_epsilon, m_proximity_epsilon, m_proximity_epsilon );
1227
std::vector<unsigned int> candidate_edges;
1228
m_broad_phase->get_potential_edge_collisions( emin, emax, candidate_edges);
1230
for(unsigned int j = 0; j < candidate_edges.size(); j++)
1232
add_to_collision_candidates( collision_candidates, Vec3ui(e, candidate_edges[j], 1) );
1236
// ---------------------------------------------------------
1238
/// Add point-triangle collision candidates for a specified vertex
1240
// ---------------------------------------------------------
1242
void DynamicSurface::add_point_candidates(unsigned int v, CollisionCandidateSet& collision_candidates)
1245
vertex_continuous_bounds(v, vmin, vmax);
1246
vmin -= Vec3d( m_proximity_epsilon, m_proximity_epsilon, m_proximity_epsilon );
1247
vmax += Vec3d( m_proximity_epsilon, m_proximity_epsilon, m_proximity_epsilon );
1249
std::vector<unsigned int> candidate_triangles;
1250
m_broad_phase->get_potential_triangle_collisions( vmin, vmax, candidate_triangles);
1252
for(unsigned int j = 0; j < candidate_triangles.size(); j++)
1254
add_to_collision_candidates( collision_candidates, Vec3ui(candidate_triangles[j], v, 0) );
1258
// ---------------------------------------------------------
1260
/// Add collision candidates for a specified vertex and all elements incident on the vertex
1262
// ---------------------------------------------------------
1264
void DynamicSurface::add_point_update_candidates(unsigned int v, CollisionCandidateSet& collision_candidates)
1266
add_point_candidates(v, collision_candidates);
1268
std::vector<unsigned int>& incident_triangles = m_mesh.m_vtxtri[v];
1269
std::vector<unsigned int>& incident_edges = m_mesh.m_vtxedge[v];
1271
for(unsigned int i = 0; i < incident_triangles.size(); i++)
1272
add_triangle_candidates(incident_triangles[i], collision_candidates);
1274
for(unsigned int i = 0; i < incident_edges.size(); i++)
1275
add_edge_candidates(incident_edges[i], collision_candidates);
1279
// ---------------------------------------------------------
1281
/// Perform one sweep of impulse collision handling, only for "deformable" vertices against "solid" triangles
1283
// ---------------------------------------------------------
1285
void DynamicSurface::handle_point_vs_solid_triangle_collisions( double dt )
1288
for(unsigned int i = 0; i < m_mesh.m_tris.size(); i++)
1290
CollisionCandidateSet triangle_collision_candidates;
1291
add_triangle_candidates(i, triangle_collision_candidates);
1293
while( false == triangle_collision_candidates.empty() )
1295
CollisionCandidateSet::iterator iter = triangle_collision_candidates.begin();
1296
Vec3ui candidate = *iter;
1297
triangle_collision_candidates.erase(iter);
1299
unsigned int t = candidate[0];
1300
Vec3ui tri = m_mesh.m_tris[t];
1301
unsigned int v = candidate[1];
1303
if ( m_masses[v] < 100 && m_masses[tri[0]] > 100 && m_masses[tri[1]] > 100 && m_masses[tri[2]] > 100 )
1306
if(tri[0] != v && tri[1] != v && tri[2] != v)
1308
double time, s1, s2, s3, rel_disp;
1311
Vec3ui sorted_tri = sort_triangle( tri );
1313
assert( sorted_tri[0] < sorted_tri[1] && sorted_tri[0] < sorted_tri[2] && sorted_tri[1] < sorted_tri[2] );
1315
if ( point_triangle_collision( m_positions[v], m_newpositions[v], v,
1316
m_positions[sorted_tri[0]], m_newpositions[sorted_tri[0]], sorted_tri[0],
1317
m_positions[sorted_tri[1]], m_newpositions[sorted_tri[1]], sorted_tri[1],
1318
m_positions[sorted_tri[2]], m_newpositions[sorted_tri[2]], sorted_tri[2],
1325
++m_num_collisions_this_step;
1327
double relvel = rel_disp / dt;
1329
apply_triangle_point_impulse(sorted_tri, v, s1, s2, s3, normal, -relvel);
1331
m_newpositions[v] = m_positions[v] + dt*m_velocities[v];
1332
m_newpositions[tri[0]] = m_positions[tri[0]] + dt*m_velocities[tri[0]];
1333
m_newpositions[tri[1]] = m_positions[tri[1]] + dt*m_velocities[tri[1]];
1334
m_newpositions[tri[2]] = m_positions[tri[2]] + dt*m_velocities[tri[2]];
1336
update_continuous_broad_phase( v );
1337
update_continuous_broad_phase( tri[0] );
1338
update_continuous_broad_phase( tri[1] );
1339
update_continuous_broad_phase( tri[2] );
1350
// ---------------------------------------------------------
1352
/// Detect all continuous collisions and apply impulses to prevent them.
1353
/// Return true if all collisions were resolved.
1355
// ---------------------------------------------------------
1357
bool DynamicSurface::handle_collisions(double dt)
1360
const unsigned int MAX_PASS = 3;
1361
const unsigned int MAX_CANDIDATES = (unsigned int) 1e+6;
1363
CollisionCandidateSet update_collision_candidates;
1365
if ( MAX_PASS == 0 )
1370
bool collision_found = true;
1371
bool candidate_overflow = false;
1373
for ( unsigned int pass = 0; ( collision_found && (pass < MAX_PASS) ); ++pass )
1375
collision_found = false;
1377
for(unsigned int i = 0; i < m_mesh.m_tris.size(); i++)
1379
CollisionCandidateSet triangle_collision_candidates;
1380
add_triangle_candidates(i, triangle_collision_candidates);
1382
while( false == triangle_collision_candidates.empty() )
1384
CollisionCandidateSet::iterator iter = triangle_collision_candidates.begin();
1385
Vec3ui candidate = *iter;
1386
triangle_collision_candidates.erase(iter);
1388
unsigned int t = candidate[0];
1389
Vec3ui tri = m_mesh.m_tris[t];
1390
unsigned int v = candidate[1];
1392
if(tri[0] != v && tri[1] != v && tri[2] != v)
1394
double time, s1, s2, s3, rel_disp;
1397
Vec3ui sorted_tri = sort_triangle( tri );
1399
if ( point_triangle_collision( m_positions[v], m_newpositions[v], v,
1400
m_positions[sorted_tri[0]], m_newpositions[sorted_tri[0]], sorted_tri[0],
1401
m_positions[sorted_tri[1]], m_newpositions[sorted_tri[1]], sorted_tri[1],
1402
m_positions[sorted_tri[2]], m_newpositions[sorted_tri[2]], sorted_tri[2],
1409
++m_num_collisions_this_step;
1411
double relvel = rel_disp / dt;
1413
apply_triangle_point_impulse(sorted_tri, v, s1, s2, s3, normal, -relvel);
1415
if ( m_verbose ) std::cout << "(PT) time: " << time << ", relative velocity before: " << relvel;
1417
double relvel_after = dot(normal, m_velocities[v] - (s1*m_velocities[sorted_tri[0]] + s2*m_velocities[sorted_tri[1]] + s3*m_velocities[sorted_tri[2]]));
1419
if ( m_verbose ) std::cout << " and relative velocity after: " << relvel_after << std::endl;
1421
m_newpositions[v] = m_positions[v] + dt*m_velocities[v];
1422
m_newpositions[tri[0]] = m_positions[tri[0]] + dt*m_velocities[tri[0]];
1423
m_newpositions[tri[1]] = m_positions[tri[1]] + dt*m_velocities[tri[1]];
1424
m_newpositions[tri[2]] = m_positions[tri[2]] + dt*m_velocities[tri[2]];
1426
update_continuous_broad_phase( v );
1427
update_continuous_broad_phase( tri[0] );
1428
update_continuous_broad_phase( tri[1] );
1429
update_continuous_broad_phase( tri[2] );
1431
if ( pass == MAX_PASS - 1 )
1433
if ( update_collision_candidates.size() < MAX_CANDIDATES )
1435
add_point_update_candidates(v, update_collision_candidates);
1436
add_point_update_candidates(tri[0], update_collision_candidates);
1437
add_point_update_candidates(tri[1], update_collision_candidates);
1438
add_point_update_candidates(tri[2], update_collision_candidates);
1442
candidate_overflow = true;
1446
collision_found = true;
1455
for(unsigned int i = 0; i < m_mesh.m_edges.size(); i++)
1457
CollisionCandidateSet edge_collision_candidates;
1458
add_edge_candidates(i, edge_collision_candidates);
1460
while ( false == edge_collision_candidates.empty() )
1462
CollisionCandidateSet::iterator iter = edge_collision_candidates.begin();
1463
Vec3ui candidate = *iter;
1464
edge_collision_candidates.erase(iter);
1466
Vec2ui e0 = m_mesh.m_edges[candidate[0]];
1467
Vec2ui e1 = m_mesh.m_edges[candidate[1]];
1469
assert( candidate[0] == i );
1471
if ( candidate[1] <= i ) { continue; }
1473
if ( e0[0] == e0[1] ) { continue; }
1474
if ( e1[0] == e1[1] ) { continue; }
1476
if(e0[0] != e1[0] && e0[0] != e1[1] && e0[1] != e1[0] && e0[1] != e1[1])
1478
double time, s0, s2, rel_disp;
1481
if ( e0[1] < e0[0] ) { swap( e0[0], e0[1] ); }
1482
if ( e1[1] < e1[0] ) { swap( e1[0], e1[1] ); }
1484
if ( ( e0[1] < e0[0] ) || ( e1[1] < e1[0] ) )
1486
std::cout << e0 << std::endl;
1487
std::cout << e1 << std::endl;
1491
if ( segment_segment_collision( m_positions[e0[0]], m_newpositions[e0[0]], e0[0],
1492
m_positions[e0[1]], m_newpositions[e0[1]], e0[1],
1493
m_positions[e1[0]], m_newpositions[e1[0]], e1[0],
1494
m_positions[e1[1]], m_newpositions[e1[1]], e1[1],
1501
++m_num_collisions_this_step;
1503
double relvel = rel_disp / dt;
1507
std::cout << "(EE) time: " << time << ", relative velocity before: " << relvel;
1508
std::cout << ", normal: " << normal;
1511
apply_edge_edge_impulse(e0, e1, s0, s2, normal, -relvel);
1513
double relvel_after = dot(normal, s0*m_velocities[e0[0]] + (1.0-s0)*m_velocities[e0[1]] - (s2*m_velocities[e1[0]] + (1.0-s2)*m_velocities[e1[1]]));
1515
if ( m_verbose ) std::cout << " and relative velocity after: " << relvel_after << std::endl;
1517
m_newpositions[e0[0]] = m_positions[e0[0]] + dt*m_velocities[e0[0]];
1518
m_newpositions[e0[1]] = m_positions[e0[1]] + dt*m_velocities[e0[1]];
1519
m_newpositions[e1[0]] = m_positions[e1[0]] + dt*m_velocities[e1[0]];
1520
m_newpositions[e1[1]] = m_positions[e1[1]] + dt*m_velocities[e1[1]];
1522
update_continuous_broad_phase( e0[0] );
1523
update_continuous_broad_phase( e0[1] );
1524
update_continuous_broad_phase( e1[0] );
1525
update_continuous_broad_phase( e1[1] );
1527
if ( pass == MAX_PASS - 1 )
1529
if ( update_collision_candidates.size() < MAX_CANDIDATES )
1531
add_point_update_candidates(e0[0], update_collision_candidates);
1532
add_point_update_candidates(e0[1], update_collision_candidates);
1533
add_point_update_candidates(e1[0], update_collision_candidates);
1534
add_point_update_candidates(e1[1], update_collision_candidates);
1538
candidate_overflow = true;
1542
collision_found = true;
1554
CollisionCandidateSet::iterator new_end = std::unique( update_collision_candidates.begin(), update_collision_candidates.end() );
1555
update_collision_candidates.erase( new_end, update_collision_candidates.end() );
1558
unsigned int n = update_collision_candidates.size();
1561
while( !update_collision_candidates.empty() && c++ < (5 * n) )
1564
CollisionCandidateSet::iterator iter = update_collision_candidates.begin();
1565
Vec3ui candidate = *iter;
1566
update_collision_candidates.erase(iter);
1570
unsigned int t = candidate[0];
1571
const Vec3ui& tri = m_mesh.m_tris[t];
1572
unsigned int v = candidate[1];
1574
if(tri[0] != v && tri[1] != v && tri[2] != v)
1576
double time, s1, s2, s3, rel_disp;
1579
Vec3ui sorted_tri = sort_triangle( tri );
1581
if ( point_triangle_collision( m_positions[v], m_newpositions[v], v,
1582
m_positions[sorted_tri[0]], m_newpositions[sorted_tri[0]], sorted_tri[0],
1583
m_positions[sorted_tri[1]], m_newpositions[sorted_tri[1]], sorted_tri[1],
1584
m_positions[sorted_tri[2]], m_newpositions[sorted_tri[2]], sorted_tri[2],
1591
++m_num_collisions_this_step;
1593
double relvel = rel_disp / dt;
1595
if ( m_verbose ) std::cout << "VT ( " << v << " " << tri << " ) relative velocity before: " << relvel;
1597
apply_triangle_point_impulse( sorted_tri, v, s1, s2, s3, normal, -relvel);
1599
double relvel_after = dot(normal, m_velocities[v] - (s1*m_velocities[tri[0]] + s2*m_velocities[tri[1]] + s3*m_velocities[tri[2]]));
1601
if ( m_verbose ) std::cout << " and relative velocity after: " << relvel_after << std::endl;
1603
m_newpositions[v] = m_positions[v] + dt*m_velocities[v];
1604
m_newpositions[tri[0]] = m_positions[tri[0]] + dt*m_velocities[tri[0]];
1605
m_newpositions[tri[1]] = m_positions[tri[1]] + dt*m_velocities[tri[1]];
1606
m_newpositions[tri[2]] = m_positions[tri[2]] + dt*m_velocities[tri[2]];
1608
update_continuous_broad_phase( v );
1609
update_continuous_broad_phase( tri[0] );
1610
update_continuous_broad_phase( tri[1] );
1611
update_continuous_broad_phase( tri[2] );
1613
if ( update_collision_candidates.size() < MAX_CANDIDATES )
1615
add_point_update_candidates(v, update_collision_candidates);
1616
add_point_update_candidates(tri[0], update_collision_candidates);
1617
add_point_update_candidates(tri[1], update_collision_candidates);
1618
add_point_update_candidates(tri[2], update_collision_candidates);
1622
candidate_overflow = true;
1629
Vec2ui e0 = m_mesh.m_edges[candidate[0]];
1630
Vec2ui e1 = m_mesh.m_edges[candidate[1]];
1631
if(e0[0] != e1[0] && e0[0] != e1[1] && e0[1] != e1[0] && e0[1] != e1[1])
1633
if ( e0[1] < e0[0] ) { swap( e0[0], e0[1] ); }
1634
if ( e1[1] < e1[0] ) { swap( e1[0], e1[1] ); }
1636
double time, s0, s2, rel_disp;
1639
if ( segment_segment_collision( m_positions[e0[0]], m_newpositions[e0[0]], e0[0],
1640
m_positions[e0[1]], m_newpositions[e0[1]], e0[1],
1641
m_positions[e1[0]], m_newpositions[e1[0]], e1[0],
1642
m_positions[e1[1]], m_newpositions[e1[1]], e1[1],
1649
++m_num_collisions_this_step;
1651
double relvel = rel_disp / dt;
1653
if ( m_verbose ) std::cout << "EE relative velocity before: " << relvel;
1655
apply_edge_edge_impulse(e0, e1, s0, s2, normal, -relvel);
1657
double relvel_after = dot(normal, s0*m_velocities[e0[0]] + (1.0-s0)*m_velocities[e0[1]] - (s2*m_velocities[e1[0]] + (1.0-s2)*m_velocities[e1[1]]));
1659
if ( m_verbose ) std::cout << " and relative velocity after: " << relvel_after << std::endl;
1661
m_newpositions[e0[0]] = m_positions[e0[0]] + dt*m_velocities[e0[0]];
1662
m_newpositions[e0[1]] = m_positions[e0[1]] + dt*m_velocities[e0[1]];
1663
m_newpositions[e1[0]] = m_positions[e1[0]] + dt*m_velocities[e1[0]];
1664
m_newpositions[e1[1]] = m_positions[e1[1]] + dt*m_velocities[e1[1]];
1667
update_continuous_broad_phase( e0[0] );
1668
update_continuous_broad_phase( e0[1] );
1669
update_continuous_broad_phase( e1[0] );
1670
update_continuous_broad_phase( e1[1] );
1672
if ( update_collision_candidates.size() < MAX_CANDIDATES )
1674
add_point_update_candidates(e0[0], update_collision_candidates);
1675
add_point_update_candidates(e0[1], update_collision_candidates);
1676
add_point_update_candidates(e1[0], update_collision_candidates);
1677
add_point_update_candidates(e1[1], update_collision_candidates);
1681
candidate_overflow = true;
1691
return ( !candidate_overflow ) && ( update_collision_candidates.empty() );
1697
// ---------------------------------------------------------
1699
/// Detect all continuous collisions
1701
// ---------------------------------------------------------
1703
bool DynamicSurface::detect_collisions( std::vector<Collision>& collisions )
1706
static const unsigned int MAX_COLLISIONS = 5000;
1708
rebuild_continuous_broad_phase();
1714
for ( unsigned int i = 0; i < m_mesh.m_tris.size(); ++i )
1716
const Vec3ui& tri = m_mesh.m_tris[i];
1718
if ( tri[0] == tri[1] || tri[1] == tri[2] || tri[2] == tri[0] ) { continue; }
1721
triangle_continuous_bounds( i, low, high );
1723
std::vector<unsigned int> potential_collisions;
1725
m_broad_phase->get_potential_vertex_collisions( low, high, potential_collisions );
1727
for ( unsigned int j = 0; j < potential_collisions.size(); ++j )
1729
unsigned int vertex_index = potential_collisions[j];
1731
assert ( m_mesh.m_vtxtri[vertex_index].size() != 0 );
1733
if( tri[0] != vertex_index && tri[1] != vertex_index && tri[2] != vertex_index )
1735
double time, s1, s2, s3, rel_disp;
1738
Vec3ui sorted_tri = sort_triangle( tri );
1740
if ( point_triangle_collision( m_positions[vertex_index], m_newpositions[vertex_index], vertex_index,
1741
m_positions[sorted_tri[0]], m_newpositions[sorted_tri[0]], sorted_tri[0],
1742
m_positions[sorted_tri[1]], m_newpositions[sorted_tri[1]], sorted_tri[1],
1743
m_positions[sorted_tri[2]], m_newpositions[sorted_tri[2]], sorted_tri[2],
1749
++m_num_collisions_this_step;
1751
Collision new_collision( false, Vec4ui( vertex_index, sorted_tri[0], sorted_tri[1], sorted_tri[2] ), normal, Vec4d( 1, -s1, -s2, -s3 ), rel_disp );
1753
collisions.push_back( new_collision );
1755
if ( collisions.size() > MAX_COLLISIONS )
1768
for ( unsigned int edge_index_a = 0; edge_index_a < m_mesh.m_edges.size(); ++edge_index_a )
1770
if ( m_mesh.m_edges[edge_index_a][0] == m_mesh.m_edges[edge_index_a][1] ) { continue; }
1773
edge_continuous_bounds(edge_index_a, low, high);
1774
std::vector<unsigned int> potential_collisions;
1775
m_broad_phase->get_potential_edge_collisions( low, high, potential_collisions );
1777
for ( unsigned int j = 0; j < potential_collisions.size(); ++j )
1780
unsigned int edge_index_b = potential_collisions[j];
1782
if ( edge_index_b <= edge_index_a ) { continue; }
1784
assert ( m_mesh.m_edges[edge_index_b][0] != m_mesh.m_edges[edge_index_b][1] );
1786
Vec2ui e0 = m_mesh.m_edges[edge_index_a];
1787
Vec2ui e1 = m_mesh.m_edges[edge_index_b];
1789
if( e0[0] != e1[0] && e0[0] != e1[1] && e0[1] != e1[0] && e0[1] != e1[1] )
1792
double time, s0, s2, rel_disp;
1795
if ( e0[1] < e0[0] ) { swap( e0[0], e0[1] ); }
1796
if ( e1[1] < e1[0] ) { swap( e1[0], e1[1] ); }
1798
if ( segment_segment_collision( m_positions[e0[0]], m_newpositions[e0[0]], e0[0],
1799
m_positions[e0[1]], m_newpositions[e0[1]], e0[1],
1800
m_positions[e1[0]], m_newpositions[e1[0]], e1[0],
1801
m_positions[e1[1]], m_newpositions[e1[1]], e1[1],
1808
++m_num_collisions_this_step;
1810
Collision new_collision( true, Vec4ui( e0[0], e0[1], e1[0], e1[1] ), normal, Vec4d( -s0, -(1-s0), s2, (1-s2) ), rel_disp );
1812
collisions.push_back( new_collision );
1814
if ( collisions.size() > MAX_COLLISIONS )
1816
std::cout << "maxed out collisions at edge " << edge_index_a << std::endl;
1820
else if ( segment_segment_collision( m_positions[e1[0]], m_newpositions[e1[0]], e1[0],
1821
m_positions[e1[1]], m_newpositions[e1[1]], e1[1],
1822
m_positions[e0[0]], m_newpositions[e0[0]], e0[0],
1823
m_positions[e0[1]], m_newpositions[e0[1]], e0[1],
1830
++m_num_collisions_this_step;
1832
Collision new_collision( true, Vec4ui( e0[0], e0[1], e1[0], e1[1] ), normal, Vec4d( -s0, -(1-s0), s2, (1-s2) ), rel_disp );
1834
collisions.push_back( new_collision );
1836
if ( collisions.size() > MAX_COLLISIONS )
1838
std::cout << "maxed out collisions at edge " << edge_index_a << std::endl;
1855
// ---------------------------------------------------------
1857
/// Detect continuous collisions among elements in the given ImpactZones, and adjacent to the given ImpactZones.
1859
// ---------------------------------------------------------
1861
void DynamicSurface::detect_new_collisions( const std::vector<ImpactZone> impact_zones, std::vector<Collision>& collisions )
1864
rebuild_continuous_broad_phase();
1866
std::vector<unsigned int> zone_vertices;
1867
std::vector<unsigned int> zone_edges;
1868
std::vector<unsigned int> zone_triangles;
1870
// Get all vertices in the impact zone
1872
for ( unsigned int i = 0; i < impact_zones.size(); ++i )
1874
for ( unsigned int j = 0; j < impact_zones[i].collisions.size(); ++j )
1876
add_unique( zone_vertices, impact_zones[i].collisions[j].vertex_indices[0] );
1877
add_unique( zone_vertices, impact_zones[i].collisions[j].vertex_indices[1] );
1878
add_unique( zone_vertices, impact_zones[i].collisions[j].vertex_indices[2] );
1879
add_unique( zone_vertices, impact_zones[i].collisions[j].vertex_indices[3] );
1881
update_continuous_broad_phase( impact_zones[i].collisions[j].vertex_indices[0] );
1882
update_continuous_broad_phase( impact_zones[i].collisions[j].vertex_indices[1] );
1883
update_continuous_broad_phase( impact_zones[i].collisions[j].vertex_indices[2] );
1884
update_continuous_broad_phase( impact_zones[i].collisions[j].vertex_indices[3] );
1889
// Get all triangles in the impact zone
1891
for ( unsigned int i = 0; i < zone_vertices.size(); ++i )
1893
for ( unsigned int j = 0; j < m_mesh.m_vtxtri[zone_vertices[i]].size(); ++j )
1895
add_unique( zone_triangles, m_mesh.m_vtxtri[zone_vertices[i]][j] );
1899
// Get all edges in the impact zone
1901
for ( unsigned int i = 0; i < zone_vertices.size(); ++i )
1903
for ( unsigned int j = 0; j < m_mesh.m_vtxedge[zone_vertices[i]].size(); ++j )
1905
add_unique( zone_edges, m_mesh.m_vtxedge[zone_vertices[i]][j] );
1909
// Check zone vertices vs all triangles
1911
for ( unsigned int i = 0; i < zone_vertices.size(); ++i )
1913
unsigned int vertex_index = zone_vertices[i];
1915
Vec3d query_low, query_high;
1916
vertex_continuous_bounds( zone_vertices[i], query_low, query_high );
1917
std::vector<unsigned int> overlapping_triangles;
1918
m_broad_phase->get_potential_triangle_collisions( query_low, query_high, overlapping_triangles );
1920
for ( unsigned int j = 0; j < overlapping_triangles.size(); ++j )
1922
const Vec3ui& tri = m_mesh.m_tris[overlapping_triangles[j]];
1924
assert( m_mesh.m_vtxtri[vertex_index].size() != 0 );
1926
if( tri[0] != vertex_index && tri[1] != vertex_index && tri[2] != vertex_index )
1928
double time, s1, s2, s3, rel_disp;
1931
Vec3ui sorted_tri = sort_triangle( tri );
1932
assert( sorted_tri[0] < sorted_tri[1] && sorted_tri[1] < sorted_tri[2] && sorted_tri[0] < sorted_tri[2] );
1934
if ( point_triangle_collision( m_positions[vertex_index], m_newpositions[vertex_index], vertex_index,
1935
m_positions[sorted_tri[0]], m_newpositions[sorted_tri[0]], sorted_tri[0],
1936
m_positions[sorted_tri[1]], m_newpositions[sorted_tri[1]], sorted_tri[1],
1937
m_positions[sorted_tri[2]], m_newpositions[sorted_tri[2]], sorted_tri[2],
1944
assert( fabs(mag(normal) - 1.0) < 1e-10 );
1946
Collision new_collision( false, Vec4ui( vertex_index, sorted_tri[0], sorted_tri[1], sorted_tri[2] ), normal, Vec4d( 1, -s1, -s2, -s3 ), rel_disp );
1948
add_unique_collision( collisions, new_collision );
1950
bool exists = false;
1951
for ( unsigned int z = 0; z < impact_zones.size(); ++z )
1953
for ( unsigned int c = 0; c < impact_zones[z].collisions.size(); ++c )
1955
if ( new_collision.same_vertices( impact_zones[z].collisions[c] ) )
1957
std::cout << "duplicate collision: " << new_collision.vertex_indices << std::endl;
1965
++m_num_collisions_this_step;
1973
// Check zone triangles vs all vertices
1975
for ( unsigned int i = 0; i < zone_triangles.size(); ++i )
1977
const Vec3ui& tri = m_mesh.m_tris[zone_triangles[i]];
1979
Vec3d query_low, query_high;
1980
triangle_continuous_bounds( zone_triangles[i], query_low, query_high );
1981
std::vector<unsigned int> overlapping_vertices;
1982
m_broad_phase->get_potential_vertex_collisions( query_low, query_high, overlapping_vertices );
1984
for ( unsigned int j = 0; j < overlapping_vertices.size(); ++j )
1986
unsigned int vertex_index = overlapping_vertices[j];
1988
assert( m_mesh.m_vtxtri[vertex_index].size() != 0 );
1990
if( tri[0] != vertex_index && tri[1] != vertex_index && tri[2] != vertex_index )
1992
double time, s1, s2, s3, rel_disp;
1995
Vec3ui sorted_tri = sort_triangle( tri );
1996
assert( sorted_tri[0] < sorted_tri[1] && sorted_tri[1] < sorted_tri[2] && sorted_tri[0] < sorted_tri[2] );
1998
if ( point_triangle_collision( m_positions[vertex_index], m_newpositions[vertex_index], vertex_index,
1999
m_positions[sorted_tri[0]], m_newpositions[sorted_tri[0]], sorted_tri[0],
2000
m_positions[sorted_tri[1]], m_newpositions[sorted_tri[1]], sorted_tri[1],
2001
m_positions[sorted_tri[2]], m_newpositions[sorted_tri[2]], sorted_tri[2],
2007
assert( fabs(mag(normal) - 1.0) < 1e-10 );
2009
Collision new_collision( false, Vec4ui( vertex_index, sorted_tri[0], sorted_tri[1], sorted_tri[2] ), normal, Vec4d( 1, -s1, -s2, -s3 ), rel_disp );
2011
add_unique_collision( collisions, new_collision );
2013
bool exists = false;
2014
for ( unsigned int z = 0; z < impact_zones.size(); ++z )
2016
for ( unsigned int c = 0; c < impact_zones[z].collisions.size(); ++c )
2018
if ( new_collision.same_vertices( impact_zones[z].collisions[c] ) )
2020
std::cout << "duplicate collision: " << new_collision.vertex_indices << std::endl;
2028
++m_num_collisions_this_step;
2037
// Check zone edges vs all edges
2039
for ( unsigned int i = 0; i < zone_edges.size(); ++i )
2041
unsigned int edge_index_a = zone_edges[i];
2043
if ( m_mesh.m_edges[edge_index_a][0] == m_mesh.m_edges[edge_index_a][1] ) { continue; }
2046
edge_continuous_bounds(edge_index_a, low, high);
2047
std::vector<unsigned int> potential_collisions;
2048
m_broad_phase->get_potential_edge_collisions( low, high, potential_collisions );
2050
for ( unsigned int j = 0; j < potential_collisions.size(); ++j )
2052
unsigned int edge_index_b = potential_collisions[j];
2054
assert ( m_mesh.m_edges[edge_index_b][0] != m_mesh.m_edges[edge_index_b][1] );
2056
Vec2ui e0 = m_mesh.m_edges[edge_index_a];
2057
Vec2ui e1 = m_mesh.m_edges[edge_index_b];
2059
if( e0[0] != e1[0] && e0[0] != e1[1] && e0[1] != e1[0] && e0[1] != e1[1] )
2061
if ( e0[1] < e0[0] ) { swap( e0[0], e0[1] ); }
2062
if ( e1[1] < e1[0] ) { swap( e1[0], e1[1] ); }
2064
assert( e0[0] < e0[1] && e1[0] < e1[1] );
2066
double time, s0, s2, rel_disp;
2069
if ( segment_segment_collision( m_positions[e0[0]], m_newpositions[e0[0]], e0[0],
2070
m_positions[e0[1]], m_newpositions[e0[1]], e0[1],
2071
m_positions[e1[0]], m_newpositions[e1[0]], e1[0],
2072
m_positions[e1[1]], m_newpositions[e1[1]], e1[1],
2081
Collision new_collision( true, Vec4ui( e0[0], e0[1], e1[0], e1[1] ), normal, Vec4d( -s0, -(1-s0), s2, (1-s2) ), rel_disp );
2083
add_unique_collision( collisions, new_collision );
2086
bool exists = false;
2087
for ( unsigned int z = 0; z < impact_zones.size(); ++z )
2089
for ( unsigned int c = 0; c < impact_zones[z].collisions.size(); ++c )
2091
if ( new_collision.same_vertices( impact_zones[z].collisions[c] ) )
2093
std::cout << "duplicate collision: " << new_collision.vertex_indices << std::endl;
2101
++m_num_collisions_this_step;
2106
else if ( segment_segment_collision( m_positions[e1[0]], m_newpositions[e1[0]], e1[0],
2107
m_positions[e1[1]], m_newpositions[e1[1]], e1[1],
2108
m_positions[e0[0]], m_newpositions[e0[0]], e0[0],
2109
m_positions[e0[1]], m_newpositions[e0[1]], e0[1],
2116
Collision new_collision( true, Vec4ui( e1[0], e1[1], e0[0], e0[1] ), normal, Vec4d( -s0, -(1-s0), s2, (1-s2) ), rel_disp );
2118
add_unique_collision( collisions, new_collision );
2120
bool exists = false;
2121
for ( unsigned int z = 0; z < impact_zones.size(); ++z )
2123
for ( unsigned int c = 0; c < impact_zones[z].collisions.size(); ++c )
2125
if ( new_collision.same_vertices( impact_zones[z].collisions[c] ) )
2127
std::cout << "duplicate collision: " << new_collision.vertex_indices << std::endl;
2135
++m_num_collisions_this_step;
2146
// ---------------------------------------------------------
2148
/// Combine impact zones which have overlapping vertex stencils
2150
// ---------------------------------------------------------
2152
void DynamicSurface::merge_impact_zones( std::vector<ImpactZone>& new_impact_zones, std::vector<ImpactZone>& master_impact_zones )
2155
bool merge_ocurred = true;
2157
for ( unsigned int i = 0; i < master_impact_zones.size(); ++i )
2159
master_impact_zones[i].all_solved = true;
2162
for ( unsigned int i = 0; i < new_impact_zones.size(); ++i )
2164
new_impact_zones[i].all_solved = false;
2168
while ( merge_ocurred )
2171
merge_ocurred = false;
2173
for ( unsigned int i = 0; i < new_impact_zones.size(); ++i )
2175
bool i_is_disjoint = true;
2177
for ( unsigned int j = 0; j < master_impact_zones.size(); ++j )
2179
// check if impact zone i and j share any vertices
2181
if ( master_impact_zones[j].share_vertices( new_impact_zones[i] ) )
2184
bool found_new_collision = false;
2186
// steal all of j's collisions
2187
for ( unsigned int c = 0; c < new_impact_zones[i].collisions.size(); ++c )
2190
bool same_collision_exists = false;
2192
for ( unsigned int m = 0; m < master_impact_zones[j].collisions.size(); ++m )
2194
if ( master_impact_zones[j].collisions[m].same_vertices( new_impact_zones[i].collisions[c] ) )
2197
same_collision_exists = true;
2203
if ( !same_collision_exists )
2205
master_impact_zones[j].collisions.push_back( new_impact_zones[i].collisions[c] );
2206
found_new_collision = true;
2210
// did we find any collisions in zone i that zone j didn't already have?
2211
if ( found_new_collision )
2213
master_impact_zones[j].all_solved &= new_impact_zones[i].all_solved;
2216
merge_ocurred = true;
2217
i_is_disjoint = false;
2223
if ( i_is_disjoint )
2225
// copy the impact zone
2227
ImpactZone new_zone;
2228
for ( unsigned int c = 0; c < new_impact_zones[i].collisions.size(); ++c )
2230
new_zone.collisions.push_back( new_impact_zones[i].collisions[c] );
2233
new_zone.all_solved = new_impact_zones[i].all_solved;
2235
master_impact_zones.push_back( new_zone );
2239
new_impact_zones = master_impact_zones;
2240
master_impact_zones.clear();
2244
master_impact_zones = new_impact_zones;
2249
// ---------------------------------------------------------
2251
/// Iteratively project out relative normal velocities for a set of collisions in an impact zone until all collisions are solved.
2253
// ---------------------------------------------------------
2255
bool DynamicSurface::iterated_inelastic_projection( ImpactZone& iz, double dt )
2257
assert( m_masses.size() == m_positions.size() );
2259
static const unsigned int MAX_PROJECTION_ITERATIONS = 20;
2261
for ( unsigned int i = 0; i < MAX_PROJECTION_ITERATIONS; ++i )
2264
bool success = inelastic_projection( iz );
2269
std::cout << "failure in inelastic projection" << std::endl;
2273
bool collision_still_exists = false;
2275
for ( unsigned int c = 0; c < iz.collisions.size(); ++c )
2277
// run collision detection on this pair again
2279
Collision& collision = iz.collisions[c];
2280
const Vec4ui& vs = collision.vertex_indices;
2282
m_newpositions[vs[0]] = m_positions[vs[0]] + dt * m_velocities[vs[0]];
2283
m_newpositions[vs[1]] = m_positions[vs[1]] + dt * m_velocities[vs[1]];
2284
m_newpositions[vs[2]] = m_positions[vs[2]] + dt * m_velocities[vs[2]];
2285
m_newpositions[vs[3]] = m_positions[vs[3]] + dt * m_velocities[vs[3]];
2287
if ( m_verbose ) { std::cout << "checking collision " << vs << std::endl; }
2289
if ( collision.is_edge_edge )
2292
double time, s0, s2, rel_disp;
2295
assert( vs[0] < vs[1] && vs[2] < vs[3] ); // should have been sorted by original collision detection
2298
if ( segment_segment_collision( m_positions[vs[0]], m_newpositions[vs[0]], vs[0],
2299
m_positions[vs[1]], m_newpositions[vs[1]], vs[1],
2300
m_positions[vs[2]], m_newpositions[vs[2]], vs[2],
2301
m_positions[vs[3]], m_newpositions[vs[3]], vs[3],
2304
time, rel_disp, false ) )
2307
std::cout << "collision remains. delta alphas: " << Vec4d( -s0, -(1-s0), s2, (1-s2) ) - collision.alphas << std::endl;
2308
std::cout << "alphas: " << Vec4d( -s0, -(1-s0), s2, (1-s2) ) << std::endl;
2309
std::cout << "normal: " << normal << std::endl;
2310
std::cout << "rel_disp: " << rel_disp << std::endl;
2311
std::cout << "time: " << time << std::endl;
2312
collision.normal = normal;
2313
collision.alphas = Vec4d( -s0, -(1-s0), s2, (1-s2) );
2314
collision.relative_displacement = rel_disp;
2315
collision_still_exists = true;
2322
double time, s1, s2, s3, rel_disp;
2325
assert( vs[1] < vs[2] && vs[2] < vs[3] && vs[1] < vs[3] ); // should have been sorted by original collision detection
2327
if ( point_triangle_collision( m_positions[vs[0]], m_newpositions[vs[0]], vs[0],
2328
m_positions[vs[1]], m_newpositions[vs[1]], vs[1],
2329
m_positions[vs[2]], m_newpositions[vs[2]], vs[2],
2330
m_positions[vs[3]], m_newpositions[vs[3]], vs[3],
2333
time, rel_disp, false ) )
2335
std::cout << "collision remains. delta alphas: " << Vec4d( 1, -s1, -s2, -s3 ) - collision.alphas << std::endl;
2336
std::cout << "alphas: " << Vec4d( 1, -s1, -s2, -s3 ) << std::endl;
2337
std::cout << "normal: " << normal << std::endl;
2338
std::cout << "rel_disp: " << rel_disp << std::endl;
2339
std::cout << "time: " << time << std::endl;
2340
collision.normal = normal;
2341
collision.alphas = Vec4d( 1, -s1, -s2, -s3 );
2342
collision.relative_displacement = rel_disp;
2343
collision_still_exists = true;
2350
if ( false == collision_still_exists )
2357
std::cout << "reached max iterations for this zone" << std::endl;
2364
// ---------------------------------------------------------
2366
/// Project out relative normal velocities for a set of collisions in an impact zone.
2368
// ---------------------------------------------------------
2370
bool DynamicSurface::inelastic_projection( const ImpactZone& iz )
2375
std::cout << " ----- using sparse solver " << std::endl;
2378
static double IMPULSE_MULTIPLIER = 0.8;
2380
const unsigned int k = iz.collisions.size(); // notation from [Harmon et al 2008]: k == number of collisions
2382
std::vector<unsigned int> zone_vertices;
2383
iz.get_all_vertices( zone_vertices );
2385
const unsigned int n = zone_vertices.size(); // n == number of distinct colliding vertices
2389
std::cout << "GCT: " << 3*n << "x" << k << std::endl;
2392
SparseMatrixDynamicCSR GCT( 3*n, k );
2395
// construct matrix grad C transpose
2396
for ( unsigned int i = 0; i < k; ++i )
2399
const Collision& coll = iz.collisions[i];
2401
for ( unsigned int v = 0; v < 4; ++v )
2403
// block row j ( == block column j of grad C )
2404
unsigned int j = coll.vertex_indices[v];
2406
std::vector<unsigned int>::iterator zone_vertex_iter = find( zone_vertices.begin(), zone_vertices.end(), j );
2408
assert( zone_vertex_iter != zone_vertices.end() );
2410
unsigned int mat_j = (unsigned int) ( zone_vertex_iter - zone_vertices.begin() );
2412
GCT(mat_j*3, i) = coll.alphas[v] * coll.normal[0];
2413
GCT(mat_j*3+1, i) = coll.alphas[v] * coll.normal[1];
2414
GCT(mat_j*3+2, i) = coll.alphas[v] * coll.normal[2];
2420
inv_masses.reserve(3*n);
2421
Array1d column_velocities;
2422
column_velocities.reserve(3*n);
2424
for ( unsigned int i = 0; i < n; ++i )
2426
if ( m_masses[zone_vertices[i]] < 100.0 )
2428
inv_masses.push_back( 1.0 / m_masses[zone_vertices[i]] );
2429
inv_masses.push_back( 1.0 / m_masses[zone_vertices[i]] );
2430
inv_masses.push_back( 1.0 / m_masses[zone_vertices[i]] );
2434
// infinite mass (scripted objects)
2436
inv_masses.push_back( 0.0 );
2437
inv_masses.push_back( 0.0 );
2438
inv_masses.push_back( 0.0 );
2441
column_velocities.push_back( m_velocities[zone_vertices[i]][0] );
2442
column_velocities.push_back( m_velocities[zone_vertices[i]][1] );
2443
column_velocities.push_back( m_velocities[zone_vertices[i]][2] );
2447
// minimize | M^(-1/2) * GC^T x - M^(1/2) * v |^2
2453
static const bool use_cgnr = false;
2454
KrylovSolverStatus solver_result;
2458
std::cout << "CGNR currently disabled: matrices are not scaled by masses." << std::endl;
2461
CGNR_Solver cg_solver;
2462
SparseMatrixStaticCSR solver_matrix( GCT );
2463
cg_solver.max_iterations = INT_MAX;
2464
solver_result = cg_solver.solve( solver_matrix, column_velocities.data, x.data );
2468
// normal equations: GC * M^(-1) GCT * x = GC * v
2471
SparseMatrixDynamicCSR A( k, k );
2473
AtDB( GCT, inv_masses.data, GCT, A );
2476
GCT.apply_transpose( column_velocities.data, b.data );
2478
if ( m_verbose ) { std::cout << "system built" << std::endl; }
2480
MINRES_CR_Solver solver;
2481
SparseMatrixStaticCSR solver_matrix( A ); // convert dynamic to static
2482
solver.max_iterations = 1000;
2483
solver_result = solver.solve( solver_matrix, b.data, x.data );
2486
if ( solver_result != KRYLOV_CONVERGED )
2488
std::cout << "CR solver failed: ";
2490
if ( solver_result == KRYLOV_BREAKDOWN )
2492
std::cout << "KRYLOV_BREAKDOWN" << std::endl;
2496
std::cout << "KRYLOV_EXCEEDED_MAX_ITERATIONS" << std::endl;
2503
Array1d applied_impulses(3*n);
2504
GCT.apply( x.data, applied_impulses.data );
2506
for ( unsigned int i = 0; i < applied_impulses.size(); ++i )
2508
column_velocities[i] -= IMPULSE_MULTIPLIER * inv_masses[i] * applied_impulses[i];
2511
for ( unsigned int i = 0; i < n; ++i )
2513
Vec3d new_velocity( column_velocities[3*i], column_velocities[3*i + 1], column_velocities[3*i + 2] );
2515
m_velocities[zone_vertices[i]][0] = column_velocities[3*i];
2516
m_velocities[zone_vertices[i]][1] = column_velocities[3*i + 1];
2517
m_velocities[zone_vertices[i]][2] = column_velocities[3*i + 2];
2526
// ---------------------------------------------------------
2528
/// Handle all collisions simultaneously by iteratively solving individual impact zones until no new collisions are detected.
2530
// ---------------------------------------------------------
2532
bool DynamicSurface::handle_collisions_simultaneous(double dt)
2536
std::vector<Vec3d> old_velocities = m_velocities;
2538
std::vector<ImpactZone> impact_zones;
2540
bool finished_detecting_collisions = false;
2542
std::vector<Collision> total_collisions;
2543
finished_detecting_collisions = detect_collisions(total_collisions);
2545
while ( false == total_collisions.empty() )
2547
// insert each new collision constraint into its own impact zone
2548
std::vector<ImpactZone> new_impact_zones;
2549
for ( unsigned int i = 0; i < total_collisions.size(); ++i )
2551
ImpactZone new_zone;
2552
new_zone.collisions.push_back( total_collisions[i] );
2553
new_impact_zones.push_back( new_zone );
2556
assert( new_impact_zones.size() == total_collisions.size() );
2558
// merge all impact zones that share vertices
2559
merge_impact_zones( new_impact_zones, impact_zones );
2561
for ( int i = 0; i < (int) impact_zones.size(); ++i )
2563
if ( impact_zones[i].all_solved )
2565
impact_zones.erase( impact_zones.begin() + i );
2570
for ( int i = 0; i < (int) impact_zones.size(); ++i )
2572
assert( false == impact_zones[i].all_solved );
2575
bool solver_ok = true;
2577
// for each impact zone
2578
for ( unsigned int i = 0; i < impact_zones.size(); ++i )
2581
std::vector<unsigned int> zone_vertices;
2583
// reset impact zone to pre-response m_velocities
2584
for ( unsigned int j = 0; j < impact_zones[i].collisions.size(); ++j )
2586
Vec4ui& vs = impact_zones[i].collisions[j].vertex_indices;
2588
m_velocities[vs[0]] = old_velocities[vs[0]];
2589
m_velocities[vs[1]] = old_velocities[vs[1]];
2590
m_velocities[vs[2]] = old_velocities[vs[2]];
2591
m_velocities[vs[3]] = old_velocities[vs[3]];
2593
zone_vertices.push_back( vs[0] );
2594
zone_vertices.push_back( vs[1] );
2595
zone_vertices.push_back( vs[2] );
2596
zone_vertices.push_back( vs[3] );
2599
// apply inelastic projection
2601
solver_ok &= iterated_inelastic_projection( impact_zones[i], dt );
2603
// reset predicted m_positions
2604
for ( unsigned int j = 0; j < impact_zones[i].collisions.size(); ++j )
2606
const Vec4ui& vs = impact_zones[i].collisions[j].vertex_indices;
2608
m_newpositions[vs[0]] = m_positions[vs[0]] + dt * m_velocities[vs[0]];
2609
m_newpositions[vs[1]] = m_positions[vs[1]] + dt * m_velocities[vs[1]];
2610
m_newpositions[vs[2]] = m_positions[vs[2]] + dt * m_velocities[vs[2]];
2611
m_newpositions[vs[3]] = m_positions[vs[3]] + dt * m_velocities[vs[3]];
2616
if ( false == solver_ok )
2618
std::cout << "solver problem" << std::endl;
2622
total_collisions.clear();
2624
if ( !finished_detecting_collisions )
2626
std::cout << "attempting to finish global collision detection" << std::endl;
2627
finished_detecting_collisions = detect_collisions( total_collisions );
2628
impact_zones.clear();
2632
detect_new_collisions( impact_zones, total_collisions );
2641
// ---------------------------------------------------------
2645
// ---------------------------------------------------------
2647
bool DynamicSurface::collision_solved( const Collision& collision )
2649
if ( collision.is_edge_edge )
2651
Vec2ui e0( collision.vertex_indices[0], collision.vertex_indices[1] );
2652
Vec2ui e1( collision.vertex_indices[2], collision.vertex_indices[3] );
2654
double time, s0, s2, rel_disp;
2657
if ( e0[1] < e0[0] ) { swap( e0[0], e0[1] ); }
2658
if ( e1[1] < e1[0] ) { swap( e1[0], e1[1] ); }
2660
if ( segment_segment_collision( m_positions[e0[0]], m_newpositions[e0[0]], e0[0],
2661
m_positions[e0[1]], m_newpositions[e0[1]], e0[1],
2662
m_positions[e1[0]], m_newpositions[e1[0]], e1[0],
2663
m_positions[e1[1]], m_newpositions[e1[1]], e1[1],
2671
else if ( segment_segment_collision( m_positions[e1[0]], m_newpositions[e1[0]], e1[0],
2672
m_positions[e1[1]], m_newpositions[e1[1]], e1[1],
2673
m_positions[e0[0]], m_newpositions[e0[0]], e0[0],
2674
m_positions[e0[1]], m_newpositions[e0[1]], e0[1],
2686
unsigned int vertex_index = collision.vertex_indices[0];
2687
Vec3ui tri( collision.vertex_indices[1], collision.vertex_indices[2], collision.vertex_indices[3] );
2689
Vec3ui sorted_tri = sort_triangle( tri );
2691
double time, s1, s2, s3, rel_disp;
2694
if ( point_triangle_collision( m_positions[vertex_index], m_newpositions[vertex_index], vertex_index,
2695
m_positions[sorted_tri[0]], m_newpositions[sorted_tri[0]], sorted_tri[0],
2696
m_positions[sorted_tri[1]], m_newpositions[sorted_tri[1]], sorted_tri[1],
2697
m_positions[sorted_tri[2]], m_newpositions[sorted_tri[2]], sorted_tri[2],
2712
// ---------------------------------------------------------
2716
// ---------------------------------------------------------
2719
bool DynamicSurface::new_rigid_impact_zones(double dt)
2723
std::vector<Vec3d> old_velocities = m_velocities;
2725
std::vector<ImpactZone> impact_zones;
2727
bool finished_detecting_collisions = false;
2729
std::vector<Collision> total_collisions;
2730
finished_detecting_collisions = detect_collisions(total_collisions);
2732
while ( false == total_collisions.empty() )
2734
// insert each new collision constraint into its own impact zone
2735
std::vector<ImpactZone> new_impact_zones;
2736
for ( unsigned int i = 0; i < total_collisions.size(); ++i )
2738
ImpactZone new_zone;
2739
new_zone.collisions.push_back( total_collisions[i] );
2740
new_impact_zones.push_back( new_zone );
2743
assert( new_impact_zones.size() == total_collisions.size() );
2745
// merge all impact zones that share vertices
2746
merge_impact_zones( new_impact_zones, impact_zones );
2748
for ( int i = 0; i < (int) impact_zones.size(); ++i )
2750
if ( impact_zones[i].all_solved )
2752
impact_zones[i].all_solved = false;
2753
impact_zones.erase( impact_zones.begin() + i );
2758
for ( int i = 0; i < (int) impact_zones.size(); ++i )
2760
assert( false == impact_zones[i].all_solved );
2763
// for each impact zone
2764
for ( unsigned int i = 0; i < impact_zones.size(); ++i )
2767
std::vector<unsigned int> zone_vertices;
2768
impact_zones[i].get_all_vertices( zone_vertices );
2769
calculate_rigid_motion(dt, zone_vertices);
2771
bool all_solved = true;
2772
for ( unsigned int c = 0; c < impact_zones[i].collisions.size(); ++c )
2774
all_solved &= collision_solved( impact_zones[i].collisions[c] );
2779
std::cout << "RIZ failed. Getting desperate!" << std::endl;
2781
Vec3d average_velocity(0,0,0);
2782
for ( unsigned int v = 0; v < zone_vertices.size(); ++v )
2784
average_velocity += m_velocities[ zone_vertices[v] ];
2786
average_velocity /= (double) zone_vertices.size();
2788
for ( unsigned int v = 0; v < zone_vertices.size(); ++v )
2790
m_velocities[ zone_vertices[v] ] = average_velocity;
2791
m_newpositions[ zone_vertices[v] ] = m_positions[ zone_vertices[v] ] + dt * m_velocities[ zone_vertices[v] ];
2797
total_collisions.clear();
2799
if ( !finished_detecting_collisions )
2801
finished_detecting_collisions = detect_collisions( total_collisions );
2802
impact_zones.clear();
2806
detect_new_collisions( impact_zones, total_collisions );
2815
// ---------------------------------------------------------
2819
// ---------------------------------------------------------
2821
void DynamicSurface::calculate_rigid_motion(double dt, std::vector<unsigned int>& vs)
2827
for(unsigned int i = 0; i < vs.size(); i++)
2829
unsigned int idx = vs[i];
2830
double m = m_masses[idx];
2834
m_velocities[idx] = ( m_newpositions[idx] - m_positions[idx] ) / dt;
2836
xcm += m * m_positions[idx];
2837
vcm += m * m_velocities[idx];
2845
for(unsigned int i = 0; i < vs.size(); i++)
2847
unsigned int idx = vs[i];
2848
double m = m_masses[idx];
2850
Vec3d xdiff = m_positions[idx] - xcm;
2851
Vec3d vdiff = m_velocities[idx] - vcm;
2853
L += m * cross(xdiff, vdiff);
2856
Mat33d I(0,0,0,0,0,0,0,0,0);
2858
for(unsigned int i = 0; i < vs.size(); i++)
2860
unsigned int idx = vs[i];
2861
double m = m_masses[idx];
2863
Vec3d xdiff = m_positions[idx] - xcm;
2864
Mat33d tens = outer(-xdiff, xdiff);
2866
double d = mag2(xdiff);
2874
// std::cout << "determinant " << determinant(I);
2875
// std::cout << "I " << std::endl << I << std::endl;
2876
// std::cout << "I^-1 " << std::endl << inverse(I) << std::endl;
2878
Vec3d w = inverse(I) * L;
2879
double wmag = mag(w);
2880
Vec3d wnorm = w/wmag;
2882
double cosdtw = cos(dt * wmag);
2883
Vec3d sindtww = sin(dt * wmag) * wnorm;
2885
Vec3d xrigid = xcm + dt * vcm;
2887
for(unsigned int i = 0; i < vs.size(); i++)
2889
unsigned int idx = vs[i];
2891
Vec3d xdiff = m_positions[idx] - xcm;
2892
Vec3d xf = dot(xdiff, wnorm) * wnorm;
2893
Vec3d xr = xdiff - xf;
2895
m_newpositions[idx] = xrigid + xf + cosdtw * xr + cross(sindtww, xr);
2897
m_velocities[idx] = ( m_newpositions[idx] - m_positions[idx] ) / dt;
2900
// std::cout << "\n\ninter-vertex distances after rigid motion: " << std::endl;
2901
// for(unsigned int i = 0; i < vs.size(); i++)
2903
// for(unsigned int j = 0; j < vs.size(); j++)
2905
// std::cout << dist( m_newpositions[vs[i]], m_newpositions[vs[j]] ) << std::endl;
2909
// std::cout << "calculated rigid motion" << std::endl;
2910
// for(unsigned int i = 0; i < vs.size(); i++)
2911
// for(unsigned int j = 0; j < vs.size(); j++)
2912
// std::cout << (dist(positions[vs[i]], positions[vs[j]]) - dist(newpositions[vs[i]], newpositions[vs[j]])) << std::endl;
2917
// ---------------------------------------------------------
2921
// ---------------------------------------------------------
2923
std::vector<unsigned int> DynamicSurface::merge_impact_zones( std::vector<unsigned int>& zones,
2930
std::vector<unsigned int> vs;
2931
for(unsigned int i = 0; i < m_positions.size(); i++)
2933
unsigned int& z = zones[i];
2959
// ---------------------------------------------------------
2961
/// Advance mesh by one time step
2963
// ---------------------------------------------------------
2965
void DynamicSurface::integrate( double dt )
2968
std::cout << "---------------------- El Topo: integration and collision handling --------------------" << std::endl;
2970
assert( m_positions.size() == m_velocities.size() );
2972
double curr_dt = dt;
2978
// Handle proximities
2980
// for(unsigned int i = 0; i < m_positions.size(); i++)
2982
// m_velocities[i] = ( m_newpositions[i] - m_positions[i] ) / curr_dt;
2985
m_num_collisions_this_step = 0;
2987
if ( m_collision_safety )
2989
rebuild_static_broad_phase();
2990
handle_edge_edge_proximities( curr_dt );
2991
handle_triangle_point_proximities( curr_dt );
2995
for(unsigned int i = 0; i < m_positions.size(); i++)
2997
m_newpositions[i] = m_positions[i] + curr_dt * m_velocities[i];
3001
if ( m_collision_safety )
3003
// Handle continuous collisions
3004
rebuild_continuous_broad_phase();
3006
bool all_collisions_handled = false;
3008
handle_point_vs_solid_triangle_collisions( curr_dt );
3009
all_collisions_handled = handle_collisions( curr_dt );
3011
// failsafe impact zones
3013
bool solver_ok = true;
3015
if ( !all_collisions_handled )
3017
//solver_ok = handle_collisions_simultaneous( curr_dt );
3022
// punt to rigid impact zones
3023
// new_rigid_impact_zones( curr_dt );
3026
//assert_predicted_mesh_is_intersection_free();
3030
m_total_num_collisions += m_num_collisions_this_step;
3033
for(unsigned int i = 0; i < m_positions.size(); i++)
3035
m_positions[i] = m_newpositions[i];
3044
// ---------------------------------------------------------
3046
/// Construct static acceleration structure
3048
// ---------------------------------------------------------
3050
void DynamicSurface::rebuild_static_broad_phase()
3052
m_broad_phase->update_broad_phase_static( *this );
3055
// ---------------------------------------------------------
3057
/// Construct continuous acceleration structure
3059
// ---------------------------------------------------------
3061
void DynamicSurface::rebuild_continuous_broad_phase()
3063
m_broad_phase->update_broad_phase_continuous( *this );
3067
// ---------------------------------------------------------
3069
/// Update the broadphase elements incident to the given vertex
3071
// ---------------------------------------------------------
3073
void DynamicSurface::update_static_broad_phase( unsigned int vertex_index )
3075
const std::vector<unsigned int>& incident_tris = m_mesh.m_vtxtri[ vertex_index ];
3076
const std::vector<unsigned int>& incident_edges = m_mesh.m_vtxedge[ vertex_index ];
3079
vertex_static_bounds( vertex_index, low, high );
3080
m_broad_phase->update_vertex( vertex_index, low, high );
3082
for ( unsigned int t = 0; t < incident_tris.size(); ++t )
3084
triangle_static_bounds( incident_tris[t], low, high );
3085
m_broad_phase->update_triangle( incident_tris[t], low, high );
3088
for ( unsigned int e = 0; e < incident_edges.size(); ++e )
3090
edge_static_bounds( incident_edges[e], low, high );
3091
m_broad_phase->update_edge( incident_edges[e], low, high );
3097
// ---------------------------------------------------------
3099
/// Update the broadphase elements incident to the given vertex, using current and predicted vertex positions
3101
// ---------------------------------------------------------
3103
void DynamicSurface::update_continuous_broad_phase( unsigned int vertex_index )
3105
const std::vector<unsigned int>& incident_tris = m_mesh.m_vtxtri[ vertex_index ];
3106
const std::vector<unsigned int>& incident_edges = m_mesh.m_vtxedge[ vertex_index ];
3109
vertex_continuous_bounds( vertex_index, low, high );
3110
m_broad_phase->update_vertex( vertex_index, low, high );
3112
for ( unsigned int t = 0; t < incident_tris.size(); ++t )
3114
triangle_continuous_bounds( incident_tris[t], low, high );
3115
m_broad_phase->update_triangle( incident_tris[t], low, high );
3118
for ( unsigned int e = 0; e < incident_edges.size(); ++e )
3120
edge_continuous_bounds( incident_edges[e], low, high );
3121
m_broad_phase->update_edge( incident_edges[e], low, high );
3126
// ---------------------------------------------------------
3128
/// Compute the (padded) AABB of a vertex
3130
// ---------------------------------------------------------
3132
void DynamicSurface::vertex_static_bounds(unsigned int v, Vec3d &xmin, Vec3d &xmax) const
3134
if ( m_mesh.m_vtxtri[v].empty() )
3136
xmin = Vec3d(m_proximity_epsilon, m_proximity_epsilon, m_proximity_epsilon);
3137
xmax = -Vec3d(m_proximity_epsilon, m_proximity_epsilon, m_proximity_epsilon);
3141
xmin = m_positions[v] - Vec3d(m_proximity_epsilon, m_proximity_epsilon, m_proximity_epsilon);
3142
xmax = m_positions[v] + Vec3d(m_proximity_epsilon, m_proximity_epsilon, m_proximity_epsilon);
3146
// ---------------------------------------------------------
3148
/// Compute the AABB of an edge
3150
// ---------------------------------------------------------
3152
void DynamicSurface::edge_static_bounds(unsigned int e, Vec3d &xmin, Vec3d &xmax) const
3154
const Vec2ui& edge = m_mesh.m_edges[e];
3155
if ( edge[0] == edge[1] )
3157
xmin = Vec3d(m_proximity_epsilon, m_proximity_epsilon, m_proximity_epsilon);
3158
xmax = -Vec3d(m_proximity_epsilon, m_proximity_epsilon, m_proximity_epsilon);
3162
minmax(m_positions[edge[0]], m_positions[edge[1]], xmin, xmax);
3163
xmin -= Vec3d(m_proximity_epsilon, m_proximity_epsilon, m_proximity_epsilon);
3164
xmax += Vec3d(m_proximity_epsilon, m_proximity_epsilon, m_proximity_epsilon);
3168
// ---------------------------------------------------------
3170
/// Compute the AABB of a triangle
3172
// ---------------------------------------------------------
3174
void DynamicSurface::triangle_static_bounds(unsigned int t, Vec3d &xmin, Vec3d &xmax) const
3176
const Vec3ui& tri = m_mesh.m_tris[t];
3177
if ( tri[0] == tri[1] )
3179
xmin = Vec3d(m_proximity_epsilon, m_proximity_epsilon, m_proximity_epsilon);
3180
xmax = -Vec3d(m_proximity_epsilon, m_proximity_epsilon, m_proximity_epsilon);
3184
minmax(m_positions[tri[0]], m_positions[tri[1]], m_positions[tri[2]], xmin, xmax);
3185
xmin -= Vec3d(m_proximity_epsilon, m_proximity_epsilon, m_proximity_epsilon);
3186
xmax += Vec3d(m_proximity_epsilon, m_proximity_epsilon, m_proximity_epsilon);
3190
// ---------------------------------------------------------
3192
/// Compute the AABB of a continuous vertex
3194
// ---------------------------------------------------------
3196
void DynamicSurface::vertex_continuous_bounds(unsigned int v, Vec3d &xmin, Vec3d &xmax) const
3198
if ( m_mesh.m_vtxtri[v].empty() )
3200
xmin = Vec3d(m_proximity_epsilon, m_proximity_epsilon, m_proximity_epsilon);
3201
xmax = -Vec3d(m_proximity_epsilon, m_proximity_epsilon, m_proximity_epsilon);
3205
minmax(m_positions[v], m_newpositions[v], xmin, xmax);
3206
xmin -= Vec3d(m_proximity_epsilon, m_proximity_epsilon, m_proximity_epsilon);
3207
xmax += Vec3d(m_proximity_epsilon, m_proximity_epsilon, m_proximity_epsilon);
3211
// ---------------------------------------------------------
3213
/// Compute the AABB of a continuous edge
3215
// ---------------------------------------------------------
3217
void DynamicSurface::edge_continuous_bounds(unsigned int e, Vec3d &xmin, Vec3d &xmax) const
3219
const Vec2ui& edge = m_mesh.m_edges[e];
3220
if ( edge[0] == edge[1] )
3222
xmin = Vec3d(m_proximity_epsilon, m_proximity_epsilon, m_proximity_epsilon);
3223
xmax = -Vec3d(m_proximity_epsilon, m_proximity_epsilon, m_proximity_epsilon);
3227
minmax(m_positions[edge[0]], m_newpositions[edge[0]], m_positions[edge[1]], m_newpositions[edge[1]], xmin, xmax);
3228
xmin -= Vec3d(m_proximity_epsilon, m_proximity_epsilon, m_proximity_epsilon);
3229
xmax += Vec3d(m_proximity_epsilon, m_proximity_epsilon, m_proximity_epsilon);
3233
// ---------------------------------------------------------
3235
/// Compute the AABB of a continuous triangle
3237
// ---------------------------------------------------------
3239
void DynamicSurface::triangle_continuous_bounds(unsigned int t, Vec3d &xmin, Vec3d &xmax) const
3241
const Vec3ui& tri = m_mesh.m_tris[t];
3242
if ( tri[0] == tri[1] )
3244
xmin = Vec3d(m_proximity_epsilon, m_proximity_epsilon, m_proximity_epsilon);
3245
xmax = -Vec3d(m_proximity_epsilon, m_proximity_epsilon, m_proximity_epsilon);
3249
minmax(m_positions[tri[0]], m_newpositions[tri[0]], m_positions[tri[1]], m_newpositions[tri[1]], m_positions[tri[2]], m_newpositions[tri[2]], xmin, xmax);
3250
xmin -= Vec3d(m_proximity_epsilon, m_proximity_epsilon, m_proximity_epsilon);
3251
xmax += Vec3d(m_proximity_epsilon, m_proximity_epsilon, m_proximity_epsilon);
3256
// --------------------------------------------------------
3258
/// Check a triangle (by index) vs all other triangles for any kind of intersection
3260
// --------------------------------------------------------
3262
bool DynamicSurface::check_triangle_vs_all_triangles_for_intersection( unsigned int tri_index )
3264
return check_triangle_vs_all_triangles_for_intersection( m_mesh.m_tris[tri_index] );
3267
// --------------------------------------------------------
3269
/// Check a triangle vs all other triangles for any kind of intersection
3271
// --------------------------------------------------------
3273
bool DynamicSurface::check_triangle_vs_all_triangles_for_intersection( const Vec3ui& tri )
3275
bool any_intersection = false;
3277
std::vector<unsigned int> overlapping_triangles;
3280
minmax( m_positions[tri[0]], m_positions[tri[1]], low, high );
3281
low -= Vec3d(m_proximity_epsilon, m_proximity_epsilon, m_proximity_epsilon);
3282
high += Vec3d(m_proximity_epsilon, m_proximity_epsilon, m_proximity_epsilon);
3284
m_broad_phase->get_potential_triangle_collisions( low, high, overlapping_triangles );
3286
for ( unsigned int i = 0; i < overlapping_triangles.size(); ++i )
3289
bool result = check_edge_triangle_intersection_by_index( tri[0], tri[1],
3290
m_mesh.m_tris[overlapping_triangles[i]][0],
3291
m_mesh.m_tris[overlapping_triangles[i]][1],
3292
m_mesh.m_tris[overlapping_triangles[i]][2],
3298
check_edge_triangle_intersection_by_index( tri[0], tri[1],
3299
m_mesh.m_tris[overlapping_triangles[i]][0],
3300
m_mesh.m_tris[overlapping_triangles[i]][1],
3301
m_mesh.m_tris[overlapping_triangles[i]][2],
3305
any_intersection = true;
3309
minmax( m_positions[tri[1]], m_positions[tri[2]], low, high );
3310
low -= Vec3d(m_proximity_epsilon, m_proximity_epsilon, m_proximity_epsilon);
3311
high += Vec3d(m_proximity_epsilon, m_proximity_epsilon, m_proximity_epsilon);
3313
overlapping_triangles.clear();
3314
m_broad_phase->get_potential_triangle_collisions( low, high, overlapping_triangles );
3316
for ( unsigned int i = 0; i < overlapping_triangles.size(); ++i )
3319
bool result = check_edge_triangle_intersection_by_index( tri[1], tri[2],
3320
m_mesh.m_tris[overlapping_triangles[i]][0],
3321
m_mesh.m_tris[overlapping_triangles[i]][1],
3322
m_mesh.m_tris[overlapping_triangles[i]][2],
3328
check_edge_triangle_intersection_by_index( tri[1], tri[2],
3329
m_mesh.m_tris[overlapping_triangles[i]][0],
3330
m_mesh.m_tris[overlapping_triangles[i]][1],
3331
m_mesh.m_tris[overlapping_triangles[i]][2],
3335
any_intersection = true;
3339
minmax( m_positions[tri[2]], m_positions[tri[0]], low, high );
3340
low -= Vec3d(m_proximity_epsilon, m_proximity_epsilon, m_proximity_epsilon);
3341
high += Vec3d(m_proximity_epsilon, m_proximity_epsilon, m_proximity_epsilon);
3343
overlapping_triangles.clear();
3344
m_broad_phase->get_potential_triangle_collisions( low, high, overlapping_triangles );
3346
for ( unsigned int i = 0; i < overlapping_triangles.size(); ++i )
3349
bool result = check_edge_triangle_intersection_by_index( tri[2], tri[0],
3350
m_mesh.m_tris[overlapping_triangles[i]][0],
3351
m_mesh.m_tris[overlapping_triangles[i]][1],
3352
m_mesh.m_tris[overlapping_triangles[i]][2],
3358
check_edge_triangle_intersection_by_index( tri[2], tri[0],
3359
m_mesh.m_tris[overlapping_triangles[i]][0],
3360
m_mesh.m_tris[overlapping_triangles[i]][1],
3361
m_mesh.m_tris[overlapping_triangles[i]][2],
3365
any_intersection = true;
3373
minmax( m_positions[tri[0]], m_positions[tri[1]], m_positions[tri[2]], low, high );
3374
low -= Vec3d(m_proximity_epsilon, m_proximity_epsilon, m_proximity_epsilon);
3375
high += Vec3d(m_proximity_epsilon, m_proximity_epsilon, m_proximity_epsilon);
3377
std::vector<unsigned int> overlapping_edges;
3378
m_broad_phase->get_potential_edge_collisions( low, high, overlapping_edges );
3380
for ( unsigned int i = 0; i < overlapping_edges.size(); ++i )
3383
bool result = check_edge_triangle_intersection_by_index( m_mesh.m_edges[overlapping_edges[i]][0], m_mesh.m_edges[overlapping_edges[i]][1],
3384
tri[0], tri[1], tri[2],
3390
check_edge_triangle_intersection_by_index( m_mesh.m_edges[overlapping_edges[i]][0], m_mesh.m_edges[overlapping_edges[i]][1],
3391
tri[0], tri[1], tri[2],
3395
any_intersection = true;
3399
return any_intersection;
3403
// ---------------------------------------------------------
3405
/// Fire an assert if any edge is intersecting any triangles
3407
// ---------------------------------------------------------
3409
void DynamicSurface::assert_mesh_is_intersection_free( bool degeneracy_counts_as_intersection )
3412
//g_intersecting_triangles.clear();
3414
m_broad_phase->update_broad_phase_static( *this );
3416
for ( unsigned int i = 0; i < m_mesh.m_tris.size(); ++i )
3418
std::vector<unsigned int> edge_candidates;
3421
triangle_static_bounds( i, low, high );
3422
m_broad_phase->get_potential_edge_collisions( low, high, edge_candidates );
3424
const Vec3ui& triangle_a = m_mesh.m_tris[i];
3426
if ( triangle_a[0] == triangle_a[1] || triangle_a[1] == triangle_a[2] || triangle_a[2] == triangle_a[0] ) { continue; }
3428
assert( m_mesh.get_edge( triangle_a[0], triangle_a[1] ) != m_mesh.m_edges.size() );
3429
assert( m_mesh.get_edge( triangle_a[1], triangle_a[2] ) != m_mesh.m_edges.size() );
3430
assert( m_mesh.get_edge( triangle_a[2], triangle_a[0] ) != m_mesh.m_edges.size() );
3432
for ( unsigned int j = 0; j < edge_candidates.size(); ++j )
3435
const Vec2ui& edge_b = m_mesh.m_edges[ edge_candidates[j] ];
3437
if ( edge_b[0] == edge_b[1] ) { continue; }
3439
if ( edge_b[0] == triangle_a[0] || edge_b[0] == triangle_a[1] || edge_b[0] == triangle_a[2]
3440
|| edge_b[1] == triangle_a[0] || edge_b[1] == triangle_a[1] || edge_b[1] == triangle_a[2] )
3445
if ( segment_triangle_intersection( m_positions[edge_b[0]], edge_b[0], m_positions[edge_b[1]], edge_b[1],
3446
m_positions[triangle_a[0]], triangle_a[0],
3447
m_positions[triangle_a[1]], triangle_a[1],
3448
m_positions[triangle_a[2]], triangle_a[2],
3449
degeneracy_counts_as_intersection, m_verbose ) )
3452
if ( m_collision_safety )
3454
std::cout << "Intersection! Triangle " << triangle_a << " vs edge " << edge_b << std::endl;
3456
segment_triangle_intersection( m_positions[edge_b[0]], edge_b[0], m_positions[edge_b[1]], edge_b[1],
3457
m_positions[triangle_a[0]], triangle_a[0],
3458
m_positions[triangle_a[1]], triangle_a[1],
3459
m_positions[triangle_a[2]], triangle_a[2],
3472
// ---------------------------------------------------------
3474
/// Using m_newpositions as the geometry, fire an assert if any edge is intersecting any triangles
3476
// ---------------------------------------------------------
3478
void DynamicSurface::assert_predicted_mesh_is_intersection_free( )
3481
rebuild_continuous_broad_phase();
3483
for ( unsigned int i = 0; i < m_mesh.m_tris.size(); ++i )
3485
std::vector<unsigned int> edge_candidates;
3488
triangle_continuous_bounds( i, low, high );
3489
m_broad_phase->get_potential_edge_collisions( low, high, edge_candidates );
3491
const Vec3ui& triangle_a = m_mesh.m_tris[i];
3493
if ( triangle_a[0] == triangle_a[1] || triangle_a[1] == triangle_a[2] || triangle_a[2] == triangle_a[0] ) { continue; }
3495
for ( unsigned int j = 0; j < edge_candidates.size(); ++j )
3498
const Vec2ui& edge_b = m_mesh.m_edges[ edge_candidates[j] ];
3500
if ( edge_b[0] == edge_b[1] ) { continue; }
3502
if ( check_edge_triangle_intersection_by_index( edge_b[0], edge_b[1],
3503
triangle_a[0], triangle_a[1], triangle_a[2],
3504
m_newpositions, m_verbose ) )
3506
if ( m_collision_safety )
3508
std::cout << "Intersection! Triangle " << triangle_a << " vs edge " << edge_b << std::endl;
3513
std::vector<Collision> check_collisions;
3514
detect_collisions( check_collisions );
3515
std::cout << "number of collisions detected: " << check_collisions.size() << std::endl;
3517
std::cout << "-----\n edge-triangle check using m_positions:" << std::endl;
3519
bool result = check_edge_triangle_intersection_by_index( edge_b[0], edge_b[1],
3520
triangle_a[0], triangle_a[1], triangle_a[2],
3521
m_positions, m_verbose );
3523
std::cout << "result: " << result << std::endl;
3525
std::cout << "-----\n edge-triangle check using new m_positions" << std::endl;
3527
result = check_edge_triangle_intersection_by_index( edge_b[0], edge_b[1],
3528
triangle_a[0], triangle_a[1], triangle_a[2],
3529
m_newpositions, m_verbose );
3531
std::cout << "result: " << result << std::endl;
3533
Vec3ui sorted_triangle = sort_triangle( triangle_a );
3535
std::cout << "sorted_triangle: " << sorted_triangle << std::endl;
3537
const Vec3d& ea = m_positions[edge_b[0]];
3538
const Vec3d& eb = m_positions[edge_b[1]];
3539
const Vec3d& ta = m_positions[sorted_triangle[0]];
3540
const Vec3d& tb = m_positions[sorted_triangle[1]];
3541
const Vec3d& tc = m_positions[sorted_triangle[2]];
3543
const Vec3d& ea_new = m_newpositions[edge_b[0]];
3544
const Vec3d& eb_new = m_newpositions[edge_b[1]];
3545
const Vec3d& ta_new = m_newpositions[sorted_triangle[0]];
3546
const Vec3d& tb_new = m_newpositions[sorted_triangle[1]];
3547
const Vec3d& tc_new = m_newpositions[sorted_triangle[2]];
3549
std::cout.precision(20);
3551
std::cout << "old: (edge0 edge1 tri0 tri1 tri2 )" << std::endl;
3553
std::cout << ea << std::endl;
3554
std::cout << eb << std::endl;
3555
std::cout << ta << std::endl;
3556
std::cout << tb << std::endl;
3557
std::cout << tc << std::endl;
3559
std::cout << "new: " << std::endl;
3561
std::cout << ea_new << std::endl;
3562
std::cout << eb_new << std::endl;
3563
std::cout << ta_new << std::endl;
3564
std::cout << tb_new << std::endl;
3565
std::cout << tc_new << std::endl;
3567
std::vector<double> possible_times;
3571
std::cout << "-----" << std::endl;
3573
if( segment_segment_collision( ea, ea_new, edge_b[0], eb, eb_new, edge_b[1], ta, ta_new, sorted_triangle[0], tb, tb_new, sorted_triangle[1] ) )
3576
bool check_flipped = segment_segment_collision( ta, ta_new, sorted_triangle[0], tb, tb_new, sorted_triangle[1], ea, ea_new, edge_b[0], eb, eb_new, edge_b[1] );
3578
assert( check_flipped );
3581
double time, s0, s2, rel_disp;
3584
assert ( segment_segment_collision( ea, ea_new, edge_b[0],
3585
eb, eb_new, edge_b[1],
3586
ta, ta_new, sorted_triangle[0],
3587
tb, tb_new, sorted_triangle[1],
3594
minmax( ea, ea_new, eb, eb_new, xmin, xmax);
3595
xmin -= Vec3d(m_proximity_epsilon, m_proximity_epsilon, m_proximity_epsilon);
3596
xmax += Vec3d(m_proximity_epsilon, m_proximity_epsilon, m_proximity_epsilon);
3597
std::vector<unsigned int> potential_collisions;
3598
m_broad_phase->get_potential_edge_collisions( xmin, xmax, potential_collisions );
3600
for ( unsigned int c = 0; c < potential_collisions.size(); ++c )
3602
const Vec2ui& other_edge = m_mesh.m_edges[ potential_collisions[c] ];
3604
if ( ( other_edge[0] == sorted_triangle[0] && other_edge[1] == sorted_triangle[1] ) ||
3605
( other_edge[1] == sorted_triangle[0] && other_edge[0] == sorted_triangle[1] ) )
3607
std::cout << "Broadphase hit" << std::endl;
3615
std::cout << "-----" << std::endl;
3617
assert( !segment_segment_collision( ea, ea_new, edge_b[0], eb, eb_new, edge_b[1], tb, tb_new, sorted_triangle[1], tc, tc_new, sorted_triangle[2] ) );
3619
std::cout << "-----" << std::endl;
3621
assert( !segment_segment_collision( ea, ea_new, edge_b[0], eb, eb_new, edge_b[1], tc, tc_new, sorted_triangle[2], ta, ta_new, sorted_triangle[0] ) );
3623
std::cout << "-----" << std::endl;
3625
assert( !point_triangle_collision( ea, ea_new, edge_b[0], ta, ta_new, sorted_triangle[0], tb, tb_new, sorted_triangle[1], tc, tc_new, sorted_triangle[2] ) );
3627
std::cout << "-----" << std::endl;
3629
assert( !point_triangle_collision( eb, eb_new, edge_b[1], ta, ta_new, sorted_triangle[0], tb, tb_new, sorted_triangle[1], tc, tc_new, sorted_triangle[2] ) );
3633
if ( m_collision_safety )
3635
std::cout << "no collisions detected" << std::endl;
added
removed
extern/eltopo/eltopo3d/dynamicsurface.cpp
