In the middle of switching to own collision detection, WIP commit (don't expect anyth...
[blender.git] / source / blender / blenkernel / intern / collision.c
1 /*  collision.c      
2
3 *
4 * ***** BEGIN GPL/BL DUAL LICENSE BLOCK *****
5 *
6 * This program is free software; you can redistribute it and/or
7 * modify it under the terms of the GNU General Public License
8 * as published by the Free Software Foundation; either version 2
9 * of the License, or (at your option) any later version. The Blender
10 * Foundation also sells licenses for use in proprietary software under
11 * the Blender License.  See http://www.blender.org/BL/ for information
12 * about this.
13 *
14 * This program is distributed in the hope that it will be useful,
15 * but WITHOUT ANY WARRANTY; without even the implied warranty of
16 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
17 * GNU General Public License for more details.
18 *
19 * You should have received a copy of the GNU General Public License
20 * along with this program; if not, write to the Free Software Foundation,
21 * Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
22 *
23 * The Original Code is Copyright (C) Blender Foundation
24 * All rights reserved.
25 *
26 * The Original Code is: all of this file.
27 *
28 * Contributor(s): none yet.
29 *
30 * ***** END GPL/BL DUAL LICENSE BLOCK *****
31 */
32
33 #include <math.h>
34 #include <stdlib.h>
35 #include <string.h>
36 #include "MEM_guardedalloc.h"
37 /* types */
38 #include "DNA_curve_types.h"
39 #include "DNA_object_types.h"
40 #include "DNA_object_force.h"
41 #include "DNA_cloth_types.h"    
42 #include "DNA_key_types.h"
43 #include "DNA_mesh_types.h"
44 #include "DNA_meshdata_types.h"
45 #include "DNA_lattice_types.h"
46 #include "DNA_scene_types.h"
47 #include "DNA_modifier_types.h"
48 #include "BLI_blenlib.h"
49 #include "BLI_arithb.h"
50 #include "BLI_edgehash.h"
51 #include "BLI_linklist.h"
52 #include "BKE_curve.h"
53 #include "BKE_deform.h"
54 #include "BKE_DerivedMesh.h"
55 #include "BKE_cdderivedmesh.h"
56 #include "BKE_displist.h"
57 #include "BKE_effect.h"
58 #include "BKE_global.h"
59 #include "BKE_mesh.h"
60 #include "BKE_object.h"
61 #include "BKE_cloth.h"
62 #include "BKE_modifier.h"
63 #include "BKE_utildefines.h"
64 #include "BKE_DerivedMesh.h"
65 #include "DNA_screen_types.h"
66 #include "BSE_headerbuttons.h"
67 #include "BIF_screen.h"
68 #include "BIF_space.h"
69 #include "mydevice.h"
70
71 #include "Bullet-C-Api.h"
72
73
74 #define DERANDOMIZE 1
75
76
77 enum TRIANGLE_MARK 
78
79         TM_MV = 1,
80         TM_ME = 2,
81         TM_V1 = 4,
82         TM_V2 = 8,
83         TM_V3 = 16,
84         TM_E1 = 32,
85         TM_E2 = 64,
86         TM_E3 = 128 
87 };
88
89 DO_INLINE int hasTriangleMark(unsigned char mark, unsigned char bit) { return mark & bit; }
90 DO_INLINE void setTriangleMark(unsigned char *mark, unsigned char bit) { mark[0] |= bit; }
91 DO_INLINE void clearTriangleMark(unsigned char *mark, unsigned char bit) { mark[0] &= ~bit; }
92
93
94 void generateTriangleMarks() 
95 {
96         /*
97         unsigned int firstEdge = 0;
98         
99         // 1. Initialization
100         memset(m_triangleMarks, 0, sizeof(unsigned char) * m_triangleCount);
101
102         // 2. The Marking Process
103         
104         // 2.1 Randomly mark triangles for covering vertices.
105         for (unsigned int v = 0; v < m_vertexCount; ++v) 
106         {
107                 if (vertexCover(v) == 0) 
108                 {
109
110                         // Randomly select an edge whose first triangle we're going to flag. 
111
112 #ifndef DERANDOMIZE
113                         firstEdge = (unsigned int)((float)(random() & 0x7FFFFFFF) /
114                                         (float)(0x80000000) *
115                                         (float)(m_vertices[v].getEdgeCount()));
116 #endif
117                         for (unsigned int ofs = 0; ofs < m_vertices[v].getEdgeCount(); ++ofs) 
118                         {
119                                 unsigned int edgeIdx = (firstEdge + ofs) % m_vertices[v].getEdgeCount();
120                                 if (m_edges[m_vertices[v].getEdge(edgeIdx)].getTriangleCount())
121                                         setTriangleMark(m_triangleMarks[m_edges[m_vertices[v].getEdge(edgeIdx)].getTriangle(0)], TM_MV);
122                         }
123                 }
124         }
125         */
126         /* If the Cloth is malformed (vertices without adjacent triangles) there might still be uncovered vertices. (Bad luck.) */
127         /*
128         // 2.2 Randomly mark triangles for covering edges.
129         for (unsigned int e = 0; e < m_edgeCount; ++e) 
130         {
131                 if (m_edges[e].getTriangleCount() && (edgeCover(e) == 0)) 
132                 {
133 #ifndef DERANDOMIZE
134                         setTriangleMark(m_triangleMarks[m_edges[e].getTriangle(static_cast<UINT32>((float)(random() & 0x7FFFFFFF) /
135                                         (float)(0x80000000) *
136                                         (float)(m_edges[e].getTriangleCount())))], TM_ME);
137 #else
138                         setTriangleMark(m_triangleMarks[m_edges[e].getTriangle(0)], TM_ME);
139 #endif
140                 }
141         }
142
143         
144         // 3. The Unmarking Process
145         for (unsigned int t = 0; (t < m_triangleCount); ++t) 
146         {
147                 bool overCoveredVertices = true;
148                 bool overCoveredEdges = true;
149                 for (unsigned char i = 0; (i < 3) && (overCoveredVertices || overCoveredEdges); ++i) 
150                 {
151
152                         if (vertexCover(m_triangles[t].getVertex(i)) == 1)
153                                 overCoveredVertices = false;
154                         if (edgeCover(m_triangles[t].getEdge(i)) == 1)
155                                 overCoveredEdges = false;
156
157                         assert(vertexCover(m_triangles[t].getVertex(i)) > 0);
158                         assert(edgeCover(m_triangles[t].getEdge(i)) > 0);
159                 }
160                 if (overCoveredVertices)
161                         clearTriangleMark(m_triangleMarks[t], TM_MV);
162                 if (overCoveredEdges)
163                         clearTriangleMark(m_triangleMarks[t], TM_ME);
164         }
165
166
167         // 4. The Bit Masking Process
168         vector<bool> vertexAssigned(m_vertexCount, false);
169         vector<bool> edgeAssigned(m_edgeCount, false);
170         for (unsigned int t = 0; (t < m_triangleCount); ++t) 
171         {
172                 for (unsigned char i = 0; i < 3; ++i) 
173                 {
174                         if (!vertexAssigned[m_triangles[t].getVertex(i)]) 
175                         {
176                                 vertexAssigned[m_triangles[t].getVertex(i)] = true;
177                                 setTriangleMark(m_triangleMarks[t], 1 << (2 + i));
178                         }
179                         if (!edgeAssigned[m_triangles[t].getEdge(i)]) 
180                         {
181                                 edgeAssigned[m_triangles[t].getEdge(i)] = true;
182                                 setTriangleMark(m_triangleMarks[t], 1 << (5 + i));
183                         }
184                 }
185         }
186         */
187 }
188
189 // w3 is not perfect
190 void bvh_compute_barycentric (float pv[3], float p1[3], float p2[3], float p3[3], double *w1, double *w2, double *w3)
191 {
192         double  tempV1[3], tempV2[3], tempV4[3];
193         double  a,b,c,d,e,f;
194
195         VECSUB (tempV1, p1, p3);        
196         VECSUB (tempV2, p2, p3);        
197         VECSUB (tempV4, pv, p3);        
198         
199         a = INPR (tempV1, tempV1);      
200         b = INPR (tempV1, tempV2);      
201         c = INPR (tempV2, tempV2);      
202         e = INPR (tempV1, tempV4);      
203         f = INPR (tempV2, tempV4);      
204         
205         d = (a * c - b * b);
206         
207         if (ABS(d) < ALMOST_ZERO) {
208                 *w1 = *w2 = *w3 = 1.0f / 3.0f;
209                 return;
210         }
211         
212         w1[0] = (e * c - b * f) / d;
213         
214         w2[0] = (f - b * w1[0]) / c;
215         
216         w3[0] = 1.0f - w1[0] - w2[0];
217 }
218
219 DO_INLINE void interpolateOnTriangle(float to[3], float v1[3], float v2[3], float v3[3], double w1, double w2, double w3) 
220 {
221         to[0] = to[1] = to[2] = 0;
222         VECADDMUL(to, v1, w1);
223         VECADDMUL(to, v2, w2);
224         VECADDMUL(to, v3, w3);
225 }
226
227
228 DO_INLINE void calculateFrictionImpulse(float to[3], float vrel[3], float normal[3], double normalVelocity,
229         double frictionConstant, double delta_V_n) 
230 {
231         float vrel_t_pre[3];
232         float vrel_t[3];
233         VECSUBS(vrel_t_pre, vrel, normal, normalVelocity);
234         VECCOPY(to, vrel_t_pre);
235         VecMulf(to, MAX2(1.0f - frictionConstant * delta_V_n / INPR(vrel_t_pre,vrel_t_pre), 0.0f));
236 }
237
238 /*
239 int collision_static(ClothModifierData *clmd, ClothModifierData *coll_clmd, LinkNode **collision_list)
240 {
241         unsigned int i = 0, numfaces = 0;
242         int result = 0;
243         LinkNode *search = NULL;
244         CollPair *collpair = NULL;
245         Cloth *cloth1, *cloth2;
246         MFace *face1, *face2;
247         double w1, w2, w3, u1, u2, u3, a1, a2, a3;
248         float v1[3], v2[3], relativeVelocity[3];
249         float magrelVel;
250         
251         cloth1 = clmd->clothObject;
252         cloth2 = coll_clmd->clothObject;
253         
254         numfaces = clmd->clothObject->numfaces;
255                 
256         for(i = 0; i < numfaces; i++)
257         {
258                 search = collision_list[i];
259                 
260                 while(search)
261                 {
262                         collpair = search->link;
263                         
264                         face1 = &(cloth1->mfaces[collpair->face1]);
265                         face2 = &(cloth2->mfaces[collpair->face2]);
266                         
267                         // compute barycentric coordinates for both collision points
268                         
269                         if(!collpair->quadA)
270                         {       
271                                 bvh_compute_barycentric(collpair->p1,
272                                                         cloth1->verts[face1->v1].txold,
273                                                         cloth1->verts[face1->v2].txold,
274                                                         cloth1->verts[face1->v3].txold, 
275                                                         &w1, &w2, &w3);
276                         }
277                         else
278                                 bvh_compute_barycentric(collpair->p1,
279                                                         cloth1->verts[face1->v4].txold,
280                                                         cloth1->verts[face1->v1].txold,
281                                                         cloth1->verts[face1->v3].txold, 
282                                                         &w1, &w2, &w3);
283                         
284                         if(!collpair->quadB)
285                                 bvh_compute_barycentric(collpair->p2,
286                                                         cloth2->verts[face2->v1].txold,
287                                                         cloth2->verts[face2->v2].txold,
288                                                         cloth2->verts[face2->v3].txold, 
289                                                         &u1, &u2, &u3);
290                         else
291                                 bvh_compute_barycentric(collpair->p2,
292                                                         cloth2->verts[face2->v4].txold,
293                                                         cloth2->verts[face2->v1].txold,
294                                                         cloth2->verts[face2->v3].txold, 
295                                                         &u1, &u2, &u3);
296                         
297                         // Calculate relative "velocity".
298                         
299                         if(!collpair->quadA)
300                                 interpolateOnTriangle(v1, cloth1->verts[face1->v1].tv, cloth1->verts[face1->v2].tv, cloth1->verts[face1->v3].tv, w1, w2, w3);
301                         else
302                                 interpolateOnTriangle(v1, cloth1->verts[face1->v4].tv, cloth1->verts[face1->v1].tv, cloth1->verts[face1->v3].tv, w1, w2, w3);
303                         
304                         if(!collpair->quadB)
305                                 interpolateOnTriangle(v2, cloth2->verts[face2->v1].tv, cloth2->verts[face2->v2].tv, cloth2->verts[face2->v3].tv, u1, u2, u3);
306                         else
307                                 interpolateOnTriangle(v2, cloth2->verts[face2->v4].tv, cloth2->verts[face2->v1].tv, cloth2->verts[face2->v3].tv, u1, u2, u3);
308                         
309                         VECSUB(relativeVelocity, v1, v2);
310                                 
311                         // Calculate the normal component of the relative velocity (actually only the magnitude - the direction is stored in 'normal').
312                         magrelVel = INPR(relativeVelocity, collpair->normal);
313                                         
314                         // Calculate masses of points.
315                         
316                         // If v_n_mag > 0 the edges are approaching each other.
317                         
318                         if(magrelVel < -ALMOST_ZERO)
319                         {
320                                 // Calculate Impulse magnitude to stop all motion in normal direction.
321                                 // const double I_mag = v_n_mag / (1/m1 + 1/m2);
322                                 float magnitude_i = magrelVel / 2.0f; // TODO implement masses
323                                 float tangential[3], magtangent, magnormal, collvel[3];
324                                 float vrel_t_pre[3];
325                                 float vrel_t[3];
326                                 double impulse;
327                                 float epsilon = clmd->coll_parms.epsilon;
328                                 float overlap = (epsilon + ALMOST_ZERO-collpair->distance);
329                                 
330                                 // calculateFrictionImpulse(tangential, relativeVelocity, collpair->normal, magrelVel, clmd->coll_parms.friction*0.01, magrelVel);
331                                 
332                                 // magtangent = INPR(tangential, tangential);
333                                 
334                                 // Apply friction impulse.
335                                 if (magtangent < ALMOST_ZERO) 
336                                 {
337                                         
338                                         // printf("friction applied: %f\n", magtangent);
339                                         // TODO check original code 
340                                         
341                                         VECSUB(cloth1->verts[face1->v1].tv, cloth1->verts[face1->v1].tv,tangential);
342                                         VECSUB(cloth1->verts[face1->v1].tv, cloth1->verts[face1->v2].tv,tangential);
343                                         VECSUB(cloth1->verts[face1->v1].tv, cloth1->verts[face1->v3].tv,tangential);
344                                         VECSUB(cloth1->verts[face1->v1].tv, cloth1->verts[face1->v4].tv,tangential);
345                                         
346                                 }
347                                 
348                                 impulse = -magrelVel / ( 1.0 + w1*w1 + w2*w2 + w3*w3);
349                                 VECADDMUL(cloth1->verts[face1->v1].impulse, collpair->normal, impulse); 
350                                 cloth1->verts[face1->v1].impulse_count++;
351                                 
352                                 VECADDMUL(cloth1->verts[face1->v2].impulse, collpair->normal, impulse); 
353                                 cloth1->verts[face1->v2].impulse_count++;
354                                 
355                                 VECADDMUL(cloth1->verts[face1->v3].impulse, collpair->normal, impulse); 
356                                 cloth1->verts[face1->v3].impulse_count++;
357                                 
358                                 if(face1->v4)
359                                 {
360                                         VECADDMUL(cloth1->verts[face1->v4].impulse, collpair->normal, impulse); 
361                                         cloth1->verts[face1->v4].impulse_count++;
362                                 }
363                                 
364                                 
365                                 if (overlap > ALMOST_ZERO) {
366                                         double I_mag  = overlap * 0.1;
367                                         
368                                         impulse = I_mag / ( 1.0 + w1*w1 + w2*w2 + w3*w3);
369                                         
370                                         VECADDMUL(cloth1->verts[face1->v1].impulse, collpair->normal, impulse); 
371                                         cloth1->verts[face1->v1].impulse_count++;
372                                                                 
373                                         VECADDMUL(cloth1->verts[face1->v2].impulse, collpair->normal, impulse); 
374                                         cloth1->verts[face1->v2].impulse_count++;
375                                 
376                                         VECADDMUL(cloth1->verts[face1->v3].impulse, collpair->normal, impulse); 
377                                         cloth1->verts[face1->v3].impulse_count++;
378                                 
379                                         if(face1->v4)
380                                         {
381                                                 VECADDMUL(cloth1->verts[face1->v4].impulse, collpair->normal, impulse); 
382                                                 cloth1->verts[face1->v4].impulse_count++;
383                                         }
384                                 
385                                 }
386                                 
387                                 result = 1;
388                                 
389                         
390                                 // printf("magnitude_i: %f\n", magnitude_i); // negative before collision in my case
391                                 
392                                 // Apply the impulse and increase impulse counters.
393
394                                 /               
395                                 // calculateFrictionImpulse(tangential, collvel, collpair->normal, magtangent, clmd->coll_parms.friction*0.01, magtangent);
396                                 VECSUBS(vrel_t_pre, collvel, collpair->normal, magnormal);
397                                 // VecMulf(vrel_t_pre, clmd->coll_parms.friction*0.01f/INPR(vrel_t_pre,vrel_t_pre));
398                                 magtangent = Normalize(vrel_t_pre);
399                                 VecMulf(vrel_t_pre, MIN2(clmd->coll_parms.friction*0.01f*magnormal,magtangent));
400                                 
401                                 VECSUB(cloth1->verts[face1->v1].tv, cloth1->verts[face1->v1].tv,vrel_t_pre);
402                                 
403                                 
404                                 
405                                 
406                         }
407                         
408                         search = search->next;
409                 }
410         }
411                 
412         return result;
413 }
414 */
415                 
416 // return distance between two triangles using bullet engine
417 double implicit_tri_check_coherence (ClothModifierData *clmd, ClothModifierData *coll_clmd, unsigned int tri_index1, unsigned int tri_index2, float pa[3], float pb[3], float normal[3], int quadA, int quadB)
418 {
419         MFace *face1=NULL, *face2=NULL;
420         float  a[3][3];
421         float  b[3][3];
422         double distance=0, tempdistance=0;
423         Cloth *cloth1=NULL, *cloth2=NULL;
424         float tpa[3], tpb[3], tnormal[3];
425         unsigned int indexA=0, indexB=0, indexC=0, indexD=0, indexE=0, indexF=0;
426         int i = 0;
427         
428         cloth1 = clmd->clothObject;
429         cloth2 = coll_clmd->clothObject;
430         
431         face1 = &(cloth1->mfaces[tri_index1]);
432         face2 = &(cloth2->mfaces[tri_index2]);
433         
434         // face a1 + face b1
435         VECCOPY(a[0], cloth1->verts[face1->v1].txold);
436         VECCOPY(a[1], cloth1->verts[face1->v2].txold);
437         VECCOPY(a[2], cloth1->verts[face1->v3].txold);
438         
439         
440         VECCOPY(b[0], cloth2->verts[face2->v1].txold);
441         VECCOPY(b[1], cloth2->verts[face2->v2].txold);
442         VECCOPY(b[2], cloth2->verts[face2->v3].txold);
443
444         distance = plNearestPoints(a,b,pa,pb,normal);
445         
446         quadA = quadB = 0;
447         
448         for(i = 0; i < 3; i++)
449         {
450                 if(i == 0)
451                 {
452                         if(face1->v4)
453                         {
454                                 indexA = face1->v4;
455                                 indexB = face1->v1;
456                                 indexC = face1->v3;
457                                 
458                                 indexD = face2->v1;
459                                 indexE = face2->v2;
460                                 indexF = face2->v3;
461                         }
462                         else 
463                                 i+=2;
464                 }
465                 
466                 if(i == 1)
467                 {
468                         if((face1->v4)&&(face2->v4))
469                         {
470                                 indexA = face1->v4;
471                                 indexB = face1->v1;
472                                 indexC = face1->v3;
473                         
474                                 indexD = face2->v4;
475                                 indexE = face2->v1;
476                                 indexF = face2->v3;
477                         }
478                         else
479                                 i++;
480                 }
481                 
482                 if(i == 2)
483                 {
484                         if(face2->v4)
485                         {
486                                 indexA = face1->v1;
487                                 indexB = face1->v2;
488                                 indexC = face1->v3;
489                         
490                                 indexD = face2->v4;
491                                 indexE = face2->v1;
492                                 indexF = face2->v3;
493                         }
494                         else
495                                 i++;
496                         
497                 }
498                 
499                 if(i<3)
500                 {
501                         // face a2 + face b1
502                         VECCOPY(a[0], cloth1->verts[indexA].txold);
503                         VECCOPY(a[1], cloth1->verts[indexB].txold);
504                         VECCOPY(a[2], cloth1->verts[indexC].txold);
505                         
506                         
507                         VECCOPY(b[0], cloth2->verts[indexD].txold);
508                         VECCOPY(b[1], cloth2->verts[indexE].txold);
509                         VECCOPY(b[2], cloth2->verts[indexF].txold);
510         
511                         tempdistance = plNearestPoints(a,b,tpa,tpb,tnormal);
512                         
513                         if(tempdistance < distance)
514                         {
515                                 VECCOPY(pa, tpa);
516                                 VECCOPY(pb, tpb);
517                                 VECCOPY(normal, tnormal);
518                                 distance = tempdistance;
519                                 
520                                 if(i == 0)
521                                 {
522                                         quadA = 1; quadB = 0;
523                                 }
524                                 else if(i == 1)
525                                 {
526                                         quadA = quadB = 1;
527                                 }
528                                 else if(i == 2)
529                                 {
530                                         quadA = 0; quadB = 1;
531                                 }
532                         }
533                 }
534         }
535         return distance;
536 }
537
538 // calculate plane normal
539 void calcPlaneNormal(float normal[3], float p11[3], float p12[3], float p13[3])
540 {
541         float temp1[3], temp2[3];
542         float tnormal[3];
543         
544         VECSUB(temp1, p12,p11);
545         VECSUB(temp2, p13,p11);
546         Crossf(normal, temp1, temp2);
547         Normalize(normal);
548         // VECCOPY(normal, tnormal);
549 }
550
551 float distance_triangle_point( float p11[3], float p12[3], float p13[3], float p21[3], float normal[3])
552 {
553         float temp[3];
554         float magnitude = 0;
555         
556         VECSUB(temp, p21, p13);
557         magnitude = INPR(temp, normal);
558         
559         if(magnitude < 0)
560         {
561                 magnitude *= -1.0f;
562                 // VecMulf(normal, -1.0f);
563         }
564         
565         return magnitude;
566 }
567
568 float nearest_point_triangle_triangle(float p11[3], float p12[3], float p13[3], float p21[3], float p22[3], float p23[3], float normal[3])
569 {
570         float distance = 0, tdistance = 0, tnormal[3];
571         
572         // first triangle 1-2-3 versus second triangle 1-2-3
573         calcPlaneNormal(normal, p11, p12, p13);
574         distance = distance_triangle_point(p11, p12, p13, p21, normal);
575         
576         tdistance = distance_triangle_point(p11, p12, p13, p22, normal);
577         
578         if(tdistance < distance)
579         {       
580                 distance = tdistance;
581         }
582         
583         tdistance = distance_triangle_point(p11, p12, p13, p23, normal);
584         
585         if(tdistance < distance)
586         {       
587                 distance = tdistance;
588         }
589         
590         // second triangle 1-2-3 versus first triangle 1-2-3
591         calcPlaneNormal(tnormal, p21, p22, p23);
592         
593         tdistance = distance_triangle_point(p21, p22, p23, p11, tnormal);
594         
595         if(tdistance < distance)
596         {       
597                 distance = tdistance;
598                 VECCOPY(normal, tnormal);
599         }
600         
601         tdistance = distance_triangle_point(p21, p22, p23, p12, tnormal);
602         
603         if(tdistance < distance)
604         {       
605                 distance = tdistance;
606                 VECCOPY(normal, tnormal);
607         }
608         
609         tdistance = distance_triangle_point(p21, p22, p23, p13, tnormal);
610         
611         if(tdistance < distance)
612         {       
613                 distance = tdistance;
614                 VECCOPY(normal, tnormal);
615         }
616         
617         
618         if (distance < 0) {
619                 VecMulf(normal, -1.0f);
620                 distance = -distance;
621         }
622         
623         return distance;        
624 }
625
626                 
627 int collision_static2(ClothModifierData *clmd, ClothModifierData *coll_clmd, LinkNode **collision_list)
628 {
629         unsigned int i = 0, numfaces = 0;
630         int result = 0;
631         LinkNode *search = NULL;
632         CollPair *collpair = NULL;
633         Cloth *cloth1, *cloth2;
634         MFace *face1, *face2;
635         double w1, w2, w3, u1, u2, u3, a1, a2, a3;
636         float v1[3], v2[3], relativeVelocity[3];
637         float magrelVel;
638         
639         cloth1 = clmd->clothObject;
640         cloth2 = coll_clmd->clothObject;
641         
642         numfaces = clmd->clothObject->numfaces;
643                 
644         for(i = 0; i < numfaces; i++)
645         {
646                 search = collision_list[i];
647                 
648                 while(search)
649                 {
650                         collpair = search->link;
651                         
652                         face1 = &(cloth1->mfaces[collpair->face1]);
653                         face2 = &(cloth2->mfaces[collpair->face2]);
654                         
655                         // compute barycentric coordinates for both collision points
656                         
657                 
658                         bvh_compute_barycentric(collpair->p1,
659                                         cloth1->verts[collpair->Aindex1].txold,
660                                         cloth1->verts[collpair->Aindex2].txold,
661                                         cloth1->verts[collpair->Aindex3].txold, 
662                                         &w1, &w2, &w3);
663                 
664                         bvh_compute_barycentric(collpair->p2,
665                                         cloth2->verts[collpair->Bindex1].txold,
666                                         cloth2->verts[collpair->Bindex1].txold,
667                                         cloth2->verts[collpair->Bindex3].txold, 
668                                         &u1, &u2, &u3);
669                         
670                         // Calculate relative "velocity".
671                         interpolateOnTriangle(v1, cloth1->verts[collpair->Aindex1].tv, cloth1->verts[collpair->Aindex2].tv, cloth1->verts[collpair->Aindex3].tv, w1, w2, w3);
672                 
673                         interpolateOnTriangle(v2, cloth2->verts[collpair->Bindex1].tv, cloth2->verts[collpair->Bindex2].tv, cloth2->verts[collpair->Bindex3].tv, u1, u2, u3);
674                 
675                         VECSUB(relativeVelocity, v1, v2);
676                                 
677                         // Calculate the normal component of the relative velocity (actually only the magnitude - the direction is stored in 'normal').
678                         magrelVel = INPR(relativeVelocity, collpair->normal);
679                                         
680                         // Calculate masses of points.
681                         
682                         // If v_n_mag > 0 the edges are approaching each other.
683                         
684                         if(magrelVel < -ALMOST_ZERO)
685                         {
686                                 // Calculate Impulse magnitude to stop all motion in normal direction.
687                                 // const double I_mag = v_n_mag / (1/m1 + 1/m2);
688                                 float magnitude_i = magrelVel / 2.0f; // TODO implement masses
689                                 float tangential[3], magtangent, magnormal, collvel[3];
690                                 float vrel_t_pre[3];
691                                 float vrel_t[3];
692                                 double impulse;
693                                 float epsilon = clmd->coll_parms.epsilon;
694                                 float overlap = (epsilon + ALMOST_ZERO-collpair->distance);
695                                 
696                                 /*
697                                 impulse = -magrelVel / ( 1.0 + w1*w1 + w2*w2 + w3*w3);
698                                 VECADDMUL(cloth1->verts[face1->v1].impulse, collpair->normal, impulse); 
699                                 cloth1->verts[face1->v1].impulse_count++;
700                                 
701                                 VECADDMUL(cloth1->verts[face1->v2].impulse, collpair->normal, impulse); 
702                                 cloth1->verts[face1->v2].impulse_count++;
703                                 
704                                 VECADDMUL(cloth1->verts[face1->v3].impulse, collpair->normal, impulse); 
705                                 cloth1->verts[face1->v3].impulse_count++;
706                                 */
707                         
708                                 
709                                 /*
710                                 if (overlap > ALMOST_ZERO) {
711                                         double I_mag  = overlap * 0.1;
712                                         
713                                         impulse = I_mag / ( 1.0 + w1*w1 + w2*w2 + w3*w3);
714                                         
715                                         VECADDMUL(cloth1->verts[face1->v1].impulse, collpair->normal, impulse); 
716                                         cloth1->verts[face1->v1].impulse_count++;
717                                                                 
718                                         VECADDMUL(cloth1->verts[face1->v2].impulse, collpair->normal, impulse); 
719                                         cloth1->verts[face1->v2].impulse_count++;
720                                 
721                                         VECADDMUL(cloth1->verts[face1->v3].impulse, collpair->normal, impulse); 
722                                         cloth1->verts[face1->v3].impulse_count++;
723                                 
724                                         if(face1->v4)
725                                         {
726                                                 VECADDMUL(cloth1->verts[face1->v4].impulse, collpair->normal, impulse); 
727                                                 cloth1->verts[face1->v4].impulse_count++;
728                                         }
729                                 
730                                 }
731                                 */
732                                 
733                                 result = 1;
734                         }
735                         
736                         search = search->next;
737                 }
738         }
739                 
740         return result;
741 }
742
743 void bvh_collision_response(ClothModifierData *clmd, ClothModifierData *coll_clmd, Tree * tree1, Tree * tree2)
744 {
745         CollPair *collpair = NULL;
746         LinkNode **linknode;
747         double distance = 0;
748         float epsilon = clmd->coll_parms.epsilon, tdistance=0;
749         MFace *face1, *face2;
750         ClothVertex *verts1, *verts2;
751         Cloth *cloth1=NULL, *cloth2=NULL;
752         int i = 0;
753         
754         linknode = clmd->coll_parms.temp;
755         
756         cloth1 = clmd->clothObject;
757         cloth2 = coll_clmd->clothObject;
758         
759         // calc SIPcode (?)
760         
761         for(i = 0; i < 4; i++)
762         {
763                 collpair = (CollPair *)MEM_callocN(sizeof(CollPair), "cloth coll pair");
764                 
765                 face1 = &(cloth1->mfaces[tree1->tri_index]);
766                 face2 = &(cloth2->mfaces[tree2->tri_index]);
767                         
768                 verts1 = cloth1->verts;
769                 verts2 = cloth2->verts;
770                 
771                 if(i == 0)
772                 {
773                         collpair->Aindex1 = face1->v1;
774                         collpair->Aindex2 = face1->v2;
775                         collpair->Aindex3 = face1->v3;
776                         collpair->Aindex4 = face1->v4;
777                         
778                         collpair->Bindex1 = face2->v1;
779                         collpair->Bindex2 = face2->v2;
780                         collpair->Bindex3 = face2->v3;
781                         collpair->Bindex4 = face2->v4;
782                         
783                 }
784                 
785                 if(i == 1)
786                 {
787                         if(face2->v4)
788                         {               
789                                 collpair->Aindex1 = face1->v1;
790                                 collpair->Aindex2 = face1->v2;
791                                 collpair->Aindex3 = face1->v3;
792                                 collpair->Aindex4 = face1->v4;
793                                 
794                                 collpair->Bindex1 = face2->v4;
795                                 collpair->Bindex2 = face2->v3;
796                                 collpair->Bindex3 = face2->v1;
797                                 collpair->Bindex4 = face2->v1;
798                         }
799                         else
800                                 i++;
801                         
802                 }
803                 
804                 if(i == 2)
805                 {
806                         if(face1->v4)
807                         {               
808                                 collpair->Aindex1 = face1->v4;
809                                 collpair->Aindex2 = face1->v3;
810                                 collpair->Aindex3 = face1->v1;
811                                 collpair->Aindex4 = face1->v2;
812                                 
813                                 collpair->Bindex1 = face2->v1;
814                                 collpair->Bindex2 = face2->v2;
815                                 collpair->Bindex3 = face2->v3;
816                                 collpair->Bindex4 = face2->v4;
817                         }
818                         else
819                                 i++;
820                 }
821                 
822                 if(i == 3)
823                 {
824                         if((face2->v4) && (face1->v4))
825                         {               
826                                 collpair->Aindex1 = face1->v4;
827                                 collpair->Aindex2 = face1->v3;
828                                 collpair->Aindex3 = face1->v1;
829                                 collpair->Aindex4 = face1->v2;
830                                 
831                                 collpair->Bindex1 = face2->v4;
832                                 collpair->Bindex2 = face2->v3;
833                                 collpair->Bindex3 = face2->v1;
834                                 collpair->Bindex4 = face2->v2;
835                         }
836                         else
837                                 i++;
838                 }
839                 
840                 if(i < 4)
841                 {
842                         distance = nearest_point_triangle_triangle(verts1[collpair->Aindex1].txold, verts1[collpair->Aindex2].txold, verts1[collpair->Aindex3].txold, verts2[collpair->Bindex1].txold, verts2[collpair->Bindex2].txold, verts2[collpair->Bindex3].txold, collpair->normal);
843                         
844                         // calc distance + normal       
845                         // distance = implicit_tri_check_coherence(clmd, coll_clmd, tree1->tri_index, tree2->tri_index, collpair->p1, collpair->p2, collpair->vector, collpair->quadA, collpair->quadB);
846                         
847                         if (distance <= (epsilon + ALMOST_ZERO)) // max overlap = 1.0 
848                         {
849                                 
850                                 printf("dist: %f, tdist: %f\n", (float)distance, tdistance);
851                                 
852                                 collpair->face1 = tree1->tri_index;
853                                 collpair->face2 = tree2->tri_index;
854                                 
855                                 collpair->distance = distance;
856                                 BLI_linklist_append(&linknode[tree1->tri_index], collpair);
857                         }
858                         else
859                         {
860                                 MEM_freeN(collpair);
861                         }
862                 }
863         }
864 }
865
866 // move collision objects forward in time and update static bounding boxes
867 void cloth_update_collision_objects(float step)
868 {
869         Base *base=NULL;
870         ClothModifierData *coll_clmd=NULL;
871         Object *coll_ob=NULL;
872         unsigned int i=0;
873         
874         // search all objects for collision object
875         for (base = G.scene->base.first; base; base = base->next)
876         {
877
878                 coll_ob = base->object;
879                 coll_clmd = (ClothModifierData *) modifiers_findByType (coll_ob, eModifierType_Cloth);
880                 if (!coll_clmd)
881                         continue;
882
883                 // if collision object go on
884                 if (coll_clmd->sim_parms.flags & CSIMSETT_FLAG_COLLOBJ)
885                 {
886                         if (coll_clmd->clothObject && coll_clmd->clothObject->tree) 
887                         {
888                                 Cloth *coll_cloth = coll_clmd->clothObject;
889                                 BVH *coll_bvh = coll_clmd->clothObject->tree;
890                                 unsigned int coll_numverts = coll_cloth->numverts;
891
892                                 // update position of collision object
893                                 for(i = 0; i < coll_numverts; i++)
894                                 {
895                                         VECCOPY(coll_cloth->verts[i].txold, coll_cloth->verts[i].tx);
896
897                                         VECADDS(coll_cloth->verts[i].tx, coll_cloth->verts[i].xold, coll_cloth->verts[i].v, step);
898                                         
899                                         // no dt here because of float rounding errors
900                                         VECSUB(coll_cloth->verts[i].tv, coll_cloth->verts[i].tx, coll_cloth->verts[i].txold);
901                                 }
902                                                 
903                                 // update BVH of collision object
904                                 bvh_update_static(coll_clmd, coll_bvh);
905                         }
906                         else
907                                 printf ("cloth_bvh_objcollision: found a collision object with clothObject or collData NULL.\n");
908                 }
909         }
910 }
911
912 #define CLOTH_MAX_THRESHOLD 5
913
914 // cloth - object collisions
915 int cloth_bvh_objcollision(ClothModifierData * clmd, float step, CM_COLLISION_RESPONSE collision_response, float dt)
916 {
917         Base *base=NULL;
918         ClothModifierData *coll_clmd=NULL;
919         Cloth *cloth=NULL;
920         Object *coll_ob=NULL;
921         BVH *cloth_bvh=NULL;
922         unsigned int i=0, numfaces = 0, numverts = 0;
923         unsigned int result = 0, ic = 0, rounds = 0;
924         ClothVertex *verts = NULL;
925         float tnull[3] = {0,0,0};
926
927         if ((clmd->sim_parms.flags & CSIMSETT_FLAG_COLLOBJ) || !(((Cloth *)clmd->clothObject)->tree))
928         {
929                 return 0;
930         }
931         cloth = clmd->clothObject;
932         verts = cloth->verts;
933         cloth_bvh = (BVH *) cloth->tree;
934         numfaces = clmd->clothObject->numfaces;
935         numverts = clmd->clothObject->numverts;
936         
937         ////////////////////////////////////////////////////////////
938         // static collisions
939         ////////////////////////////////////////////////////////////
940
941         // update cloth bvh
942         bvh_update_static(clmd, cloth_bvh);
943         
944         // update collision objects
945         cloth_update_collision_objects(step);
946
947         do
948         {
949                 result = 0;
950                 ic = 0;
951                         
952                 // handle all collision objects
953                 for (base = G.scene->base.first; base; base = base->next)
954                 {
955         
956                         coll_ob = base->object;
957                         coll_clmd = (ClothModifierData *) modifiers_findByType (coll_ob, eModifierType_Cloth);
958                         if (!coll_clmd)
959                                 continue;
960         
961                         // if collision object go on
962                         if (coll_clmd->sim_parms.flags & CSIMSETT_FLAG_COLLOBJ)
963                         {
964                                 if (coll_clmd->clothObject && coll_clmd->clothObject->tree) 
965                                 {
966                                         LinkNode **collision_list = MEM_callocN (sizeof(LinkNode *)*(numfaces), "collision_list");
967                                         BVH *coll_bvh = coll_clmd->clothObject->tree;
968         
969                                         if(collision_list)
970                                         {                                       
971                                                 memset(collision_list, 0, sizeof(LinkNode *)*numfaces);
972                                                 clmd->coll_parms.temp = collision_list;
973         
974                                                 bvh_traverse(clmd, coll_clmd, cloth_bvh->root, coll_bvh->root, step, collision_response);
975                                                 
976                                                 result += collision_static2(clmd, coll_clmd, collision_list);
977                                                 
978                                                 // calculate velocities
979                                                 
980                                                 // free temporary list 
981                                                 for(i = 0; i < numfaces; i++)
982                                                 {
983                                                         LinkNode *search = collision_list[i];
984                                                         while(search)
985                                                         {
986                                                                 LinkNode *next= search->next;
987                                                                 CollPair *collpair = search->link;
988                                                                 
989                                                                 if(collpair)
990                                                                         MEM_freeN(collpair);    
991         
992                                                                 search = next;
993                                                         }
994         
995                                                         BLI_linklist_free(collision_list[i],NULL); 
996                                                 }
997                                                 if(collision_list)
998                                                         MEM_freeN(collision_list);
999         
1000                                                 clmd->coll_parms.temp = NULL;
1001                                         }
1002                                         
1003         
1004                                 }
1005                                 else
1006                                         printf ("cloth_bvh_objcollision: found a collision object with clothObject or collData NULL.\n");
1007                         }
1008                 }
1009                 
1010                 // now apply impulses parallel
1011                 
1012                 for(i = 0; i < numverts; i++)
1013                 {
1014                         if(verts[i].impulse_count)
1015                         {
1016                                 VECADDMUL(verts[i].tv, verts[i].impulse, 1.0f / verts[i].impulse_count);
1017                                 VECCOPY(verts[i].impulse, tnull);
1018                                 verts[i].impulse_count = 0;
1019                                 
1020                                 ic++;
1021                         }
1022                 }
1023                 
1024                 printf("ic: %d\n", ic);
1025                 rounds++;
1026         }
1027         while(result && (CLOTH_MAX_THRESHOLD>rounds));
1028         
1029         printf("\n");
1030                         
1031         ////////////////////////////////////////////////////////////
1032         // update positions + velocities
1033         ////////////////////////////////////////////////////////////
1034
1035         // TODO 
1036
1037
1038         ////////////////////////////////////////////////////////////
1039         // moving collisions
1040         ////////////////////////////////////////////////////////////
1041
1042         // TODO 
1043         // bvh_update_moving(clmd, clmd->clothObject->tree);
1044
1045         return MIN2(result, 1);
1046 }