← Back to branch summary

~ubuntu-branches/ubuntu/trusty/blender/trusty

« back to all changes in this revision

Viewing changes to extern/eltopo/eltopo3d/impactzonesolver.cpp

  • Committer: Package Import Robot
  • Author(s): Jeremy Bicha
  • Date: 2013-03-06 12:08:47 UTC
  • mfrom: (1.5.1) (14.1.8 experimental)
  • Revision ID: package-import@ubuntu.com-20130306120847-frjfaryb2zrotwcg
Tags: 2.66a-1ubuntu1
https://launchpad.net/bugs/1076930
https://launchpad.net/bugs/1089256
https://launchpad.net/bugs/1052743
https://launchpad.net/bugs/999024
* Resynchronize with Debian (LP: #1076930, #1089256, #1052743, #999024,
  #1122888, #1147084)
* debian/control:
  - Lower build-depends on libavcodec-dev since we're not
    doing the libav9 transition in Ubuntu yet

Show diffs side-by-side

added added

removed removed

Lines of Context:
extern/eltopo/eltopo3d/impactzonesolver.cpp
1
 
// ---------------------------------------------------------
2
 
//
3
 
//  impactzonesolver.cpp
4
 
//  Tyson Brochu 2011
5
 
//  
6
 
//  Encapsulates two impact zone solvers: inelastic impact zones, and rigid impact zones.
7
 
//
8
 
// ---------------------------------------------------------
9
 
 
10
 
#include <collisionpipeline.h>
11
 
#include <dynamicsurface.h>
12
 
#include <impactzonesolver.h>
13
 
#include <krylov_solvers.h>
14
 
#include <mat.h>
15
 
#include <sparse_matrix.h>
16
 
#include <runstats.h>
17
 
 
18
 
namespace {
19
 
    
20
 
    // ---------------------------------------------------------
21
 
    ///
22
 
    /// Combine impact zones which have overlapping vertex stencils
23
 
    ///
24
 
    // ---------------------------------------------------------
25
 
    
26
 
    void merge_impact_zones( std::vector<ImpactZone>& new_impact_zones, std::vector<ImpactZone>& master_impact_zones )
27
 
    {
28
 
        
29
 
        bool merge_ocurred = true;
30
 
        
31
 
        for ( size_t i = 0; i < master_impact_zones.size(); ++i )
32
 
        {
33
 
            master_impact_zones[i].m_all_solved = true;
34
 
        }
35
 
        
36
 
        for ( size_t i = 0; i < new_impact_zones.size(); ++i )
37
 
        {
38
 
            new_impact_zones[i].m_all_solved = false;
39
 
        }
40
 
        
41
 
        
42
 
        while ( merge_ocurred )
43
 
        {
44
 
            
45
 
            merge_ocurred = false;
46
 
            
47
 
            for ( size_t i = 0; i < new_impact_zones.size(); ++i )
48
 
            {
49
 
                bool i_is_disjoint = true;
50
 
                
51
 
                for ( size_t j = 0; j < master_impact_zones.size(); ++j )
52
 
                {
53
 
                    // check if impact zone i and j share any vertices
54
 
                    
55
 
                    if ( master_impact_zones[j].share_vertices( new_impact_zones[i] ) )
56
 
                    {
57
 
                        
58
 
                        bool found_new_collision = false;
59
 
                        
60
 
                        // steal all of j's collisions
61
 
                        for ( size_t c = 0; c < new_impact_zones[i].m_collisions.size(); ++c )
62
 
                        {
63
 
                            
64
 
                            bool same_collision_exists = false;
65
 
                            
66
 
                            for ( size_t m = 0; m < master_impact_zones[j].m_collisions.size(); ++m )
67
 
                            {                 
68
 
                                if ( master_impact_zones[j].m_collisions[m].same_vertices( new_impact_zones[i].m_collisions[c] ) )
69
 
                                {
70
 
                                    
71
 
                                    same_collision_exists = true;
72
 
                                    break;
73
 
                                }
74
 
                                
75
 
                            }
76
 
                            
77
 
                            if ( !same_collision_exists )
78
 
                            {
79
 
                                master_impact_zones[j].m_collisions.push_back( new_impact_zones[i].m_collisions[c] );
80
 
                                found_new_collision = true;
81
 
                            }
82
 
                        }
83
 
                        
84
 
                        // did we find any collisions in zone i that zone j didn't already have?
85
 
                        if ( found_new_collision )
86
 
                        {
87
 
                            master_impact_zones[j].m_all_solved &= new_impact_zones[i].m_all_solved;
88
 
                        }
89
 
                        
90
 
                        merge_ocurred = true;
91
 
                        i_is_disjoint = false;
92
 
                        break;
93
 
                    }
94
 
                    
95
 
                }     // end for(j)
96
 
                
97
 
                if ( i_is_disjoint )
98
 
                {
99
 
                    // copy the impact zone
100
 
                    
101
 
                    ImpactZone new_zone;
102
 
                    for ( size_t c = 0; c < new_impact_zones[i].m_collisions.size(); ++c )
103
 
                    {
104
 
                        new_zone.m_collisions.push_back( new_impact_zones[i].m_collisions[c] );
105
 
                    }
106
 
                    
107
 
                    new_zone.m_all_solved = new_impact_zones[i].m_all_solved;
108
 
                    
109
 
                    master_impact_zones.push_back( new_zone );
110
 
                }
111
 
            }     // end for(i)
112
 
            
113
 
            new_impact_zones = master_impact_zones;
114
 
            master_impact_zones.clear();
115
 
            
116
 
        }  // while
117
 
        
118
 
        master_impact_zones = new_impact_zones;
119
 
        
120
 
    }
121
 
    
122
 
    // ---------------------------------------------------------
123
 
    ///
124
 
    /// Helper function: multiply transpose(A) * D * B
125
 
    ///
126
 
    // ---------------------------------------------------------
127
 
    
128
 
    void AtDB(const SparseMatrixDynamicCSR &A, const double* diagD, const SparseMatrixDynamicCSR &B, SparseMatrixDynamicCSR &C)
129
 
    {
130
 
        assert(A.m==B.m);
131
 
        C.resize(A.n, B.n);
132
 
        C.set_zero();
133
 
        for(int k=0; k<A.m; ++k)
134
 
        {
135
 
            const DynamicSparseVector& r = A.row[k];
136
 
            
137
 
            for( DynamicSparseVector::const_iterator p=r.begin(); p != r.end(); ++p )
138
 
            {
139
 
                int i = p->index;
140
 
                double multiplier = p->value * diagD[k];
141
 
                C.add_sparse_row( i, B.row[k], multiplier );
142
 
            }
143
 
        }
144
 
    }
145
 
    
146
 
}  // unnamed namespace 
147
 
 
148
 
 
149
 
// ---------------------------------------------------------
150
 
///
151
 
/// Constructor
152
 
///
153
 
// ---------------------------------------------------------
154
 
 
155
 
ImpactZoneSolver::ImpactZoneSolver( DynamicSurface& surface) :
156
 
m_surface( surface ),
157
 
m_rigid_zone_infinite_mass( 1000.0 )
158
 
{}
159
 
 
160
 
 
161
 
// ---------------------------------------------------------
162
 
///
163
 
/// Iteratively project out relative normal velocities for a set of collisions in an impact zone until all collisions are solved.
164
 
///
165
 
// ---------------------------------------------------------
166
 
 
167
 
bool ImpactZoneSolver::iterated_inelastic_projection( ImpactZone& iz, double dt )
168
 
{
169
 
    assert( m_surface.m_masses.size() == m_surface.get_num_vertices() );
170
 
    
171
 
    static const unsigned int MAX_PROJECTION_ITERATIONS = 20;
172
 
    
173
 
    for ( unsigned int i = 0; i < MAX_PROJECTION_ITERATIONS; ++i )
174
 
    {
175
 
        bool success = inelastic_projection( iz );
176
 
        
177
 
        if ( !success )
178
 
        {
179
 
            if ( m_surface.m_verbose ) { std::cout << "failure in inelastic projection" << std::endl; }
180
 
            return false;
181
 
        }
182
 
        
183
 
        bool collision_still_exists = false;
184
 
        
185
 
        for ( size_t c = 0; c < iz.m_collisions.size(); ++c )
186
 
        {
187
 
            
188
 
            // run collision detection on this pair again
189
 
            
190
 
            Collision& collision = iz.m_collisions[c];
191
 
            const Vec4st& vs = collision.m_vertex_indices;
192
 
            
193
 
            m_surface.set_newposition( vs[0], m_surface.get_position(vs[0]) + dt * m_surface.m_velocities[vs[0]]);
194
 
            m_surface.set_newposition( vs[1], m_surface.get_position(vs[1]) + dt * m_surface.m_velocities[vs[1]]);
195
 
            m_surface.set_newposition( vs[2], m_surface.get_position(vs[2]) + dt * m_surface.m_velocities[vs[2]]);
196
 
            m_surface.set_newposition( vs[3], m_surface.get_position(vs[3]) + dt * m_surface.m_velocities[vs[3]]);         
197
 
            
198
 
            if ( m_surface.m_verbose ) { std::cout << "checking collision " << vs << std::endl; }
199
 
            
200
 
            if ( collision.m_is_edge_edge )
201
 
            {
202
 
                
203
 
                double s0, s2, rel_disp;
204
 
                Vec3d normal;
205
 
                
206
 
                assert( vs[0] < vs[1] && vs[2] < vs[3] );       // should have been sorted by original collision detection
207
 
                
208
 
                if ( segment_segment_collision( m_surface.get_position(vs[0]), m_surface.get_newposition(vs[0]), vs[0],
209
 
                                               m_surface.get_position(vs[1]), m_surface.get_newposition(vs[1]), vs[1],
210
 
                                               m_surface.get_position(vs[2]), m_surface.get_newposition(vs[2]), vs[2],
211
 
                                               m_surface.get_position(vs[3]), m_surface.get_newposition(vs[3]), vs[3],
212
 
                                               s0, s2,
213
 
                                               normal,
214
 
                                               rel_disp ) )               
215
 
                {
216
 
                    collision.m_normal = normal;
217
 
                    collision.m_alphas = Vec4d( -s0, -(1-s0), s2, (1-s2) );
218
 
                    collision.m_relative_displacement = rel_disp;
219
 
                    collision_still_exists = true;
220
 
                }
221
 
                
222
 
            }
223
 
            else
224
 
            {
225
 
                
226
 
                double s1, s2, s3, rel_disp;
227
 
                Vec3d normal;
228
 
                
229
 
                assert( vs[1] < vs[2] && vs[2] < vs[3] && vs[1] < vs[3] );    // should have been sorted by original collision detection
230
 
                
231
 
                if ( point_triangle_collision( m_surface.get_position(vs[0]), m_surface.get_newposition(vs[0]), vs[0],
232
 
                                              m_surface.get_position(vs[1]), m_surface.get_newposition(vs[1]), vs[1],
233
 
                                              m_surface.get_position(vs[2]), m_surface.get_newposition(vs[2]), vs[2],
234
 
                                              m_surface.get_position(vs[3]), m_surface.get_newposition(vs[3]), vs[3],
235
 
                                              s1, s2, s3,
236
 
                                              normal,
237
 
                                              rel_disp ) )                                 
238
 
                {
239
 
                    collision.m_normal = normal;
240
 
                    collision.m_alphas = Vec4d( 1, -s1, -s2, -s3 );
241
 
                    collision.m_relative_displacement = rel_disp;
242
 
                    collision_still_exists = true;
243
 
                }
244
 
                
245
 
            }
246
 
            
247
 
        } // for collisions
248
 
        
249
 
        if ( false == collision_still_exists )  
250
 
        {
251
 
            return true; 
252
 
        }
253
 
        
254
 
    } // for iterations
255
 
    
256
 
    if ( m_surface.m_verbose ) 
257
 
    { 
258
 
        std::cout << "reached max iterations for this zone" << std::endl; 
259
 
    }
260
 
    
261
 
    return false;
262
 
    
263
 
}
264
 
 
265
 
 
266
 
// ---------------------------------------------------------
267
 
///
268
 
/// Project out relative normal velocities for a set of collisions in an impact zone.
269
 
///
270
 
// ---------------------------------------------------------
271
 
 
272
 
bool ImpactZoneSolver::inelastic_projection( const ImpactZone& iz )
273
 
{
274
 
    
275
 
    if ( m_surface.m_verbose )
276
 
    {
277
 
        std::cout << " ----- using sparse solver " << std::endl;
278
 
    }
279
 
    
280
 
    const size_t k = iz.m_collisions.size();    // notation from [Harmon et al 2008]: k == number of collisions
281
 
    
282
 
    std::vector<size_t> zone_vertices;
283
 
    iz.get_all_vertices( zone_vertices );
284
 
    
285
 
    const size_t n = zone_vertices.size();       // n == number of distinct colliding vertices
286
 
    
287
 
    if ( m_surface.m_verbose ) { std::cout << "GCT: " << 3*n << "x" << k << std::endl; }
288
 
    
289
 
    SparseMatrixDynamicCSR GCT( to_int(3*n), to_int(k) );
290
 
    GCT.set_zero();
291
 
    
292
 
    // construct matrix grad C transpose
293
 
    for ( int i = 0; i < to_int(k); ++i )
294
 
    {
295
 
        // set col i
296
 
        const Collision& coll = iz.m_collisions[i];
297
 
        
298
 
        for ( unsigned int v = 0; v < 4; ++v )
299
 
        {
300
 
            // block row j ( == block column j of grad C )
301
 
            size_t j = coll.m_vertex_indices[v];
302
 
            
303
 
            std::vector<size_t>::iterator zone_vertex_iter = find( zone_vertices.begin(), zone_vertices.end(), j );
304
 
            
305
 
            assert( zone_vertex_iter != zone_vertices.end() );
306
 
            
307
 
            int mat_j = to_int( zone_vertex_iter - zone_vertices.begin() );
308
 
            
309
 
            GCT(mat_j*3, i) = coll.m_alphas[v] * coll.m_normal[0];
310
 
            GCT(mat_j*3+1, i) = coll.m_alphas[v] * coll.m_normal[1];
311
 
            GCT(mat_j*3+2, i) = coll.m_alphas[v] * coll.m_normal[2];
312
 
            
313
 
        }
314
 
    }
315
 
    
316
 
    Array1d inv_masses;
317
 
    inv_masses.reserve(3*n);
318
 
    Array1d column_velocities;
319
 
    column_velocities.reserve(3*n);
320
 
    
321
 
    for ( size_t i = 0; i < n; ++i )
322
 
    {
323
 
        
324
 
        inv_masses.push_back( 1.0 / m_surface.m_masses[zone_vertices[i]] );
325
 
        inv_masses.push_back( 1.0 / m_surface.m_masses[zone_vertices[i]] );
326
 
        inv_masses.push_back( 1.0 / m_surface.m_masses[zone_vertices[i]] );
327
 
        
328
 
        column_velocities.push_back( m_surface.m_velocities[zone_vertices[i]][0] );
329
 
        column_velocities.push_back( m_surface.m_velocities[zone_vertices[i]][1] );
330
 
        column_velocities.push_back( m_surface.m_velocities[zone_vertices[i]][2] );
331
 
    }
332
 
    
333
 
    //
334
 
    // minimize | M^(-1/2) * GC^T x - M^(1/2) * v |^2
335
 
    //
336
 
    
337
 
    // solution vector
338
 
    Array1d x(k);
339
 
    
340
 
    KrylovSolverStatus solver_result;
341
 
    
342
 
    // normal equations: GC * M^(-1) GCT * x = GC * v
343
 
    //                   A * x = b
344
 
    
345
 
    SparseMatrixDynamicCSR A( to_int(k), to_int(k) );
346
 
    A.set_zero();
347
 
    AtDB( GCT, inv_masses.data, GCT, A ); 
348
 
    
349
 
    Array1d b(k);
350
 
    GCT.apply_transpose( column_velocities.data, b.data );   
351
 
    
352
 
    if ( m_surface.m_verbose )  { std::cout << "system built" << std::endl; }
353
 
    
354
 
    MINRES_CR_Solver solver;   
355
 
    SparseMatrixStaticCSR solver_matrix( A );    // convert dynamic to static
356
 
    solver.max_iterations = 1000;
357
 
    solver_result = solver.solve( solver_matrix, b.data, x.data ); 
358
 
    
359
 
    if ( solver_result != KRYLOV_CONVERGED )
360
 
    {
361
 
        if ( m_surface.m_verbose )
362
 
        {
363
 
            std::cout << "CR solver failed: ";      
364
 
            if ( solver_result == KRYLOV_BREAKDOWN )
365
 
            {
366
 
                std::cout << "KRYLOV_BREAKDOWN" << std::endl;
367
 
            }
368
 
            else
369
 
            {
370
 
                std::cout << "KRYLOV_EXCEEDED_MAX_ITERATIONS" << std::endl;
371
 
            }
372
 
            
373
 
            double residual_norm = BLAS::abs_max(solver.r);
374
 
            std::cout << "residual_norm: " << residual_norm << std::endl;
375
 
            
376
 
        }
377
 
        
378
 
        return false;          
379
 
    } 
380
 
    
381
 
    // apply impulses 
382
 
    Array1d applied_impulses(3*n);
383
 
    GCT.apply( x.data, applied_impulses.data );
384
 
    
385
 
    static const double IMPULSE_MULTIPLIER = 0.8;
386
 
    
387
 
    for ( size_t i = 0; i < applied_impulses.size(); ++i )
388
 
    {
389
 
        column_velocities[i] -= IMPULSE_MULTIPLIER * inv_masses[i] * applied_impulses[i];      
390
 
    }
391
 
    
392
 
    for ( size_t i = 0; i < n; ++i )
393
 
    {
394
 
        m_surface.m_velocities[zone_vertices[i]][0] = column_velocities[3*i];
395
 
        m_surface.m_velocities[zone_vertices[i]][1] = column_velocities[3*i + 1];
396
 
        m_surface.m_velocities[zone_vertices[i]][2] = column_velocities[3*i + 2];      
397
 
    }
398
 
    
399
 
    
400
 
    return true;
401
 
    
402
 
}
403
 
 
404
 
 
405
 
// ---------------------------------------------------------
406
 
///
407
 
/// Handle all collisions simultaneously by iteratively solving individual impact zones until no new collisions are detected.
408
 
///
409
 
// ---------------------------------------------------------
410
 
 
411
 
bool ImpactZoneSolver::inelastic_impact_zones(double dt)
412
 
{
413
 
    
414
 
    // copy
415
 
    std::vector<Vec3d> old_velocities = m_surface.m_velocities;
416
 
    
417
 
    std::vector<ImpactZone> impact_zones;
418
 
    
419
 
    bool finished_detecting_collisions = false;
420
 
    
421
 
    std::vector<Collision> total_collisions;
422
 
    finished_detecting_collisions = m_surface.m_collision_pipeline->detect_collisions(total_collisions);
423
 
    
424
 
    while ( false == total_collisions.empty() )
425
 
    {      
426
 
        // insert each new collision constraint into its own impact zone
427
 
        std::vector<ImpactZone> new_impact_zones;
428
 
        for ( size_t i = 0; i < total_collisions.size(); ++i )
429
 
        {
430
 
            ImpactZone new_zone;
431
 
            new_zone.m_collisions.push_back( total_collisions[i] );
432
 
            new_impact_zones.push_back( new_zone );
433
 
        }
434
 
        
435
 
        // now we have one zone for each collision
436
 
        assert( new_impact_zones.size() == total_collisions.size() );
437
 
        
438
 
        // merge all impact zones that share vertices
439
 
        merge_impact_zones( new_impact_zones, impact_zones );
440
 
        
441
 
        // remove impact zones which have been solved
442
 
        for ( int i = 0; i < (int) impact_zones.size(); ++i )
443
 
        {
444
 
            if ( impact_zones[i].m_all_solved ) 
445
 
            {
446
 
                impact_zones.erase( impact_zones.begin() + i );
447
 
                --i;
448
 
            }
449
 
        }
450
 
        
451
 
        for ( int i = 0; i < (int) impact_zones.size(); ++i )
452
 
        {
453
 
            assert( false == impact_zones[i].m_all_solved );
454
 
        }            
455
 
        
456
 
        bool all_zones_solved_ok = true;
457
 
        
458
 
        // for each impact zone
459
 
        for ( size_t i = 0; i < impact_zones.size(); ++i )
460
 
        {
461
 
            
462
 
            // reset impact zone to pre-response m_velocities
463
 
            for ( size_t j = 0; j < impact_zones[i].m_collisions.size(); ++j )
464
 
            {
465
 
                const Vec4st& vs = impact_zones[i].m_collisions[j].m_vertex_indices;            
466
 
                m_surface.m_velocities[vs[0]] = old_velocities[vs[0]];
467
 
                m_surface.m_velocities[vs[1]] = old_velocities[vs[1]];
468
 
                m_surface.m_velocities[vs[2]] = old_velocities[vs[2]];
469
 
                m_surface.m_velocities[vs[3]] = old_velocities[vs[3]]; 
470
 
            }
471
 
            
472
 
            // apply inelastic projection
473
 
            
474
 
            all_zones_solved_ok &= iterated_inelastic_projection( impact_zones[i], dt );
475
 
            
476
 
            // reset predicted positions
477
 
            for ( size_t j = 0; j < impact_zones[i].m_collisions.size(); ++j )
478
 
            {
479
 
                const Vec4st& vs = impact_zones[i].m_collisions[j].m_vertex_indices;            
480
 
                
481
 
                m_surface.set_newposition( vs[0], m_surface.get_position(vs[0]) + dt * m_surface.m_velocities[vs[0]] );
482
 
                m_surface.set_newposition( vs[1], m_surface.get_position(vs[1]) + dt * m_surface.m_velocities[vs[1]] );
483
 
                m_surface.set_newposition( vs[2], m_surface.get_position(vs[2]) + dt * m_surface.m_velocities[vs[2]] );
484
 
                m_surface.set_newposition( vs[3], m_surface.get_position(vs[3]) + dt * m_surface.m_velocities[vs[3]] );
485
 
                
486
 
            } 
487
 
            
488
 
        }  // for IZs
489
 
        
490
 
        
491
 
        if ( false == all_zones_solved_ok )
492
 
        {
493
 
            if ( m_surface.m_verbose ) 
494
 
            { 
495
 
                std::cout << "at least one impact zone had a solver problem" << std::endl; 
496
 
            }
497
 
            
498
 
            return false;
499
 
        }
500
 
        
501
 
        total_collisions.clear();
502
 
        
503
 
        if ( !finished_detecting_collisions )
504
 
        {
505
 
            if ( m_surface.m_verbose ) { std::cout << "attempting to finish global collision detection" << std::endl; }
506
 
            finished_detecting_collisions = m_surface.m_collision_pipeline->detect_collisions( total_collisions );
507
 
            impact_zones.clear();
508
 
        }
509
 
        else
510
 
        {
511
 
            bool detect_ok = m_surface.m_collision_pipeline->detect_new_collisions( impact_zones, total_collisions );
512
 
            if ( !detect_ok )
513
 
            {
514
 
                return false;
515
 
            }
516
 
        }
517
 
        
518
 
    }
519
 
    
520
 
    return true;
521
 
    
522
 
}
523
 
 
524
 
 
525
 
 
526
 
// ---------------------------------------------------------
527
 
///
528
 
///  Rigid Impact Zones, as described in [Bridson, Fedkiw, Anderson 2002].
529
 
///
530
 
// ---------------------------------------------------------
531
 
 
532
 
 
533
 
bool ImpactZoneSolver::rigid_impact_zones(double dt)
534
 
{
535
 
    
536
 
    extern RunStats g_stats;
537
 
    g_stats.add_to_int( "ImpactZoneSolver:rigid_impact_zones", 1 );
538
 
    
539
 
    // copy
540
 
    std::vector<Vec3d> old_velocities = m_surface.m_velocities;
541
 
    
542
 
    std::vector<ImpactZone> impact_zones;
543
 
    
544
 
    bool finished_detecting_collisions = false;
545
 
    
546
 
    std::vector<Collision> total_collisions;
547
 
    finished_detecting_collisions = m_surface.m_collision_pipeline->detect_collisions(total_collisions);
548
 
    
549
 
    while ( false == total_collisions.empty() )
550
 
    {      
551
 
        // insert each new collision constraint into its own impact zone
552
 
        std::vector<ImpactZone> new_impact_zones;
553
 
        for ( size_t i = 0; i < total_collisions.size(); ++i )
554
 
        {
555
 
            ImpactZone new_zone;
556
 
            new_zone.m_collisions.push_back( total_collisions[i] );
557
 
            new_impact_zones.push_back( new_zone );
558
 
            
559
 
        }  // for loop over total_collisions
560
 
        
561
 
        assert( new_impact_zones.size() == total_collisions.size() );
562
 
        
563
 
        // merge all impact zones that share vertices
564
 
        merge_impact_zones( new_impact_zones, impact_zones );
565
 
        
566
 
        for ( int i = 0; i < (int) impact_zones.size(); ++i )
567
 
        {
568
 
            if ( impact_zones[i].m_all_solved ) 
569
 
            {
570
 
                impact_zones[i].m_all_solved = false;
571
 
                impact_zones.erase( impact_zones.begin() + i );
572
 
                --i;
573
 
            }
574
 
        }
575
 
        
576
 
        for ( int i = 0; i < (int) impact_zones.size(); ++i )
577
 
        {
578
 
            assert( false == impact_zones[i].m_all_solved );
579
 
        }            
580
 
        
581
 
        // for each impact zone
582
 
        for ( size_t i = 0; i < impact_zones.size(); ++i )
583
 
        {
584
 
            
585
 
            std::vector<size_t> zone_vertices;
586
 
            impact_zones[i].get_all_vertices( zone_vertices );
587
 
            bool rigid_motion_ok = calculate_rigid_motion(dt, zone_vertices);
588
 
            
589
 
            if ( !rigid_motion_ok )
590
 
            {
591
 
                std::cout << "rigid impact zone fails" << std::endl;
592
 
                return false;
593
 
            }
594
 
            
595
 
        }  
596
 
        
597
 
        total_collisions.clear();
598
 
        
599
 
        if ( !finished_detecting_collisions )
600
 
        {
601
 
            finished_detecting_collisions = m_surface.m_collision_pipeline->detect_collisions( total_collisions );
602
 
            impact_zones.clear();
603
 
        }
604
 
        else
605
 
        {
606
 
            bool detect_ok = m_surface.m_collision_pipeline->detect_new_collisions( impact_zones, total_collisions );
607
 
            std::cout << "new collisions detected: " << total_collisions.size() << std::endl;
608
 
            
609
 
            if ( !detect_ok )
610
 
            {
611
 
                return false;
612
 
            }
613
 
            
614
 
        }
615
 
        
616
 
    }
617
 
    
618
 
    return true;
619
 
}
620
 
 
621
 
 
622
 
// ---------------------------------------------------------
623
 
///
624
 
/// Compute the best-fit rigid motion for the set of moving vertices
625
 
///
626
 
// ---------------------------------------------------------
627
 
 
628
 
bool ImpactZoneSolver::calculate_rigid_motion(double dt, std::vector<size_t>& vs)
629
 
{
630
 
    Vec3d xcm(0,0,0);
631
 
    Vec3d vcm(0,0,0);
632
 
    double mass = 0;
633
 
    
634
 
    for(size_t i = 0; i < vs.size(); i++)
635
 
    {
636
 
        size_t idx = vs[i];
637
 
        
638
 
        double m = m_surface.m_masses[idx];
639
 
        
640
 
        if ( m_surface.vertex_is_solid(idx) )
641
 
        {
642
 
            m = m_rigid_zone_infinite_mass;
643
 
        }
644
 
        
645
 
        assert( m != std::numeric_limits<double>::infinity() );
646
 
        
647
 
        mass += m;
648
 
        
649
 
        m_surface.m_velocities[idx] = ( m_surface.get_newposition(idx) - m_surface.get_position(idx) ) / dt;
650
 
        
651
 
        xcm += m * m_surface.get_position(idx);
652
 
        vcm += m * m_surface.m_velocities[idx];
653
 
    }
654
 
    
655
 
    
656
 
    double min_dist_t0 = 1e+30;
657
 
    double min_dist_t1 = 1e+30;
658
 
    for(size_t i = 0; i < vs.size(); i++)
659
 
    {
660
 
        for(size_t j = i+1; j < vs.size(); j++)
661
 
        {
662
 
            min_dist_t0 = min( min_dist_t0, dist( m_surface.get_position(vs[i]), m_surface.get_position(vs[j]) ) );
663
 
            min_dist_t1 = min( min_dist_t1, dist( m_surface.get_newposition(vs[i]), m_surface.get_newposition(vs[j]) ) );
664
 
        }
665
 
    }
666
 
    
667
 
    assert( mass > 0 );
668
 
    
669
 
    xcm /= mass;
670
 
    vcm /= mass;
671
 
    
672
 
    Vec3d L(0,0,0);
673
 
    
674
 
    for(size_t i = 0; i < vs.size(); i++)
675
 
    {
676
 
        size_t idx = vs[i];
677
 
        
678
 
        double m = m_surface.m_masses[idx];
679
 
        
680
 
        if ( m_surface.vertex_is_solid(idx) )
681
 
        {
682
 
            m = m_rigid_zone_infinite_mass;
683
 
        }
684
 
        
685
 
        assert( m != std::numeric_limits<double>::infinity() );
686
 
        
687
 
        Vec3d xdiff = m_surface.get_position(idx) - xcm;
688
 
        Vec3d vdiff = m_surface.m_velocities[idx] - vcm;
689
 
        
690
 
        L += m * cross(xdiff, vdiff);
691
 
    }
692
 
    
693
 
    Mat33d I(0,0,0,0,0,0,0,0,0);
694
 
    
695
 
    for(size_t i = 0; i < vs.size(); i++)
696
 
    {
697
 
        size_t idx = vs[i];
698
 
        double m = m_surface.m_masses[idx];
699
 
        
700
 
        if ( m_surface.vertex_is_solid(idx) )
701
 
        {
702
 
            m = m_rigid_zone_infinite_mass;
703
 
        }
704
 
        
705
 
        assert( m != std::numeric_limits<double>::infinity() );
706
 
        
707
 
        Vec3d xdiff = m_surface.get_position(idx) - xcm;
708
 
        Mat33d tens = outer(-xdiff, xdiff);
709
 
        
710
 
        double d = mag2(xdiff);
711
 
        tens(0,0) += d;
712
 
        tens(1,1) += d;
713
 
        tens(2,2) += d;
714
 
        
715
 
        I += m * tens;
716
 
    }
717
 
    
718
 
    double det = determinant(I);
719
 
    assert( det != 0 );   
720
 
    Vec3d w = inverse(I) * L;
721
 
    double wmag = mag(w);
722
 
    
723
 
    if ( wmag == 0 )
724
 
    {
725
 
        return false;
726
 
    }
727
 
    
728
 
    assert( wmag > 0 );
729
 
    
730
 
    Vec3d wnorm = w/wmag;
731
 
    
732
 
    double cosdtw = cos(dt * wmag);
733
 
    Vec3d sindtww = sin(dt * wmag) * wnorm;
734
 
    
735
 
    Vec3d xrigid = xcm + dt * vcm;
736
 
    
737
 
    double max_velocity_mag = -1.0;
738
 
    
739
 
    for(size_t i = 0; i < vs.size(); i++)
740
 
    {
741
 
        size_t idx = vs[i];
742
 
        
743
 
        Vec3d xdiff = m_surface.get_position(idx) - xcm;
744
 
        Vec3d xf = dot(xdiff, wnorm) * wnorm;
745
 
        Vec3d xr = xdiff - xf;
746
 
        
747
 
        m_surface.set_newposition( idx, xrigid + xf + cosdtw * xr + cross(sindtww, xr) );
748
 
        
749
 
        m_surface.m_velocities[idx] = ( m_surface.get_newposition(idx) - m_surface.get_position(idx) ) / dt;
750
 
        
751
 
        max_velocity_mag = max( max_velocity_mag, mag( m_surface.m_velocities[idx] ) );
752
 
        
753
 
    }
754
 
    
755
 
    min_dist_t1 = 1e+30;
756
 
    for(size_t i = 0; i < vs.size(); i++)
757
 
    {
758
 
        for(size_t j = i+1; j < vs.size(); j++)
759
 
        {
760
 
            min_dist_t1 = min( min_dist_t1, dist( m_surface.get_newposition(vs[i]), m_surface.get_newposition(vs[j]) ) );
761
 
        }
762
 
    }
763
 
    
764
 
    return true;
765
 
    
766
 
}
767
 
 
768