← Back to branch summary

~ubuntu-branches/ubuntu/trusty/yade/trusty

« back to all changes in this revision

Viewing changes to pkg/dem/Polyhedra_support.cpp

  • Committer: Package Import Robot
  • Author(s): Anton Gladky, cf3f8d9
  • Date: 2013-10-30 20:56:33 UTC
  • mfrom: (20.1.9 sid)
  • Revision ID: package-import@ubuntu.com-20131030205633-1f01r7hjce17d723
Tags: 1.05.0-2
http://bugs.debian.org/726009
[cf3f8d9] Pass -ftrack-macro-expansion=0 only if gcc>=4.8. (Closes: #726009)

Show diffs side-by-side

added added

removed removed

Lines of Context:
pkg/dem/Polyhedra_support.cpp
 
1
// © 2013 Jan Elias, http://www.fce.vutbr.cz/STM/elias.j/, elias.j@fce.vutbr.cz
 
2
// https://www.vutbr.cz/www_base/gigadisk.php?i=95194aa9a
 
3
 
 
4
#ifdef YADE_CGAL
 
5
 
 
6
#include"Polyhedra.hpp"
 
7
 
 
8
#define _USE_MATH_DEFINES
 
9
 
 
10
 
 
11
//EMPRIRICAL CONSTANTS - ADJUST IF SEGMENTATION FAULT OCCUR, IT IS A PROBLEM OF CGAL. THESE ARE USED TO CHECK CGAL IMPUTS
 
12
//DISTANCE_LIMIT controls numerical issues in calculating intersection. It should be small enough to neglect only extremely small overlaps, but large enough to prevent errors during computation of convex hull
 
13
#define DISTANCE_LIMIT 1E-11    //-11
 
14
//MERGE_PLANES_LIMIT - if two facets of two intersecting polyhedron differ less, then they are treated ose one only
 
15
#define MERGE_PLANES_LIMIT 1E-18 //18
 
16
//SIMPLIFY_LIMIT - if two facets of one polyhedron differ less, then they are joint into one facet
 
17
#define SIMPLIFY_LIMIT 1E-20 //19
 
18
//FIND_NORMAL_LIMIT - to determine which facet of intersection belongs to which polyhedron
 
19
#define FIND_NORMAL_LIMIT 1E-40
 
20
 
 
21
 
 
22
//**********************************************************************************
 
23
//return volume and centroid of polyhedron
 
24
 
 
25
bool P_volume_centroid(Polyhedron P, double * volume, Vector3r * centroid){
 
26
 
 
27
 
 
28
        Vector3r basepoint = FromCGALPoint(P.vertices_begin()->point()); 
 
29
        Vector3r A,B,C,D; 
 
30
        (*volume) = 0;
 
31
        double vtet;
 
32
        (*centroid) = Vector3r(0.,0.,0.);
 
33
        
 
34
        //compute centroid and volume
 
35
        for (Polyhedron::Facet_iterator fIter = P.facets_begin(); fIter != P.facets_end(); fIter++){
 
36
                Polyhedron::Halfedge_around_facet_circulator hfc0;
 
37
                hfc0 = fIter->facet_begin();
 
38
                int n = fIter->facet_degree();  
 
39
                A = FromCGALPoint(hfc0->vertex()->point());
 
40
                C = FromCGALPoint(hfc0->next()->vertex()->point());
 
41
                for (int i=2; i<n; i++){
 
42
                        ++hfc0;
 
43
                        B = C;
 
44
                        C = FromCGALPoint(hfc0->next()->vertex()->point());
 
45
                        vtet = std::abs((basepoint-C).dot((A-C).cross(B-C)))/6.;        
 
46
                        *volume += vtet;
 
47
                        *centroid = *centroid + (basepoint+A+B+C) / 4. * vtet;                  
 
48
                }
 
49
        }
 
50
        *centroid = *centroid/(*volume);
 
51
        return true;
 
52
}
 
53
 
 
54
//**********************************************************************************
 
55
//STOLEN FROM TETRA body of Vaclav Smilauer
 
56
 
 
57
/*! Calculates tetrahedron inertia relative to the origin (0,0,0), with unit density (scales linearly).
 
58
 
 
59
See article F. Tonon, "Explicit Exact Formulas for the 3-D Tetrahedron Inertia Tensor in Terms of its Vertex Coordinates", http://www.scipub.org/fulltext/jms2/jms2118-11.pdf
 
60
*/
 
61
 
 
62
//Centroid MUST be [0,0,0]
 
63
Matrix3r TetraInertiaTensor(Vector3r av,Vector3r bv,Vector3r cv,Vector3r dv){
 
64
        #define x1 av[0]
 
65
        #define y1 av[1]
 
66
        #define z1 av[2]
 
67
        #define x2 bv[0]
 
68
        #define y2 bv[1]
 
69
        #define z2 bv[2]
 
70
        #define x3 cv[0]
 
71
        #define y3 cv[1]
 
72
        #define z3 cv[2]
 
73
        #define x4 dv[0]
 
74
        #define y4 dv[1]
 
75
        #define z4 dv[2]
 
76
 
 
77
        // Jacobian of transformation to the reference 4hedron
 
78
        double detJ=(x2-x1)*(y3-y1)*(z4-z1)+(x3-x1)*(y4-y1)*(z2-z1)+(x4-x1)*(y2-y1)*(z3-z1)
 
79
                -(x2-x1)*(y4-y1)*(z3-z1)-(x3-x1)*(y2-y1)*(z4-z1)-(x4-x1)*(y3-y1)*(z2-z1);
 
80
        detJ=fabs(detJ);
 
81
        double a=detJ*(y1*y1+y1*y2+y2*y2+y1*y3+y2*y3+
 
82
                y3*y3+y1*y4+y2*y4+y3*y4+y4*y4+z1*z1+z1*z2+
 
83
                z2*z2+z1*z3+z2*z3+z3*z3+z1*z4+z2*z4+z3*z4+z4*z4)/60.;
 
84
        double b=detJ*(x1*x1+x1*x2+x2*x2+x1*x3+x2*x3+x3*x3+
 
85
                x1*x4+x2*x4+x3*x4+x4*x4+z1*z1+z1*z2+z2*z2+z1*z3+
 
86
                z2*z3+z3*z3+z1*z4+z2*z4+z3*z4+z4*z4)/60.;
 
87
        double c=detJ*(x1*x1+x1*x2+x2*x2+x1*x3+x2*x3+x3*x3+x1*x4+
 
88
                x2*x4+x3*x4+x4*x4+y1*y1+y1*y2+y2*y2+y1*y3+
 
89
                y2*y3+y3*y3+y1*y4+y2*y4+y3*y4+y4*y4)/60.;
 
90
        // a' in the article etc.
 
91
        double a__=detJ*(2*y1*z1+y2*z1+y3*z1+y4*z1+y1*z2+
 
92
                2*y2*z2+y3*z2+y4*z2+y1*z3+y2*z3+2*y3*z3+
 
93
                y4*z3+y1*z4+y2*z4+y3*z4+2*y4*z4)/120.;
 
94
        double b__=detJ*(2*x1*z1+x2*z1+x3*z1+x4*z1+x1*z2+
 
95
                2*x2*z2+x3*z2+x4*z2+x1*z3+x2*z3+2*x3*z3+
 
96
                x4*z3+x1*z4+x2*z4+x3*z4+2*x4*z4)/120.;
 
97
        double c__=detJ*(2*x1*y1+x2*y1+x3*y1+x4*y1+x1*y2+
 
98
                2*x2*y2+x3*y2+x4*y2+x1*y3+x2*y3+2*x3*y3+
 
99
                x4*y3+x1*y4+x2*y4+x3*y4+2*x4*y4)/120.;
 
100
 
 
101
        Matrix3r ret; ret<<
 
102
                a   , -c__, -b__,
 
103
                -c__, b   , -a__,
 
104
                -b__, -a__, c   ;
 
105
        return ret;
 
106
 
 
107
        #undef x1
 
108
        #undef y1
 
109
        #undef z1
 
110
        #undef x2
 
111
        #undef y2
 
112
        #undef z2
 
113
        #undef x3
 
114
        #undef y3
 
115
        #undef z3
 
116
        #undef x4
 
117
        #undef y4
 
118
        #undef z4
 
119
}
 
120
 
 
121
//**********************************************************************************
 
122
//distace of point from a plane (squared) with sign
 
123
double Oriented_squared_distance(Plane P, CGALpoint x){
 
124
        double h = P.a()*x.x()+P.b()*x.y()+P.c()*x.z()+P.d();
 
125
        return ((h>0.)-(h<0.))*pow(h,2)/(CGALvector(P.a(),P.b(),P.c())).squared_length();
 
126
}
 
127
 
 
128
//**********************************************************************************
 
129
// test if point is inside polyhedra in strong sence, i.e. boundary location is not enough
 
130
bool Is_inside_Polyhedron(Polyhedron P, CGALpoint inside){
 
131
        Polyhedron::Plane_iterator pi;  
 
132
        for(pi=P.planes_begin(); pi!=P.planes_end(); ++pi){
 
133
                if( ! pi->has_on_negative_side(inside)) return false;
 
134
        }
 
135
        return true;
 
136
}
 
137
 
 
138
//**********************************************************************************
 
139
// test if point is inside polyhedra not closer than lim to its boundary
 
140
bool Is_inside_Polyhedron(Polyhedron P, CGALpoint inside, double lim){
 
141
        Polyhedron::Plane_iterator pi;  
 
142
        lim = pow(lim,2);
 
143
        for(pi=P.planes_begin(); pi!=P.planes_end(); ++pi){
 
144
                if(Oriented_squared_distance(*pi, inside)>-lim) return false;
 
145
        }
 
146
        return true;
 
147
}
 
148
 
 
149
//**********************************************************************************
 
150
//test if two polyhedron intersect
 
151
bool do_intersect(Polyhedron A, Polyhedron B){
 
152
        std::vector<int> sep_plane;
 
153
        sep_plane.assign(3,0);
 
154
        return do_intersect(A, B, sep_plane);
 
155
}
 
156
 
 
157
//**********************************************************************************
 
158
//test if two polyhedron intersect based on previous data
 
159
bool do_intersect(Polyhedron A, Polyhedron B, std::vector<int> &sep_plane){
 
160
 
 
161
 
 
162
        bool found;
 
163
        //check previous separation plane
 
164
        switch (sep_plane[0]){
 
165
                case 1: //separation plane was previously determined as sep_plane[2]-th plane of A polyhedron 
 
166
                        {
 
167
                        if(unlikely((unsigned) sep_plane[2]>=A.size_of_facets())) break;
 
168
                        Polyhedron::Facet_iterator fIter = A.facets_begin();                                    
 
169
                        for (int i=0; i<sep_plane[2]; i++) ++fIter;
 
170
                        found = true;
 
171
                        for (Polyhedron::Vertex_iterator vIter = B.vertices_begin(); vIter != B.vertices_end(); ++vIter){
 
172
                                if (! fIter->plane().has_on_positive_side(vIter->point())) {found = false; break;};     
 
173
                        }  
 
174
                        if(found) return false;                 
 
175
                        }                       
 
176
                        break;
 
177
                case 2: //separation plane was previously determined as sep_plane[2]-th plane of B polyhedron 
 
178
                        {
 
179
                        if(unlikely((unsigned) sep_plane[2]>=B.size_of_facets())) break; 
 
180
                        Polyhedron::Facet_iterator fIter = B.facets_begin();            
 
181
                        for (int i=0; i<sep_plane[2]; i++) ++fIter;
 
182
                        found = true;
 
183
                        for (Polyhedron::Vertex_iterator vIter = A.vertices_begin(); vIter != A.vertices_end(); ++vIter){
 
184
                                if (! fIter->plane().has_on_positive_side(vIter->point())) {found = false; break;};     
 
185
                        }  
 
186
                        if(found) return false;
 
187
                        }
 
188
                        break;
 
189
                case 3: //separation plane was previously given by sep_plane[1]-th and sep_plane[2]-th edge of A & B polyhedrons
 
190
                        {
 
191
                        if(unlikely((unsigned) sep_plane[1]>=A.size_of_halfedges()/2)) break; 
 
192
                        if(unlikely((unsigned) sep_plane[2]>=B.size_of_halfedges()/2)) break;   
 
193
                        Polyhedron::Edge_iterator eIter1 = A.edges_begin(); 
 
194
                        Polyhedron::Edge_iterator eIter2 = B.edges_begin();
 
195
                        for (int i=0; i<sep_plane[1]; i++) ++eIter1;
 
196
                        for (int i=0; i<sep_plane[2]; i++) ++eIter2;
 
197
                        found = true;
 
198
                        Plane X(eIter1->vertex()->point(),CGAL::cross_product((eIter1->vertex()->point() - eIter1->opposite()->vertex()->point()),(eIter2->vertex()->point() - eIter2->opposite()->vertex()->point())));                
 
199
                        if (!X.has_on_positive_side(B.vertices_begin()->point())) X = X.opposite();
 
200
 
 
201
                        double lim = pow(DISTANCE_LIMIT,2);
 
202
                        for (Polyhedron::Vertex_iterator vIter = A.vertices_begin(); vIter != A.vertices_end(); ++vIter){
 
203
                                if (Oriented_squared_distance(X, vIter->point())>lim) {found = false; break;};  
 
204
                        }                               
 
205
                        for (Polyhedron::Vertex_iterator vIter = B.vertices_begin(); vIter != B.vertices_end(); ++vIter){
 
206
                                if (! X.has_on_positive_side(vIter->point())) {found = false; break;};  
 
207
                        }
 
208
                        if(found) return false;
 
209
                        }
 
210
                        break;  
 
211
        }
 
212
 
 
213
        //regular test with no previous information about separating plane
 
214
        //test all planes from A
 
215
        int i = 0;
 
216
        for (Polyhedron::Facet_iterator fIter = A.facets_begin(); fIter != A.facets_end(); fIter++, i++){
 
217
                found = true;
 
218
                for (Polyhedron::Vertex_iterator vIter = B.vertices_begin(); vIter != B.vertices_end(); ++vIter){
 
219
                        if (! fIter->plane().has_on_positive_side(vIter->point())) {found = false; break;};     
 
220
                }  
 
221
                if(found) {sep_plane[0] = 1; sep_plane[1] = 1; sep_plane[2] = i; return false;}
 
222
        }
 
223
        //test all planes from B
 
224
        i = 0;
 
225
        for (Polyhedron::Facet_iterator fIter = B.facets_begin(); fIter != B.facets_end(); fIter++, i++){
 
226
                found = true;
 
227
                for (Polyhedron::Vertex_iterator vIter = A.vertices_begin(); vIter != A.vertices_end(); ++vIter){
 
228
                        if (! fIter->plane().has_on_positive_side(vIter->point())) {found = false; break;};     
 
229
                }  
 
230
                if(found) {sep_plane[0] = 2; sep_plane[1] = 2; sep_plane[2] = i; return false;}
 
231
        }
 
232
        //test all pairs of edges from A & B
 
233
        Plane X;
 
234
        CGALvector vA;
 
235
        double lim = pow(DISTANCE_LIMIT,2);
 
236
        i = 0;
 
237
        for (Polyhedron::Edge_iterator eIter1 = A.edges_begin(); eIter1 != A.edges_end(); ++eIter1, i++){
 
238
                vA = eIter1->vertex()->point() - eIter1->opposite()->vertex()->point();
 
239
                int j = 0;
 
240
                for (Polyhedron::Edge_iterator eIter2 = B.edges_begin(); eIter2 != B.edges_end(); ++eIter2, j++){
 
241
                        found = true;
 
242
                        X = Plane(eIter1->vertex()->point(),CGAL::cross_product(vA,(eIter2->vertex()->point() - eIter2->opposite()->vertex()->point()) ));              
 
243
                        if (!X.has_on_positive_side(B.vertices_begin()->point())) X = X.opposite();
 
244
                        for (Polyhedron::Vertex_iterator vIter = A.vertices_begin(); vIter != A.vertices_end(); ++vIter){
 
245
                                if (Oriented_squared_distance(X, vIter->point())>lim) {found = false; break;};  
 
246
                        }                               
 
247
                        for (Polyhedron::Vertex_iterator vIter = B.vertices_begin(); vIter != B.vertices_end(); ++vIter){
 
248
                                if (! X.has_on_positive_side(vIter->point())) {found = false; break;};  
 
249
                        }
 
250
                        if(found) {sep_plane[0] = 3; sep_plane[1] = i; sep_plane[2] = j; return false;}
 
251
                }               
 
252
        }       
 
253
        
 
254
        sep_plane[0] = 0;
 
255
        return true;
 
256
}
 
257
 
 
258
//**********************************************************************************
 
259
//norm of difference between two planes
 
260
double PlaneDifference(const Plane &a, const Plane &b){
 
261
        double la = sqrt(pow(a.a(),2) + pow(a.b(),2) + pow(a.c(),2) + pow(a.d(),2)); 
 
262
        double lb = sqrt(pow(b.a(),2) + pow(b.b(),2) + pow(b.c(),2) + pow(b.d(),2)); 
 
263
        return pow(a.a()/la-b.a()/lb,2) + pow(a.b()/la-b.b()/lb,2) + pow(a.c()/la-b.c()/lb,2) + pow(a.d()/la-b.d()/lb,2);
 
264
 
 
265
        //in case we do not care of the orientation     
 
266
        //return min(pow(a.a()/la-b.a()/lb,2) + pow(a.b()/la-b.b()/lb,2) + pow(a.c()/la-b.c()/lb,2) + pow(a.d()/la-b.d()/lb,2),pow(a.a()/la+b.a()/lb,2) + pow(a.b()/la+b.b()/lb,2) + pow(a.c()/la+b.c()/lb,2) + pow(a.d()/la+b.d()/lb,2));
 
267
}
 
268
 
 
269
//**********************************************************************************
 
270
//connect triagular facets if possible
 
271
Polyhedron Simplify(Polyhedron P, double limit){
 
272
        bool elimination = true;
 
273
        while(elimination){
 
274
                elimination = false;
 
275
                for (Polyhedron::Edge_iterator hei = P.edges_begin(); hei!=P.edges_end(); ++hei){                       
 
276
                        if (PlaneDifference(hei->facet()->plane(),hei->opposite()->facet()->plane()) < limit){                  
 
277
                                if (hei->vertex()->vertex_degree() < 3) hei = P.erase_center_vertex(hei); 
 
278
                                else if(hei->opposite()->vertex()->vertex_degree() < 3) hei = P.erase_center_vertex(hei->opposite());
 
279
                                else hei = P.join_facet(hei);
 
280
                                elimination = true;
 
281
                                break;
 
282
                        }               
 
283
                }
 
284
        }
 
285
        if (P.size_of_facets() < 4) P.clear();
 
286
        return P;
 
287
}
 
288
 
 
289
//**********************************************************************************
 
290
//list of facets + edges
 
291
void PrintPolyhedron(Polyhedron P){
 
292
        Vector3r A,B,C;
 
293
        cout << "*** faces ***" << endl;
 
294
        for (Polyhedron::Facet_iterator fIter = P.facets_begin(); fIter!=P.facets_end(); ++fIter){
 
295
                Polyhedron::Halfedge_around_facet_circulator hfc0;
 
296
                hfc0 = fIter->facet_begin();
 
297
                int n = fIter->facet_degree();  
 
298
                A = FromCGALPoint(hfc0->vertex()->point());
 
299
                C = FromCGALPoint(hfc0->next()->vertex()->point());
 
300
                for (int i=2; i<n; i++){
 
301
                        ++hfc0;
 
302
                        B = C;
 
303
                        C = FromCGALPoint(hfc0->next()->vertex()->point());
 
304
                        cout << A << " "<< B << " "<< C << endl; 
 
305
                }
 
306
        }
 
307
        cout << "*** edges ***" << endl;
 
308
        for (Polyhedron::Edge_iterator hei = P.edges_begin(); hei!=P.edges_end(); ++hei){
 
309
                cout << hei->vertex()->point() << " " <<  hei->opposite()->vertex()->point() << endl;
 
310
        }
 
311
}
 
312
 
 
313
//**********************************************************************************
 
314
//list of facets
 
315
void PrintPolyhedronFacets(Polyhedron P){
 
316
        Vector3r A,B,C;
 
317
        for (Polyhedron::Facet_iterator fIter = P.facets_begin(); fIter!=P.facets_end(); ++fIter){
 
318
                cout << "***" << endl;
 
319
                Polyhedron::Halfedge_around_facet_circulator hfc0;
 
320
                hfc0 = fIter->facet_begin();
 
321
                int n = fIter->facet_degree();  
 
322
                for (int i = 0; i<n; ++hfc0, i++){
 
323
                        cout << hfc0->vertex()->point() << endl; 
 
324
                }
 
325
        }
 
326
}
 
327
 
 
328
//**********************************************************************************
 
329
//calculate intersection of three planes (exceptional cases not checked) - not in use, can be deleted
 
330
CGALpoint PlaneIntersection( Plane a,  Plane b,  Plane c){
 
331
        Matrix3r A;
 
332
        A<<
 
333
                a.a(),a.b(),a.c(),
 
334
                b.a(),b.b(),b.c(),
 
335
                c.a(),c.b(),c.c();
 
336
        Vector3r x = A.inverse()*Vector3r(-a.d(),-b.d(),-c.d());
 
337
        return CGALpoint(x.x(),x.y(),x.z());
 
338
}
 
339
 
 
340
//**********************************************************************************
 
341
//solve system of 3x3 by Cramers rule
 
342
Vector3r SolveLinSys3x3(Matrix3r A, Vector3r y){
 
343
        //only system 3x3 by Cramers rule 
 
344
        double det =    A(0,0)*A(1,1)*A(2,2)+A(0,1)*A(1,2)*A(2,0)+A(0,2)*A(1,0)*A(2,1)-A(0,2)*A(1,1)*A(2,0)-A(0,1)*A(1,0)*A(2,2)-A(0,0)*A(1,2)*A(2,1);  
 
345
        if (det == 0) {cerr << "error in linear solver" << endl; return Vector3r(0,0,0);}
 
346
        return Vector3r((y(0)*A(1,1)*A(2,2)+A(0,1)*A(1,2)*y(2)+A(0,2)*y(1)*A(2,1)-A(0,2)*A(1,1)*y(2)-A(0,1)*y(1)*A(2,2)-y(0)*A(1,2)*A(2,1))/det, (A(0,0)*y(1)*A(2,2)+y(0)*A(1,2)*A(2,0)+A(0,2)*A(1,0)*y(2)-A(0,2)*y(1)*A(2,0)-y(0)*A(1,0)*A(2,2)-A(0,0)*A(1,2)*y(2))/det, (A(0,0)*A(1,1)*y(2)+A(0,1)*y(1)*A(2,0)+y(0)*A(1,0)*A(2,1)-y(0)*A(1,1)*A(2,0)-A(0,1)*A(1,0)*y(2)-A(0,0)*y(1)*A(2,1))/det);
 
347
}
 
348
 
 
349
//**********************************************************************************
 
350
/*return convex hull of points 
 
351
critical point, because CGAL often returnes segmentation fault. The planes must be sufficiently "different". This is, however, checked elswhere by DISTANCE_LIMIT variable.
 
352
*/
 
353
Polyhedron ConvexHull(vector<CGALpoint> &planes){
 
354
        Polyhedron Int;
 
355
        //cout << "C"; cout.flush();
 
356
        if (planes.size()>3) CGAL::convex_hull_3(planes.begin(), planes.end(), Int);
 
357
        //cout << "-C"<< endl;  cout.flush();
 
358
        return Int;
 
359
}
 
360
 
 
361
//**********************************************************************************
 
362
//determination of normal direction of intersection
 
363
 
 
364
Vector3r FindNormal(Polyhedron Int, Polyhedron PA, Polyhedron PB){
 
365
 
 
366
        //determine which plane is from which polyhedra
 
367
        Polyhedron::Plane_iterator pi, pj;
 
368
        std::transform( Int.facets_begin(), Int.facets_end(), Int.planes_begin(),Plane_equation());
 
369
        std::transform( PA.facets_begin(), PA.facets_end(), PA.planes_begin(),Plane_equation());        
 
370
        std::transform( PB.facets_begin(), PB.facets_end(), PB.planes_begin(),Plane_equation());
 
371
        vector<bool> from_A(Int.size_of_facets());
 
372
        vector<double> minsA(Int.size_of_facets());
 
373
        vector<double> minsB(Int.size_of_facets());
 
374
        int i=0;
 
375
        double minA, minB, k;
 
376
        for(pi = Int.planes_begin(); pi!=Int.planes_end(); ++pi,i++){
 
377
                minA = 1.;
 
378
                minB = 1.;
 
379
                for(pj=PA.planes_begin(); pj!=PA.planes_end(); ++pj){
 
380
                        k = PlaneDifference(*pi, *pj);
 
381
                        if (k<minA) {
 
382
                                minA = k;
 
383
                                minsA[i] = minA;
 
384
                                if (minA<FIND_NORMAL_LIMIT) {from_A[i] = true; break;} //already satisfactory
 
385
                        }
 
386
 
 
387
                }
 
388
                if (minA<FIND_NORMAL_LIMIT) continue;
 
389
                for(pj=PB.planes_begin(); pj!=PB.planes_end(); ++pj){
 
390
                        k = PlaneDifference(*pi, *pj);
 
391
                        
 
392
                        if (k<minB){
 
393
                                minB = k;
 
394
                                minsB[i] = minB;
 
395
                                if (minB<FIND_NORMAL_LIMIT || minB<minA)  break; //already satisfactory
 
396
                        }
 
397
                }
 
398
                from_A[i] = ((minA < minB) ? true : false);             
 
399
        }
 
400
        //check that not all belongs to A and not all belongs to B      
 
401
        if (*std::min_element(from_A.begin(),from_A.end())==1){
 
402
                int loc = std::min_element(minsB.begin(),minsB.end()) - minsB.begin();          
 
403
                from_A[loc] = false;
 
404
        }else if (*std::max_element(from_A.begin(),from_A.end())==0){
 
405
                int loc = std::min_element(minsA.begin(),minsA.end()) - minsA.begin();
 
406
                from_A[loc] = true;
 
407
        }
 
408
 
 
409
        //find intersecting segments
 
410
        vector<Segment> segments;       
 
411
        int a,b;
 
412
 
 
413
        for (Polyhedron::Edge_iterator hei = Int.edges_begin(); hei!=Int.edges_end(); ++hei){
 
414
                a = std::distance(Int.facets_begin(), hei->facet());
 
415
                b = std::distance(Int.facets_begin(), hei->opposite()->facet());
 
416
                if ((from_A[a] && !from_A[b]) || (from_A[b] && !from_A[a])) segments.push_back(Segment(hei->vertex()->point(),hei->opposite()->vertex()->point()));
 
417
        }
 
418
 
 
419
        //find normal direction
 
420
        Plane fit;
 
421
        linear_least_squares_fitting_3(segments.begin(),segments.end(),fit,CGAL::Dimension_tag<1>());
 
422
        CGALvector CGALnormal=fit.orthogonal_vector();
 
423
        CGALnormal = CGALnormal/sqrt(CGALnormal.squared_length()); 
 
424
        // reverse direction if projection of the (contact_point-centroid_of_B) vector onto the normal is negative (i.e. the normal points more towards B)
 
425
 
 
426
        return FromCGALVector(CGALnormal);
 
427
}
 
428
 
 
429
//**********************************************************************************
 
430
//not used
 
431
double CalculateProjectionArea(Polyhedron Int, CGALvector CGALnormal){
 
432
        //calculate area of projection of Intersection into the normal plane
 
433
        double area = 0.;
 
434
        double abs_size;
 
435
        Polyhedron::Halfedge_around_facet_circulator hfc0;
 
436
        CGALvector normal2;
 
437
        double norm2;   
 
438
        std::transform( Int.facets_begin(), Int.facets_end(), Int.planes_begin(),Plane_equation());
 
439
        for(Polyhedron::Facet_iterator fi = Int.facets_begin(); fi!= Int.facets_end(); ++fi){
 
440
                assert(fi->is_triangle()); 
 
441
                hfc0 = fi->facet_begin();
 
442
                normal2 = fi->plane().orthogonal_vector();
 
443
                norm2 = normal2.squared_length();
 
444
                if(norm2 < 1E-20) continue;
 
445
                abs_size = 0.5*sqrt((cross_product(CGALvector(hfc0->vertex()->point(),hfc0->next()->vertex()->point()),CGALvector(hfc0->vertex()->point(),hfc0->next()->next()->vertex()->point()))).squared_length());
 
446
                // factor 0.5 because this procedure returnes doubles projected area
 
447
                if (abs_size>0) area += 0.5*abs_size*abs(CGALnormal*normal2/sqrt(norm2));
 
448
        }
 
449
        return area;
 
450
}
 
451
 
 
452
//**********************************************************************************
 
453
//prepare data for CGAL convex hull
 
454
vector<Plane> MergePlanes(vector<Plane> planes1, vector<Plane> planes2, double limit){
 
455
        vector<Plane> P = planes1;
 
456
        bool add;
 
457
        for(vector<Plane>::iterator i = planes2.begin(); i!=planes2.end(); ++i){
 
458
                add = true;
 
459
                for(vector<Plane>::iterator j = planes1.begin(); j!=planes1.end(); ++j){
 
460
                        if (PlaneDifference(*i,*j) < limit){    
 
461
                                add = false;
 
462
                                break;
 
463
                        }
 
464
                }
 
465
                if (add) P.push_back(*i);
 
466
        }       
 
467
        return P;
 
468
}
 
469
 
 
470
//**********************************************************************************
 
471
//returnes intersecting polyhedron of polyhedron & plane (possibly empty)
 
472
Polyhedron Polyhedron_Plane_intersection(Polyhedron A, Plane B, CGALpoint centroid, CGALpoint X){
 
473
        
 
474
        Polyhedron Intersection;
 
475
        CGALpoint inside;
 
476
        vector<Plane> planes1, planes2;
 
477
        vector<CGALpoint> dual_planes;
 
478
        // test if do intersect, find some intersecting point
 
479
        bool intersection_found = false;
 
480
        double lim = pow(DISTANCE_LIMIT,2);
 
481
        // test centroid of previous intersection
 
482
        if(Is_inside_Polyhedron(A,X,DISTANCE_LIMIT) && Oriented_squared_distance(B,X)<=-lim) {
 
483
                intersection_found = true;
 
484
                inside = X;
 
485
        // find new point by checking polyhedron vertices that lies on negative side of the plane
 
486
        } else {        
 
487
                for(Polyhedron::Vertex_iterator vIter = A.vertices_begin(); vIter != A.vertices_end() && !intersection_found ; vIter++){
 
488
                        if(Oriented_squared_distance(B,vIter->point())<=-lim) {
 
489
                                intersection_found = true;                      
 
490
                                CGAL::Object result = CGAL::intersection(Line(vIter->point(),centroid), B);                     
 
491
                                if (const CGALpoint *ipoint = CGAL::object_cast<CGALpoint>(&result)) {
 
492
                                        inside = vIter->point() + 0.5*CGALvector(vIter->point(),*ipoint);
 
493
                                }else{
 
494
                                        cerr << "Error in line-plane intersection" << endl; 
 
495
                                }
 
496
                        }
 
497
                }
 
498
        }
 
499
        //no intersectiong point => no intersection polyhedron
 
500
        if(!intersection_found) return Intersection;
 
501
 
 
502
        
 
503
        //set the intersection point to origin  
 
504
        Transformation transl_back(CGAL::TRANSLATION, inside - CGALpoint(0.,0.,0.));
 
505
        Transformation transl(CGAL::TRANSLATION, CGALpoint(0.,0.,0.)-inside);
 
506
 
 
507
        std::transform( A.points_begin(), A.points_end(), A.points_begin(), transl);
 
508
        B = transl.transform(B);
 
509
 
 
510
        //dualize plane
 
511
        planes1.push_back(B);
 
512
 
 
513
        //dualize polyhedron
 
514
        std::transform( A.facets_begin(), A.facets_end(), A.planes_begin(),Plane_equation());
 
515
        for(Polyhedron::Plane_iterator pi =A.planes_begin(); pi!=A.planes_end(); ++pi) planes2.push_back(*pi);;
 
516
 
 
517
        //merge planes
 
518
        planes1 = MergePlanes(planes1,planes2, MERGE_PLANES_LIMIT);//MERGE_PLANES_LIMIT);
 
519
        for(vector<Plane>::iterator pi =planes1.begin(); pi!= planes1.end(); ++pi) dual_planes.push_back(CGALpoint(-pi->a()/pi->d(),-pi->b()/pi->d(),-pi->c()/pi->d()));
 
520
 
 
521
        //compute convex hull of it
 
522
        Intersection = ConvexHull(dual_planes);
 
523
        if (Intersection.empty()) {cout << "0" << endl; cout.flush(); return Intersection;}
 
524
 
 
525
        //simplify
 
526
        std::transform( Intersection.facets_begin(), Intersection.facets_end(), Intersection.planes_begin(),Plane_equation());
 
527
        Intersection = Simplify(Intersection, SIMPLIFY_LIMIT);
 
528
        std::transform( Intersection.facets_begin(), Intersection.facets_end(), Intersection.planes_begin(),Plane_equation());
 
529
 
 
530
        //dualize again
 
531
        dual_planes.clear();
 
532
        for(Polyhedron::Plane_iterator pi =Intersection.planes_begin(); pi!=Intersection.planes_end(); ++pi) dual_planes.push_back(CGALpoint(-pi->a()/pi->d(),-pi->b()/pi->d(),-pi->c()/pi->d()));
 
533
 
 
534
        //compute convex hull of it
 
535
        Intersection = ConvexHull(dual_planes);
 
536
 
 
537
        //return to original position
 
538
        std::transform( Intersection.points_begin(), Intersection.points_end(), Intersection.points_begin(), transl_back);
 
539
        
 
540
        if (Intersection.size_of_facets() < 4) Intersection.clear();
 
541
        return Intersection;
 
542
}
 
543
 
 
544
//**********************************************************************************
 
545
//returnes intersecting polyhedron of polyhedron & plane defined by diriction and point
 
546
Polyhedron Polyhedron_Plane_intersection(Polyhedron A, Vector3r direction,  Vector3r plane_point){
 
547
        Plane B(ToCGALPoint(plane_point), ToCGALVector(direction)); 
 
548
        CGALpoint X = ToCGALPoint(plane_point) - 1E-8*ToCGALVector(direction);
 
549
        return Polyhedron_Plane_intersection(A, B, ToCGALPoint(plane_point), X);
 
550
}
 
551
 
 
552
//**********************************************************************************
 
553
//returnes intersecting polyhedron of two polyhedrons (possibly empty)
 
554
Polyhedron Polyhedron_Polyhedron_intersection(Polyhedron A, Polyhedron B, CGALpoint X, CGALpoint centroidA,  CGALpoint centroidB, std::vector<int> &sep_plane){
 
555
 
 
556
        Polyhedron Intersection;
 
557
 
 
558
        vector<Plane> planes1, planes2;
 
559
        vector<CGALpoint> dual_planes;
 
560
        Polyhedron::Plane_iterator pi;  
 
561
        CGALpoint inside(0,0,0);
 
562
        
 
563
        bool intersection_found = false;
 
564
        std::transform( A.facets_begin(), A.facets_end(), A.planes_begin(),Plane_equation());
 
565
        std::transform( B.facets_begin(), B.facets_end(), B.planes_begin(),Plane_equation());
 
566
        Matrix3r Amatrix;
 
567
        Vector3r y;
 
568
        // test that X is really inside 
 
569
        if(Is_inside_Polyhedron(A,X,DISTANCE_LIMIT) && Is_inside_Polyhedron(B,X,DISTANCE_LIMIT)) {
 
570
                intersection_found = true;
 
571
                inside = X;
 
572
        } else {
 
573
                if (!do_intersect(A, B, sep_plane)) return Intersection; 
 
574
                //some intersection point
 
575
                double dist_S, dist_T;
 
576
                double lim2 = pow(DISTANCE_LIMIT,2);
 
577
                CGALvector d1;
 
578
                double factor = sqrt(DISTANCE_LIMIT * 1.5);
 
579
                //test vertices A - not needed, edges are enough
 
580
                for (Polyhedron::Vertex_iterator vIter = A.vertices_begin(); vIter != A.vertices_end() && !intersection_found; vIter++){
 
581
                        d1 = centroidA-vIter->point();
 
582
                        inside = vIter->point() + d1/sqrt(d1.squared_length())*DISTANCE_LIMIT*20.;
 
583
                        intersection_found = (Is_inside_Polyhedron(A,inside,DISTANCE_LIMIT) && Is_inside_Polyhedron(B,inside,DISTANCE_LIMIT));          
 
584
                }
 
585
                //test vertices B - necessary
 
586
                for (Polyhedron::Vertex_iterator vIter = B.vertices_begin(); vIter != B.vertices_end() && !intersection_found; vIter++){
 
587
                        d1 = centroidB-vIter->point();
 
588
                        inside = vIter->point() + d1/sqrt(d1.squared_length())*DISTANCE_LIMIT*20.;
 
589
                        intersection_found = (Is_inside_Polyhedron(A,inside,DISTANCE_LIMIT) && Is_inside_Polyhedron(B,inside,DISTANCE_LIMIT));
 
590
                }       
 
591
 
 
592
                //test edges
 
593
                for (Polyhedron::Edge_iterator eIter = A.edges_begin(); eIter != A.edges_end() && !intersection_found; eIter++){
 
594
                        for (Polyhedron::Facet_iterator fIter = B.facets_begin(); fIter != B.facets_end() && !intersection_found; fIter++){
 
595
                                dist_S = Oriented_squared_distance(fIter->plane(), eIter->vertex()->point());
 
596
                                dist_T = Oriented_squared_distance(fIter->plane(), eIter->opposite()->vertex()->point());
 
597
                                if (dist_S*dist_T >= 0 || abs(dist_S)<lim2 || abs(dist_T)<lim2) continue;
 
598
                                inside = eIter->vertex()->point() + (eIter->opposite()->vertex()->point()-eIter->vertex()->point())*sqrt(abs(dist_S))/(sqrt(abs(dist_S))+sqrt(abs(dist_T)));
 
599
                                // the fact that edge intersects the facet (not only its plane) is not explicitely checked, it sufices to check that the resulting point is inside both polyhedras
 
600
                                Plane p1 = fIter->plane();
 
601
                                Plane p2 = eIter->facet()->plane();
 
602
                                Plane p3 = eIter->opposite()->facet()->plane();
 
603
                                Amatrix << p1.a(),p1.b(),p1.c(),
 
604
                                     p2.a(),p2.b(),p2.c(),
 
605
                                     p3.a(),p3.b(),p3.c();
 
606
                                y = Vector3r(-p1.d()-factor*sqrt(pow(p1.a(),2)+pow(p1.b(),2)+pow(p1.c(),2)),-p2.d()-factor*sqrt(pow(p2.a(),2)+pow(p2.b(),2)+pow(p2.c(),2)),-p3.d()-factor*sqrt(pow(p3.a(),2)+pow(p3.b(),2)+pow(p3.c(),2)));
 
607
                                inside = ToCGALPoint(SolveLinSys3x3(Amatrix,y));
 
608
                                intersection_found = (Is_inside_Polyhedron(A,inside,DISTANCE_LIMIT) && Is_inside_Polyhedron (B,inside,DISTANCE_LIMIT));
 
609
                        }
 
610
                }        
 
611
        }
 
612
        
 
613
        //Polyhedrons do not intersect
 
614
        if (!intersection_found)  return Intersection;
 
615
 
 
616
        //set the intersection point to origin  
 
617
        Transformation transl_back(CGAL::TRANSLATION, inside - CGALpoint(0.,0.,0.));
 
618
        Transformation transl(CGAL::TRANSLATION, CGALpoint(0.,0.,0.)-inside);
 
619
 
 
620
        std::transform( A.points_begin(), A.points_end(), A.points_begin(), transl);
 
621
        std::transform( B.points_begin(), B.points_end(), B.points_begin(), transl);
 
622
 
 
623
        //dualize polyhedrons
 
624
        std::transform( A.facets_begin(), A.facets_end(), A.planes_begin(),Plane_equation());
 
625
        std::transform( B.facets_begin(), B.facets_end(), B.planes_begin(),Plane_equation());
 
626
        for(Polyhedron::Plane_iterator pi =A.planes_begin(); pi!=A.planes_end(); ++pi) planes1.push_back(*pi);
 
627
        for(Polyhedron::Plane_iterator pi =B.planes_begin(); pi!=B.planes_end(); ++pi) planes2.push_back(*pi);
 
628
 
 
629
 
 
630
        //merge planes
 
631
        planes1 = MergePlanes(planes1,planes2, MERGE_PLANES_LIMIT);//MERGE_PLANES_LIMIT);
 
632
        for(vector<Plane>::iterator pi =planes1.begin(); pi!= planes1.end(); ++pi) dual_planes.push_back(CGALpoint(-pi->a()/pi->d(),-pi->b()/pi->d(),-pi->c()/pi->d()));
 
633
 
 
634
        //compute convex hull of it
 
635
        Intersection = ConvexHull(dual_planes);
 
636
        if (Intersection.empty()) {cout << "0" << endl; cout.flush(); return Intersection;};
 
637
 
 
638
        //simplify
 
639
        std::transform( Intersection.facets_begin(), Intersection.facets_end(), Intersection.planes_begin(),Plane_equation());
 
640
        Intersection = Simplify(Intersection, SIMPLIFY_LIMIT);
 
641
        std::transform( Intersection.facets_begin(), Intersection.facets_end(), Intersection.planes_begin(),Plane_equation());
 
642
        
 
643
        //dualize again
 
644
        dual_planes.clear();
 
645
        for(Polyhedron::Plane_iterator pi =Intersection.planes_begin(); pi!=Intersection.planes_end(); ++pi) dual_planes.push_back(CGALpoint(-pi->a()/pi->d(),-pi->b()/pi->d(),-pi->c()/pi->d()));
 
646
 
 
647
        //compute convex hull of it
 
648
        Intersection = ConvexHull(dual_planes);
 
649
        //return to original position
 
650
        std::transform( Intersection.points_begin(), Intersection.points_end(), Intersection.points_begin(), transl_back);
 
651
        
 
652
        if (Intersection.size_of_facets() < 4) Intersection.clear();
 
653
        return Intersection;
 
654
}
 
655
 
 
656
//**********************************************************************************
 
657
/*
 
658
//returnes intersection of Sphere and polyhedron (possibly empty) - not finished
 
659
bool Sphere_Polyhedron_intersection(Polyhedron A, double r, CGALpoint C, CGALpoint centroid,  double volume, CGALvector normal, double area){
 
660
        volume = 0;
 
661
        area = 0;
 
662
        
 
663
        double r2 = pow(r,2);
 
664
        //check points
 
665
        Polyhedron::Vertex_iterator closest_vert;
 
666
        double dist2;
 
667
        double min_dist2 = (C-centroid).squared_length();
 
668
        //test vertices
 
669
        for(Polyhedron::Vertex_iterator vi=A.vertices_begin(); vi!=A.vertices_end(); ++vi){
 
670
                dist2 = (C-vi->point()).squared_length();
 
671
                if(min_dist2 > dist2) {min_dist2 = dist2; closest_vert=vi;}             
 
672
        } 
 
673
        if(min_dist2 < r2){ //test if we are already inside the sphere
 
674
                
 
675
                return true;
 
676
        }       
 
677
                
 
678
        //test edges
 
679
        CGALvector v1(C-closest_vert->point()); 
 
680
        v1 = v1/sqrt(v1.squared_length());
 
681
        Polyhedron::Halfedge_around_vertex_circulator closest_edge;
 
682
        int i =0;
 
683
        double cos_angle;
 
684
        double max_cos_angle=-1;
 
685
        CGALvector v2;
 
686
        for(Polyhedron::Halfedge_around_vertex_circulator hfi=closest_vert->vertex_begin(); i<(int)closest_vert->degree(); ++hfi, i++){
 
687
                v2 = hfi->opposite()->vertex()->point()-closest_vert->point();
 
688
                v2 = v2/sqrt(v2.squared_length());
 
689
                cos_angle = v1*v2; 
 
690
                if(cos_angle > max_cos_angle) closest_edge=hfi;  max_cos_angle=cos_angle; //find the edge with minimum angle
 
691
        } 
 
692
        if(max_cos_angle<=0) return false; //all edges goes with angle above or equal 90
 
693
        double k = (C - closest_vert->point())*v2;  //find closest point on the most descending edge (it must be on edge, otherwise opposite point would be already closer)
 
694
        CGALpoint closest_point = closest_vert->point() + k*v2;
 
695
        if((C-closest_point).squared_length()<r2){ //test if we are already inside the sphere
 
696
        
 
697
                return true;    
 
698
        }       
 
699
 
 
700
 
 
701
        //check planes
 
702
        v1 = (C-closest_point); 
 
703
        v1 = v1/sqrt(v1.squared_length());
 
704
        v2 = closest_edge->vertex()->point()-closest_edge->opposite()->vertex()->point();
 
705
        CGALvector v3 = closest_edge->facet()->plane().orthogonal_vector(); 
 
706
        CGALvector v4 = closest_edge->opposite()->facet()->plane().orthogonal_vector(); 
 
707
        v3 = CGAL::cross_product(v3,v2);
 
708
        v4 = CGAL::cross_product(v4,v2*(-1));
 
709
        Plane closest_plane;
 
710
        if (v1*v3>=0 && v1*v4>=0) return false; // both planes running away
 
711
        if (v1*v3<0) closest_plane = closest_edge->facet()->plane();
 
712
        else closest_plane = closest_edge->opposite()->facet()->plane();
 
713
        closest_point = closest_plane.projection(C);
 
714
        if((C-closest_point).squared_length()<r2){ //test if we are already inside the sphere
 
715
                double h = sqrt((C-closest_point).squared_length());
 
716
                volume = 3.1415*pow(h,2)/3*(3*r-h);
 
717
                return true;    
 
718
        }
 
719
 
 
720
        return false;   
 
721
}
 
722
*/
 
723
 
 
724
//**********************************************************************************
 
725
Vector3r FromCGALPoint(CGALpoint A){
 
726
        return Vector3r(A.x(), A.y(), A.z());
 
727
}
 
728
 
 
729
//**********************************************************************************
 
730
Vector3r FromCGALVector(CGALvector A){
 
731
        return Vector3r(A.x(), A.y(), A.z());
 
732
}
 
733
 
 
734
//**********************************************************************************
 
735
CGALpoint ToCGALPoint(Vector3r A){
 
736
        return CGALpoint(A[0], A[1], A[2]);
 
737
}
 
738
 
 
739
//**********************************************************************************
 
740
CGALvector ToCGALVector(Vector3r A){
 
741
        return CGALvector(A[0], A[1], A[2]);
 
742
}
 
743
 
 
744
//**********************************************************************************
 
745
//new polyhedra
 
746
shared_ptr<Body> NewPolyhedra(vector<Vector3r> v, shared_ptr<Material> mat){
 
747
        shared_ptr<Body> body(new Body);
 
748
        body->material=mat;
 
749
        body->shape=shared_ptr<Polyhedra>(new Polyhedra());
 
750
        Polyhedra* A = static_cast<Polyhedra*>(body->shape.get());      
 
751
        A->v = v;
 
752
        A->Initialize();
 
753
        body->state->pos= A->GetCentroid();
 
754
        body->state->mass=body->material->density*A->GetVolume();
 
755
        body->state->inertia =A->GetInertia()*body->material->density;
 
756
        body->state->ori = A->GetOri();
 
757
        body->bound=shared_ptr<Aabb>(new Aabb);
 
758
        body->setAspherical(true);
 
759
        return body;
 
760
}
 
761
 
 
762
 
 
763
//**********************************************************************************
 
764
//split polyhedra
 
765
void SplitPolyhedra(const shared_ptr<Body>& body, Vector3r direction){
 
766
 
 
767
        const Se3r& se3=body->state->se3; 
 
768
        Polyhedra* A = static_cast<Polyhedra*>(body->shape.get());
 
769
        State* X = static_cast<State*>(body->state.get());
 
770
 
 
771
        Vector3r OrigPos = X->pos;
 
772
        Vector3r OrigVel = X->vel;
 
773
        Vector3r OrigAngVel = X->angVel;
 
774
 
 
775
        //move and rotate CGAL structure Polyhedron
 
776
        Matrix3r rot_mat = (se3.orientation).toRotationMatrix();
 
777
        Vector3r trans_vec = se3.position;
 
778
        Transformation t_rot_trans(rot_mat(0,0),rot_mat(0,1),rot_mat(0,2), trans_vec[0],rot_mat(1,0),rot_mat(1,1),rot_mat(1,2),trans_vec[1],rot_mat(2,0),rot_mat(2,1),rot_mat(2,2),trans_vec[2],1.);
 
779
        Polyhedron PA = A->GetPolyhedron();
 
780
        std::transform( PA.points_begin(), PA.points_end(), PA.points_begin(), t_rot_trans);
 
781
 
 
782
        //calculate splitted polyhedrons
 
783
        Polyhedron S1 = Polyhedron_Plane_intersection(PA, direction,  trans_vec);
 
784
        Polyhedron S2 = Polyhedron_Plane_intersection(PA, (-1)*direction,  trans_vec);
 
785
        
 
786
        
 
787
        //replace original polyhedron
 
788
        A->Clear();             
 
789
        for(Polyhedron::Vertex_iterator vi = S1.vertices_begin(); vi !=  S1.vertices_end(); vi++){ A->v.push_back(FromCGALPoint(vi->point()));}; 
 
790
        A->Initialize();
 
791
        X->pos = A->GetCentroid();
 
792
        X->ori = A->GetOri();
 
793
        X->mass=body->material->density*A->GetVolume();
 
794
        X->refPos[0] = X->refPos[0]+1.;
 
795
        X->inertia =A->GetInertia()*body->material->density;
 
796
        
 
797
        //second polyhedron
 
798
        vector<Vector3r> v2;
 
799
        for(Polyhedron::Vertex_iterator vi = S2.vertices_begin(); vi !=  S2.vertices_end(); vi++){ v2.push_back(FromCGALPoint(vi->point()));}; 
 
800
        shared_ptr<Body> BP = NewPolyhedra(v2, body->material);
 
801
        //BP->shape->color = body->shape->color;
 
802
        BP->shape->color = Vector3r(double(rand())/RAND_MAX,double(rand())/RAND_MAX,double(rand())/RAND_MAX);
 
803
 
 
804
        //append to the rest
 
805
        Scene* scene=Omega::instance().getScene().get();
 
806
        //scene->bodies->erase(body->id);
 
807
        scene->bodies->insert(BP);
 
808
 
 
809
        //set proper state variables
 
810
        X->vel = OrigVel + OrigAngVel.cross(X->pos-OrigPos);
 
811
        BP->state->vel = OrigVel + OrigAngVel.cross(BP->state->pos-OrigPos);
 
812
        X->angVel = OrigAngVel; 
 
813
        BP->state->angVel = OrigAngVel;
 
814
 
 
815
}
 
816
 
 
817
#endif // YADE_CGAL