4 * ***** BEGIN GPL LICENSE BLOCK *****
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.
11 * This program is distributed in the hope that it will be useful,
12 * but WITHOUT ANY WARRANTY; without even the implied warranty of
13 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14 * GNU General Public License for more details.
16 * You should have received a copy of the GNU General Public License
17 * along with this program; if not, write to the Free Software Foundation,
18 * Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
20 * The Original Code is Copyright (C) Blender Foundation
21 * All rights reserved.
23 * The Original Code is: all of this file.
25 * Contributor(s): none yet.
27 * ***** END GPL LICENSE BLOCK *****
30 #include "MEM_guardedalloc.h"
32 #include "BKE_cloth.h"
34 #include "DNA_cloth_types.h"
35 #include "DNA_group_types.h"
36 #include "DNA_mesh_types.h"
37 #include "DNA_object_types.h"
38 #include "DNA_object_force.h"
39 #include "DNA_scene_types.h"
40 #include "DNA_meshdata_types.h"
42 #include "BLI_blenlib.h"
44 #include "BLI_edgehash.h"
46 #include "BKE_DerivedMesh.h"
47 #include "BKE_global.h"
48 #include "BKE_scene.h"
50 #include "BKE_object.h"
51 #include "BKE_modifier.h"
52 #include "BKE_utildefines.h"
53 #include "BKE_DerivedMesh.h"
55 #include "Bullet-C-Api.h"
57 #include "BLI_kdopbvh.h"
58 #include "BKE_collision.h"
61 /***********************************
62 Collision modifier code start
63 ***********************************/
65 /* step is limited from 0 (frame start position) to 1 (frame end position) */
66 void collision_move_object ( CollisionModifierData *collmd, float step, float prevstep )
68 float tv[3] = {0, 0, 0};
71 for ( i = 0; i < collmd->numverts; i++ )
73 VECSUB ( tv, collmd->xnew[i].co, collmd->x[i].co );
74 VECADDS ( collmd->current_x[i].co, collmd->x[i].co, tv, prevstep );
75 VECADDS ( collmd->current_xnew[i].co, collmd->x[i].co, tv, step );
76 VECSUB ( collmd->current_v[i].co, collmd->current_xnew[i].co, collmd->current_x[i].co );
79 bvhtree_update_from_mvert ( collmd->bvhtree, collmd->mfaces, collmd->numfaces, collmd->current_x, collmd->current_xnew, collmd->numverts, 1 );
82 BVHTree *bvhtree_build_from_mvert ( MFace *mfaces, unsigned int numfaces, MVert *x, unsigned int UNUSED(numverts), float epsilon )
87 MFace *tface = mfaces;
89 tree = BLI_bvhtree_new ( numfaces*2, epsilon, 4, 26 );
92 for ( i = 0; i < numfaces; i++, tface++ )
94 VECCOPY ( &co[0*3], x[tface->v1].co );
95 VECCOPY ( &co[1*3], x[tface->v2].co );
96 VECCOPY ( &co[2*3], x[tface->v3].co );
98 VECCOPY ( &co[3*3], x[tface->v4].co );
100 BLI_bvhtree_insert ( tree, i, co, ( mfaces->v4 ? 4 : 3 ) );
104 BLI_bvhtree_balance ( tree );
109 void bvhtree_update_from_mvert ( BVHTree * bvhtree, MFace *faces, int numfaces, MVert *x, MVert *xnew, int UNUSED(numverts), int moving )
112 MFace *mfaces = faces;
113 float co[12], co_moving[12];
121 for ( i = 0; i < numfaces; i++, mfaces++ )
123 VECCOPY ( &co[0*3], x[mfaces->v1].co );
124 VECCOPY ( &co[1*3], x[mfaces->v2].co );
125 VECCOPY ( &co[2*3], x[mfaces->v3].co );
127 VECCOPY ( &co[3*3], x[mfaces->v4].co );
129 // copy new locations into array
130 if ( moving && xnew )
132 // update moving positions
133 VECCOPY ( &co_moving[0*3], xnew[mfaces->v1].co );
134 VECCOPY ( &co_moving[1*3], xnew[mfaces->v2].co );
135 VECCOPY ( &co_moving[2*3], xnew[mfaces->v3].co );
137 VECCOPY ( &co_moving[3*3], xnew[mfaces->v4].co );
139 ret = BLI_bvhtree_update_node ( bvhtree, i, co, co_moving, ( mfaces->v4 ? 4 : 3 ) );
143 ret = BLI_bvhtree_update_node ( bvhtree, i, co, NULL, ( mfaces->v4 ? 4 : 3 ) );
146 // check if tree is already full
151 BLI_bvhtree_update_tree ( bvhtree );
155 /***********************************
156 Collision modifier code end
157 ***********************************/
160 * gsl_poly_solve_cubic -
162 * copied from SOLVE_CUBIC.C --> GSL
165 #define mySWAP(a,b) do { double tmp = b ; b = a ; a = tmp ; } while(0)
168 gsl_poly_solve_cubic (double a, double b, double c,
169 double *x0, double *x1, double *x2)
171 double q = (a * a - 3 * b);
172 double r = (2 * a * a * a - 9 * a * b + 27 * c);
177 double Q3 = Q * Q * Q;
180 double CR2 = 729 * r * r;
181 double CQ3 = 2916 * q * q * q;
183 if (R == 0 && Q == 0)
192 /* this test is actually R2 == Q3, written in a form suitable
193 for exact computation with integers */
195 /* Due to finite precision some double roots may be missed, and
196 considered to be a pair of complex roots z = x +/- epsilon i
197 close to the real axis. */
199 double sqrtQ = sqrt (Q);
203 *x0 = -2 * sqrtQ - a / 3;
209 *x0 = - sqrtQ - a / 3;
210 *x1 = - sqrtQ - a / 3;
211 *x2 = 2 * sqrtQ - a / 3;
215 else if (CR2 < CQ3) /* equivalent to R2 < Q3 */
217 double sqrtQ = sqrt (Q);
218 double sqrtQ3 = sqrtQ * sqrtQ * sqrtQ;
219 double theta = acos (R / sqrtQ3);
220 double norm = -2 * sqrtQ;
221 *x0 = norm * cos (theta / 3) - a / 3;
222 *x1 = norm * cos ((theta + 2.0 * M_PI) / 3) - a / 3;
223 *x2 = norm * cos ((theta - 2.0 * M_PI) / 3) - a / 3;
225 /* Sort *x0, *x1, *x2 into increasing order */
242 double sgnR = (R >= 0 ? 1 : -1);
243 double A = -sgnR * pow (fabs (R) + sqrt (R2 - Q3), 1.0/3.0);
253 * gsl_poly_solve_quadratic
258 gsl_poly_solve_quadratic (double a, double b, double c,
259 double *x0, double *x1)
261 double disc = b * b - 4 * a * c;
263 if (a == 0) /* Handle linear case */
280 double r = fabs (0.5 * sqrt (disc) / a);
286 double sgnb = (b > 0 ? 1 : -1);
287 double temp = -0.5 * (b + sgnb * sqrt (disc));
288 double r1 = temp / a ;
289 double r2 = c / temp ;
320 * See Bridson et al. "Robust Treatment of Collision, Contact and Friction for Cloth Animation"
321 * page 4, left column
324 static int cloth_get_collision_time ( double a[3], double b[3], double c[3], double d[3], double e[3], double f[3], double solution[3] )
329 double g = a[0] * c[1] * e[2] - a[0] * c[2] * e[1] +
330 a[1] * c[2] * e[0] - a[1] * c[0] * e[2] +
331 a[2] * c[0] * e[1] - a[2] * c[1] * e[0];
334 double h = -b[2] * c[1] * e[0] + b[1] * c[2] * e[0] - a[2] * d[1] * e[0] +
335 a[1] * d[2] * e[0] + b[2] * c[0] * e[1] - b[0] * c[2] * e[1] +
336 a[2] * d[0] * e[1] - a[0] * d[2] * e[1] - b[1] * c[0] * e[2] +
337 b[0] * c[1] * e[2] - a[1] * d[0] * e[2] + a[0] * d[1] * e[2] -
338 a[2] * c[1] * f[0] + a[1] * c[2] * f[0] + a[2] * c[0] * f[1] -
339 a[0] * c[2] * f[1] - a[1] * c[0] * f[2] + a[0] * c[1] * f[2];
342 double i = -b[2] * d[1] * e[0] + b[1] * d[2] * e[0] +
343 b[2] * d[0] * e[1] - b[0] * d[2] * e[1] -
344 b[1] * d[0] * e[2] + b[0] * d[1] * e[2] -
345 b[2] * c[1] * f[0] + b[1] * c[2] * f[0] -
346 a[2] * d[1] * f[0] + a[1] * d[2] * f[0] +
347 b[2] * c[0] * f[1] - b[0] * c[2] * f[1] +
348 a[2] * d[0] * f[1] - a[0] * d[2] * f[1] -
349 b[1] * c[0] * f[2] + b[0] * c[1] * f[2] -
350 a[1] * d[0] * f[2] + a[0] * d[1] * f[2];
353 double j = -b[2] * d[1] * f[0] + b[1] * d[2] * f[0] +
354 b[2] * d[0] * f[1] - b[0] * d[2] * f[1] -
355 b[1] * d[0] * f[2] + b[0] * d[1] * f[2];
358 printf("r1: %lf\n", a[0] * c[1] * e[2] - a[0] * c[2] * e[1]);
359 printf("r2: %lf\n", a[1] * c[2] * e[0] - a[1] * c[0] * e[2]);
360 printf("r3: %lf\n", a[2] * c[0] * e[1] - a[2] * c[1] * e[0]);
362 printf("x1 x: %f, y: %f, z: %f\n", a[0], a[1], a[2]);
363 printf("x2 x: %f, y: %f, z: %f\n", c[0], c[1], c[2]);
364 printf("x3 x: %f, y: %f, z: %f\n", e[0], e[1], e[2]);
366 printf("v1 x: %f, y: %f, z: %f\n", b[0], b[1], b[2]);
367 printf("v2 x: %f, y: %f, z: %f\n", d[0], d[1], d[2]);
368 printf("v3 x: %f, y: %f, z: %f\n", f[0], f[1], f[2]);
370 printf("t^3: %lf, t^2: %lf, t^1: %lf, t^0: %lf\n", j, i, h, g);
373 // Solve cubic equation to determine times t1, t2, t3, when the collision will occur.
374 if ( ABS ( j ) > DBL_EPSILON )
379 num_sols = gsl_poly_solve_cubic ( i, h, g, &solution[0], &solution[1], &solution[2] );
383 num_sols = gsl_poly_solve_quadratic ( i, h, g, &solution[0], &solution[1] );
387 // printf("num_sols: %d, sol1: %lf, sol2: %lf, sol3: %lf\n", num_sols, solution[0], solution[1], solution[2]);
389 // Discard negative solutions
390 if ( ( num_sols >= 1 ) && ( solution[0] < DBL_EPSILON ) )
393 solution[0] = solution[num_sols];
395 if ( ( num_sols >= 2 ) && ( solution[1] < DBL_EPSILON ) )
398 solution[1] = solution[num_sols];
400 if ( ( num_sols == 3 ) && ( solution[2] < DBL_EPSILON ) )
408 if ( solution[0] > solution[1] )
410 double tmp = solution[0];
411 solution[0] = solution[1];
415 else if ( num_sols == 3 )
419 if ( solution[0] > solution[1] )
421 double tmp = solution[0]; solution[0] = solution[1]; solution[1] = tmp;
423 if ( solution[1] > solution[2] )
425 double tmp = solution[1]; solution[1] = solution[2]; solution[2] = tmp;
427 if ( solution[0] > solution[1] )
429 double tmp = solution[0]; solution[0] = solution[1]; solution[1] = tmp;
439 static void collision_compute_barycentric ( float pv[3], float p1[3], float p2[3], float p3[3], float *w1, float *w2, float *w3 )
441 double tempV1[3], tempV2[3], tempV4[3];
444 VECSUB ( tempV1, p1, p3 );
445 VECSUB ( tempV2, p2, p3 );
446 VECSUB ( tempV4, pv, p3 );
448 a = INPR ( tempV1, tempV1 );
449 b = INPR ( tempV1, tempV2 );
450 c = INPR ( tempV2, tempV2 );
451 e = INPR ( tempV1, tempV4 );
452 f = INPR ( tempV2, tempV4 );
454 d = ( a * c - b * b );
456 if ( ABS ( d ) < ALMOST_ZERO )
458 *w1 = *w2 = *w3 = 1.0 / 3.0;
462 w1[0] = ( float ) ( ( e * c - b * f ) / d );
467 w2[0] = ( float ) ( ( f - b * ( double ) w1[0] ) / c );
472 w3[0] = 1.0f - w1[0] - w2[0];
475 DO_INLINE void collision_interpolateOnTriangle ( float to[3], float v1[3], float v2[3], float v3[3], double w1, double w2, double w3 )
477 to[0] = to[1] = to[2] = 0;
478 VECADDMUL ( to, v1, w1 );
479 VECADDMUL ( to, v2, w2 );
480 VECADDMUL ( to, v3, w3 );
484 int cloth_collision_response_static ( ClothModifierData *clmd, CollisionModifierData *collmd, CollPair *collpair, CollPair *collision_end )
488 float w1, w2, w3, u1, u2, u3;
489 float v1[3], v2[3], relativeVelocity[3];
491 float epsilon2 = BLI_bvhtree_getepsilon ( collmd->bvhtree );
493 cloth1 = clmd->clothObject;
495 for ( ; collpair != collision_end; collpair++ )
497 // only handle static collisions here
498 if ( collpair->flag & COLLISION_IN_FUTURE )
501 // compute barycentric coordinates for both collision points
502 collision_compute_barycentric ( collpair->pa,
503 cloth1->verts[collpair->ap1].txold,
504 cloth1->verts[collpair->ap2].txold,
505 cloth1->verts[collpair->ap3].txold,
509 collision_compute_barycentric ( collpair->pb,
510 collmd->current_x[collpair->bp1].co,
511 collmd->current_x[collpair->bp2].co,
512 collmd->current_x[collpair->bp3].co,
515 // Calculate relative "velocity".
516 collision_interpolateOnTriangle ( v1, cloth1->verts[collpair->ap1].tv, cloth1->verts[collpair->ap2].tv, cloth1->verts[collpair->ap3].tv, w1, w2, w3 );
518 collision_interpolateOnTriangle ( v2, collmd->current_v[collpair->bp1].co, collmd->current_v[collpair->bp2].co, collmd->current_v[collpair->bp3].co, u1, u2, u3 );
520 VECSUB ( relativeVelocity, v2, v1 );
522 // Calculate the normal component of the relative velocity (actually only the magnitude - the direction is stored in 'normal').
523 magrelVel = INPR ( relativeVelocity, collpair->normal );
525 // printf("magrelVel: %f\n", magrelVel);
527 // Calculate masses of points.
530 // If v_n_mag < 0 the edges are approaching each other.
531 if ( magrelVel > ALMOST_ZERO )
533 // Calculate Impulse magnitude to stop all motion in normal direction.
534 float magtangent = 0, repulse = 0, d = 0;
535 double impulse = 0.0;
539 // calculate tangential velocity
540 VECCOPY ( temp, collpair->normal );
541 mul_v3_fl( temp, magrelVel );
542 VECSUB ( vrel_t_pre, relativeVelocity, temp );
544 // Decrease in magnitude of relative tangential velocity due to coulomb friction
545 // in original formula "magrelVel" should be the "change of relative velocity in normal direction"
546 magtangent = MIN2 ( clmd->coll_parms->friction * 0.01 * magrelVel,sqrt ( INPR ( vrel_t_pre,vrel_t_pre ) ) );
548 // Apply friction impulse.
549 if ( magtangent > ALMOST_ZERO )
551 normalize_v3( vrel_t_pre );
553 impulse = magtangent / ( 1.0 + w1*w1 + w2*w2 + w3*w3 ); // 2.0 *
554 VECADDMUL ( cloth1->verts[collpair->ap1].impulse, vrel_t_pre, w1 * impulse );
555 VECADDMUL ( cloth1->verts[collpair->ap2].impulse, vrel_t_pre, w2 * impulse );
556 VECADDMUL ( cloth1->verts[collpair->ap3].impulse, vrel_t_pre, w3 * impulse );
559 // Apply velocity stopping impulse
560 // I_c = m * v_N / 2.0
561 // no 2.0 * magrelVel normally, but looks nicer DG
562 impulse = magrelVel / ( 1.0 + w1*w1 + w2*w2 + w3*w3 );
564 VECADDMUL ( cloth1->verts[collpair->ap1].impulse, collpair->normal, w1 * impulse );
565 cloth1->verts[collpair->ap1].impulse_count++;
567 VECADDMUL ( cloth1->verts[collpair->ap2].impulse, collpair->normal, w2 * impulse );
568 cloth1->verts[collpair->ap2].impulse_count++;
570 VECADDMUL ( cloth1->verts[collpair->ap3].impulse, collpair->normal, w3 * impulse );
571 cloth1->verts[collpair->ap3].impulse_count++;
573 // Apply repulse impulse if distance too short
574 // I_r = -min(dt*kd, m(0,1d/dt - v_n))
575 spf = (float)clmd->sim_parms->stepsPerFrame / clmd->sim_parms->timescale;
577 d = clmd->coll_parms->epsilon*8.0/9.0 + epsilon2*8.0/9.0 - collpair->distance;
578 if ( ( magrelVel < 0.1*d*spf ) && ( d > ALMOST_ZERO ) )
580 repulse = MIN2 ( d*1.0/spf, 0.1*d*spf - magrelVel );
582 // stay on the safe side and clamp repulse
583 if ( impulse > ALMOST_ZERO )
584 repulse = MIN2 ( repulse, 5.0*impulse );
585 repulse = MAX2 ( impulse, repulse );
587 impulse = repulse / ( 1.0 + w1*w1 + w2*w2 + w3*w3 ); // original 2.0 / 0.25
588 VECADDMUL ( cloth1->verts[collpair->ap1].impulse, collpair->normal, impulse );
589 VECADDMUL ( cloth1->verts[collpair->ap2].impulse, collpair->normal, impulse );
590 VECADDMUL ( cloth1->verts[collpair->ap3].impulse, collpair->normal, impulse );
599 //Determines collisions on overlap, collisions are written to collpair[i] and collision+number_collision_found is returned
600 CollPair* cloth_collision ( ModifierData *md1, ModifierData *md2, BVHTreeOverlap *overlap, CollPair *collpair )
602 ClothModifierData *clmd = ( ClothModifierData * ) md1;
603 CollisionModifierData *collmd = ( CollisionModifierData * ) md2;
604 MFace *face1=NULL, *face2 = NULL;
606 ClothVertex *verts1 = clmd->clothObject->verts;
609 float epsilon1 = clmd->coll_parms->epsilon;
610 float epsilon2 = BLI_bvhtree_getepsilon ( collmd->bvhtree );
613 face1 = & ( clmd->clothObject->mfaces[overlap->indexA] );
614 face2 = & ( collmd->mfaces[overlap->indexB] );
616 // check all 4 possible collisions
617 for ( i = 0; i < 4; i++ )
622 collpair->ap1 = face1->v1;
623 collpair->ap2 = face1->v2;
624 collpair->ap3 = face1->v3;
627 collpair->bp1 = face2->v1;
628 collpair->bp2 = face2->v2;
629 collpair->bp3 = face2->v3;
636 collpair->ap1 = face1->v1;
637 collpair->ap2 = face1->v4;
638 collpair->ap3 = face1->v3;
641 collpair->bp1 = face2->v1;
642 collpair->bp2 = face2->v2;
643 collpair->bp3 = face2->v3;
653 collpair->ap1 = face1->v1;
654 collpair->ap2 = face1->v2;
655 collpair->ap3 = face1->v3;
658 collpair->bp1 = face2->v1;
659 collpair->bp2 = face2->v4;
660 collpair->bp3 = face2->v3;
667 if ( face1->v4 && face2->v4 )
670 collpair->ap1 = face1->v1;
671 collpair->ap2 = face1->v4;
672 collpair->ap3 = face1->v3;
675 collpair->bp1 = face2->v1;
676 collpair->bp2 = face2->v4;
677 collpair->bp3 = face2->v3;
684 // calc distance + normal
685 distance = plNearestPoints (
686 verts1[collpair->ap1].txold, verts1[collpair->ap2].txold, verts1[collpair->ap3].txold, collmd->current_x[collpair->bp1].co, collmd->current_x[collpair->bp2].co, collmd->current_x[collpair->bp3].co, collpair->pa,collpair->pb,collpair->vector );
688 // just be sure that we don't add anything
689 distance = 2.0 * ( epsilon1 + epsilon2 + ALMOST_ZERO );
692 if ( distance <= ( epsilon1 + epsilon2 + ALMOST_ZERO ) )
694 normalize_v3_v3( collpair->normal, collpair->vector );
696 collpair->distance = distance;
702 float w1, w2, w3, u1, u2, u3;
703 float v1[3], v2[3], relativeVelocity[3];
705 // calc relative velocity
707 // compute barycentric coordinates for both collision points
708 collision_compute_barycentric ( collpair->pa,
709 verts1[collpair->ap1].txold,
710 verts1[collpair->ap2].txold,
711 verts1[collpair->ap3].txold,
715 collision_compute_barycentric ( collpair->pb,
716 collmd->current_x[collpair->bp1].co,
717 collmd->current_x[collpair->bp2].co,
718 collmd->current_x[collpair->bp3].co,
721 // Calculate relative "velocity".
722 collision_interpolateOnTriangle ( v1, verts1[collpair->ap1].tv, verts1[collpair->ap2].tv, verts1[collpair->ap3].tv, w1, w2, w3 );
724 collision_interpolateOnTriangle ( v2, collmd->current_v[collpair->bp1].co, collmd->current_v[collpair->bp2].co, collmd->current_v[collpair->bp3].co, u1, u2, u3 );
726 VECSUB ( relativeVelocity, v2, v1 );
728 if(sqrt(INPR(relativeVelocity, relativeVelocity)) >= distance)
730 // check for collision in the future
731 collpair->flag |= COLLISION_IN_FUTURE;
740 static int cloth_collision_response_moving( ClothModifierData *clmd, CollisionModifierData *collmd, CollPair *collpair, CollPair *collision_end )
744 float w1, w2, w3, u1, u2, u3;
745 float v1[3], v2[3], relativeVelocity[3];
748 cloth1 = clmd->clothObject;
750 for ( ; collpair != collision_end; collpair++ )
752 // compute barycentric coordinates for both collision points
753 collision_compute_barycentric ( collpair->pa,
754 cloth1->verts[collpair->ap1].txold,
755 cloth1->verts[collpair->ap2].txold,
756 cloth1->verts[collpair->ap3].txold,
760 collision_compute_barycentric ( collpair->pb,
761 collmd->current_x[collpair->bp1].co,
762 collmd->current_x[collpair->bp2].co,
763 collmd->current_x[collpair->bp3].co,
766 // Calculate relative "velocity".
767 collision_interpolateOnTriangle ( v1, cloth1->verts[collpair->ap1].tv, cloth1->verts[collpair->ap2].tv, cloth1->verts[collpair->ap3].tv, w1, w2, w3 );
769 collision_interpolateOnTriangle ( v2, collmd->current_v[collpair->bp1].co, collmd->current_v[collpair->bp2].co, collmd->current_v[collpair->bp3].co, u1, u2, u3 );
771 VECSUB ( relativeVelocity, v2, v1 );
773 // Calculate the normal component of the relative velocity (actually only the magnitude - the direction is stored in 'normal').
774 magrelVel = INPR ( relativeVelocity, collpair->normal );
776 // printf("magrelVel: %f\n", magrelVel);
778 // Calculate masses of points.
781 // If v_n_mag < 0 the edges are approaching each other.
782 if ( magrelVel > ALMOST_ZERO )
784 // Calculate Impulse magnitude to stop all motion in normal direction.
785 float magtangent = 0;
786 double impulse = 0.0;
790 // calculate tangential velocity
791 VECCOPY ( temp, collpair->normal );
792 mul_v3_fl( temp, magrelVel );
793 VECSUB ( vrel_t_pre, relativeVelocity, temp );
795 // Decrease in magnitude of relative tangential velocity due to coulomb friction
796 // in original formula "magrelVel" should be the "change of relative velocity in normal direction"
797 magtangent = MIN2 ( clmd->coll_parms->friction * 0.01 * magrelVel,sqrt ( INPR ( vrel_t_pre,vrel_t_pre ) ) );
799 // Apply friction impulse.
800 if ( magtangent > ALMOST_ZERO )
802 normalize_v3( vrel_t_pre );
804 impulse = 2.0 * magtangent / ( 1.0 + w1*w1 + w2*w2 + w3*w3 );
805 VECADDMUL ( cloth1->verts[collpair->ap1].impulse, vrel_t_pre, w1 * impulse );
806 VECADDMUL ( cloth1->verts[collpair->ap2].impulse, vrel_t_pre, w2 * impulse );
807 VECADDMUL ( cloth1->verts[collpair->ap3].impulse, vrel_t_pre, w3 * impulse );
810 // Apply velocity stopping impulse
811 // I_c = m * v_N / 2.0
812 // no 2.0 * magrelVel normally, but looks nicer DG
813 impulse = magrelVel / ( 1.0 + w1*w1 + w2*w2 + w3*w3 );
815 VECADDMUL ( cloth1->verts[collpair->ap1].impulse, collpair->normal, w1 * impulse );
816 cloth1->verts[collpair->ap1].impulse_count++;
818 VECADDMUL ( cloth1->verts[collpair->ap2].impulse, collpair->normal, w2 * impulse );
819 cloth1->verts[collpair->ap2].impulse_count++;
821 VECADDMUL ( cloth1->verts[collpair->ap3].impulse, collpair->normal, w3 * impulse );
822 cloth1->verts[collpair->ap3].impulse_count++;
824 // Apply repulse impulse if distance too short
825 // I_r = -min(dt*kd, m(0,1d/dt - v_n))
827 d = clmd->coll_parms->epsilon*8.0/9.0 + epsilon2*8.0/9.0 - collpair->distance;
828 if ( ( magrelVel < 0.1*d*clmd->sim_parms->stepsPerFrame ) && ( d > ALMOST_ZERO ) )
830 repulse = MIN2 ( d*1.0/clmd->sim_parms->stepsPerFrame, 0.1*d*clmd->sim_parms->stepsPerFrame - magrelVel );
832 // stay on the safe side and clamp repulse
833 if ( impulse > ALMOST_ZERO )
834 repulse = MIN2 ( repulse, 5.0*impulse );
835 repulse = MAX2 ( impulse, repulse );
837 impulse = repulse / ( 1.0 + w1*w1 + w2*w2 + w3*w3 ); // original 2.0 / 0.25
838 VECADDMUL ( cloth1->verts[collpair->ap1].impulse, collpair->normal, impulse );
839 VECADDMUL ( cloth1->verts[collpair->ap2].impulse, collpair->normal, impulse );
840 VECADDMUL ( cloth1->verts[collpair->ap3].impulse, collpair->normal, impulse );
851 static float projectPointOntoLine(float *p, float *a, float *b)
856 return INPR(pa, ba) / INPR(ba, ba);
859 static void calculateEENormal(float *np1, float *np2, float *np3, float *np4,float *out_normal)
861 float line1[3], line2[3];
864 VECSUB(line1, np2, np1);
865 VECSUB(line2, np3, np1);
867 // printf("l1: %f, l1: %f, l2: %f, l2: %f\n", line1[0], line1[1], line2[0], line2[1]);
869 cross_v3_v3v3(out_normal, line1, line2);
873 length = normalize_v3(out_normal);
874 if (length <= FLT_EPSILON)
875 { // lines are collinear
876 VECSUB(out_normal, np2, np1);
877 normalize_v3(out_normal);
881 static void findClosestPointsEE(float *x1, float *x2, float *x3, float *x4, float *w1, float *w2)
883 float temp[3], temp2[3];
885 double a, b, c, e, f;
887 VECSUB(temp, x2, x1);
888 a = INPR(temp, temp);
890 VECSUB(temp2, x4, x3);
891 b = -INPR(temp, temp2);
893 c = INPR(temp2, temp2);
895 VECSUB(temp2, x3, x1);
896 e = INPR(temp, temp2);
898 VECSUB(temp, x4, x3);
899 f = -INPR(temp, temp2);
901 *w1 = (e * c - b * f) / (a * c - b * b);
902 *w2 = (f - b * *w1) / c;
906 // calculates the distance of 2 edges
907 static float edgedge_distance(float np11[3], float np12[3], float np21[3], float np22[3], float *out_a1, float *out_a2, float *out_normal)
909 float line1[3], line2[3], cross[3];
911 float temp[3], temp2[3];
912 float dist_a1, dist_a2;
914 VECSUB(line1, np12, np11);
915 VECSUB(line2, np22, np21);
917 cross_v3_v3v3(cross, line1, line2);
918 length = INPR(cross, cross);
920 if (length < FLT_EPSILON)
922 *out_a2 = projectPointOntoLine(np11, np21, np22);
923 if ((*out_a2 >= -FLT_EPSILON) && (*out_a2 <= 1.0 + FLT_EPSILON))
926 calculateEENormal(np11, np12, np21, np22, out_normal);
927 VECSUB(temp, np22, np21);
928 mul_v3_fl(temp, *out_a2);
929 VECADD(temp2, temp, np21);
930 VECADD(temp2, temp2, np11);
931 return INPR(temp2, temp2);
934 CLAMP(*out_a2, 0.0, 1.0);
937 *out_a1 = projectPointOntoLine(np22, np11, np12);
938 if ((*out_a1 >= -FLT_EPSILON) && (*out_a1 <= 1.0 + FLT_EPSILON))
940 calculateEENormal(np11, np12, np21, np22, out_normal);
942 // return (np22 - (np11 + (np12 - np11) * out_a1)).lengthSquared();
943 VECSUB(temp, np12, np11);
944 mul_v3_fl(temp, *out_a1);
945 VECADD(temp2, temp, np11);
946 VECSUB(temp2, np22, temp2);
947 return INPR(temp2, temp2);
952 *out_a1 = projectPointOntoLine(np21, np11, np12);
953 if ((*out_a1 >= -FLT_EPSILON) && (*out_a1 <= 1.0 + FLT_EPSILON))
955 calculateEENormal(np11, np11, np21, np22, out_normal);
957 // return (np21 - (np11 + (np12 - np11) * out_a1)).lengthSquared();
958 VECSUB(temp, np12, np11);
959 mul_v3_fl(temp, *out_a1);
960 VECADD(temp2, temp, np11);
961 VECSUB(temp2, np21, temp2);
962 return INPR(temp2, temp2);
966 CLAMP(*out_a1, 0.0, 1.0);
967 calculateEENormal(np11, np12, np21, np22, out_normal);
972 VECSUB(temp, np12, np22);
976 VECSUB(temp, np12, np21);
983 VECSUB(temp, np11, np22);
987 VECSUB(temp, np11, np21);
991 return INPR(temp, temp);
996 // If the lines aren't parallel (but coplanar) they have to intersect
998 findClosestPointsEE(np11, np12, np21, np22, out_a1, out_a2);
1000 // If both points are on the finite edges, we're done.
1001 if (*out_a1 >= 0.0 && *out_a1 <= 1.0 && *out_a2 >= 0.0 && *out_a2 <= 1.0)
1005 // p1= np11 + (np12 - np11) * out_a1;
1006 VECSUB(temp, np12, np11);
1007 mul_v3_fl(temp, *out_a1);
1008 VECADD(p1, np11, temp);
1010 // p2 = np21 + (np22 - np21) * out_a2;
1011 VECSUB(temp, np22, np21);
1012 mul_v3_fl(temp, *out_a2);
1013 VECADD(p2, np21, temp);
1015 calculateEENormal(np11, np12, np21, np22, out_normal);
1016 VECSUB(temp, p1, p2);
1017 return INPR(temp, temp);
1022 * Clamp both points to the finite edges.
1023 * The one that moves most during clamping is one part of the solution.
1026 CLAMP(dist_a1, 0.0, 1.0);
1028 CLAMP(dist_a2, 0.0, 1.0);
1030 // Now project the "most clamped" point on the other line.
1031 if (dist_a1 > dist_a2)
1036 // p1 = np11 + (np12 - np11) * out_a1;
1037 VECSUB(temp, np12, np11);
1038 mul_v3_fl(temp, *out_a1);
1039 VECADD(p1, np11, temp);
1041 *out_a2 = projectPointOntoLine(p1, np21, np22);
1042 CLAMP(*out_a2, 0.0, 1.0);
1044 calculateEENormal(np11, np12, np21, np22, out_normal);
1046 // return (p1 - (np21 + (np22 - np21) * out_a2)).lengthSquared();
1047 VECSUB(temp, np22, np21);
1048 mul_v3_fl(temp, *out_a2);
1049 VECADD(temp, temp, np21);
1050 VECSUB(temp, p1, temp);
1051 return INPR(temp, temp);
1058 // p2 = np21 + (np22 - np21) * out_a2;
1059 VECSUB(temp, np22, np21);
1060 mul_v3_fl(temp, *out_a2);
1061 VECADD(p2, np21, temp);
1063 *out_a1 = projectPointOntoLine(p2, np11, np12);
1064 CLAMP(*out_a1, 0.0, 1.0);
1066 calculateEENormal(np11, np12, np21, np22, out_normal);
1068 // return ((np11 + (np12 - np11) * out_a1) - p2).lengthSquared();
1069 VECSUB(temp, np12, np11);
1070 mul_v3_fl(temp, *out_a1);
1071 VECADD(temp, temp, np11);
1072 VECSUB(temp, temp, p2);
1073 return INPR(temp, temp);
1077 printf("Error in edgedge_distance: end of function\n");
1081 static int cloth_collision_moving_edges ( ClothModifierData *clmd, CollisionModifierData *collmd, CollPair *collpair )
1083 EdgeCollPair edgecollpair;
1085 ClothVertex *verts1=NULL;
1086 unsigned int i = 0, k = 0;
1087 int numsolutions = 0;
1088 double x1[3], v1[3], x2[3], v2[3], x3[3], v3[3];
1089 double solution[3], solution2[3];
1090 MVert *verts2 = collmd->current_x; // old x
1091 MVert *velocity2 = collmd->current_v; // velocity
1093 float triA[3][3], triB[3][3];
1096 cloth1 = clmd->clothObject;
1097 verts1 = cloth1->verts;
1099 for(i = 0; i < 9; i++)
1101 // 9 edge - edge possibilities
1103 if(i == 0) // cloth edge: 1-2; coll edge: 1-2
1105 edgecollpair.p11 = collpair->ap1;
1106 edgecollpair.p12 = collpair->ap2;
1108 edgecollpair.p21 = collpair->bp1;
1109 edgecollpair.p22 = collpair->bp2;
1111 else if(i == 1) // cloth edge: 1-2; coll edge: 2-3
1113 edgecollpair.p11 = collpair->ap1;
1114 edgecollpair.p12 = collpair->ap2;
1116 edgecollpair.p21 = collpair->bp2;
1117 edgecollpair.p22 = collpair->bp3;
1119 else if(i == 2) // cloth edge: 1-2; coll edge: 1-3
1121 edgecollpair.p11 = collpair->ap1;
1122 edgecollpair.p12 = collpair->ap2;
1124 edgecollpair.p21 = collpair->bp1;
1125 edgecollpair.p22 = collpair->bp3;
1127 else if(i == 3) // cloth edge: 2-3; coll edge: 1-2
1129 edgecollpair.p11 = collpair->ap2;
1130 edgecollpair.p12 = collpair->ap3;
1132 edgecollpair.p21 = collpair->bp1;
1133 edgecollpair.p22 = collpair->bp2;
1135 else if(i == 4) // cloth edge: 2-3; coll edge: 2-3
1137 edgecollpair.p11 = collpair->ap2;
1138 edgecollpair.p12 = collpair->ap3;
1140 edgecollpair.p21 = collpair->bp2;
1141 edgecollpair.p22 = collpair->bp3;
1143 else if(i == 5) // cloth edge: 2-3; coll edge: 1-3
1145 edgecollpair.p11 = collpair->ap2;
1146 edgecollpair.p12 = collpair->ap3;
1148 edgecollpair.p21 = collpair->bp1;
1149 edgecollpair.p22 = collpair->bp3;
1151 else if(i ==6) // cloth edge: 1-3; coll edge: 1-2
1153 edgecollpair.p11 = collpair->ap1;
1154 edgecollpair.p12 = collpair->ap3;
1156 edgecollpair.p21 = collpair->bp1;
1157 edgecollpair.p22 = collpair->bp2;
1159 else if(i ==7) // cloth edge: 1-3; coll edge: 2-3
1161 edgecollpair.p11 = collpair->ap1;
1162 edgecollpair.p12 = collpair->ap3;
1164 edgecollpair.p21 = collpair->bp2;
1165 edgecollpair.p22 = collpair->bp3;
1167 else if(i == 8) // cloth edge: 1-3; coll edge: 1-3
1169 edgecollpair.p11 = collpair->ap1;
1170 edgecollpair.p12 = collpair->ap3;
1172 edgecollpair.p21 = collpair->bp1;
1173 edgecollpair.p22 = collpair->bp3;
1176 if((edgecollpair.p11 == 3) && (edgecollpair.p12 == 16))
1178 if((edgecollpair.p11 == 16) && (edgecollpair.p12 == 3))
1182 // if ( !cloth_are_edges_adjacent ( clmd, collmd, &edgecollpair ) )
1184 // always put coll points in p21/p22
1185 VECSUB ( x1, verts1[edgecollpair.p12].txold, verts1[edgecollpair.p11].txold );
1186 VECSUB ( v1, verts1[edgecollpair.p12].tv, verts1[edgecollpair.p11].tv );
1188 VECSUB ( x2, verts2[edgecollpair.p21].co, verts1[edgecollpair.p11].txold );
1189 VECSUB ( v2, velocity2[edgecollpair.p21].co, verts1[edgecollpair.p11].tv );
1191 VECSUB ( x3, verts2[edgecollpair.p22].co, verts1[edgecollpair.p11].txold );
1192 VECSUB ( v3, velocity2[edgecollpair.p22].co, verts1[edgecollpair.p11].tv );
1194 numsolutions = cloth_get_collision_time ( x1, v1, x2, v2, x3, v3, solution );
1196 if((edgecollpair.p11 == 3 && edgecollpair.p12==16)|| (edgecollpair.p11==16 && edgecollpair.p12==3))
1198 if(edgecollpair.p21==6 || edgecollpair.p22 == 6)
1200 printf("dist: %f, sol[k]: %lf, sol2[k]: %lf\n", distance, solution[k], solution2[k]);
1201 printf("a1: %f, a2: %f, b1: %f, b2: %f\n", x1[0], x2[0], x3[0], v1[0]);
1202 printf("b21: %d, b22: %d\n", edgecollpair.p21, edgecollpair.p22);
1206 for ( k = 0; k < numsolutions; k++ )
1208 // printf("sol %d: %lf\n", k, solution[k]);
1209 if ( ( solution[k] >= ALMOST_ZERO ) && ( solution[k] <= 1.0 ) && ( solution[k] > ALMOST_ZERO))
1212 float out_normal[3];
1218 VECADDS(triA[0], verts1[edgecollpair.p11].txold, verts1[edgecollpair.p11].tv, solution[k]);
1219 VECADDS(triA[1], verts1[edgecollpair.p12].txold, verts1[edgecollpair.p12].tv, solution[k]);
1221 VECADDS(triB[0], collmd->current_x[edgecollpair.p21].co, collmd->current_v[edgecollpair.p21].co, solution[k]);
1222 VECADDS(triB[1], collmd->current_x[edgecollpair.p22].co, collmd->current_v[edgecollpair.p22].co, solution[k]);
1224 // TODO: check for collisions
1225 distance = edgedge_distance(triA[0], triA[1], triB[0], triB[1], &a, &b, out_normal);
1227 if ((distance <= clmd->coll_parms->epsilon + BLI_bvhtree_getepsilon ( collmd->bvhtree ) + ALMOST_ZERO) && (INPR(out_normal, out_normal) > 0))
1229 float vrel_1_to_2[3], temp[3], temp2[3], out_normalVelocity;
1232 VECCOPY(vrel_1_to_2, verts1[edgecollpair.p11].tv);
1233 mul_v3_fl(vrel_1_to_2, 1.0 - a);
1234 VECCOPY(temp, verts1[edgecollpair.p12].tv);
1237 VECADD(vrel_1_to_2, vrel_1_to_2, temp);
1239 VECCOPY(temp, verts1[edgecollpair.p21].tv);
1240 mul_v3_fl(temp, 1.0 - b);
1241 VECCOPY(temp2, verts1[edgecollpair.p22].tv);
1242 mul_v3_fl(temp2, b);
1243 VECADD(temp, temp, temp2);
1245 VECSUB(vrel_1_to_2, vrel_1_to_2, temp);
1247 out_normalVelocity = INPR(vrel_1_to_2, out_normal);
1249 // this correction results in wrong normals sometimes?
1250 if(out_normalVelocity < 0.0)
1252 out_normalVelocity*= -1.0;
1253 negate_v3(out_normal);
1256 /* Inelastic repulsion impulse. */
1258 // Calculate which normal velocity we need.
1259 desiredVn = (out_normalVelocity * (float)solution[k] - (.1 * (clmd->coll_parms->epsilon + BLI_bvhtree_getepsilon ( collmd->bvhtree )) - sqrt(distance)) - ALMOST_ZERO);
1261 // Now calculate what impulse we need to reach that velocity.
1262 I_mag = (out_normalVelocity - desiredVn) / 2.0; // / (1/m1 + 1/m2);
1264 // Finally apply that impulse.
1265 impulse = (2.0 * -I_mag) / (a*a + (1.0-a)*(1.0-a) + b*b + (1.0-b)*(1.0-b));
1267 VECADDMUL ( verts1[edgecollpair.p11].impulse, out_normal, (1.0-a) * impulse );
1268 verts1[edgecollpair.p11].impulse_count++;
1270 VECADDMUL ( verts1[edgecollpair.p12].impulse, out_normal, a * impulse );
1271 verts1[edgecollpair.p12].impulse_count++;
1279 // missing from collision.hpp
1281 // mintime = MIN2(mintime, (float)solution[k]);
1291 static int cloth_collision_moving ( ClothModifierData *clmd, CollisionModifierData *collmd, CollPair *collpair, CollPair *collision_end )
1294 cloth1 = clmd->clothObject;
1296 for ( ; collpair != collision_end; collpair++ )
1298 // only handle moving collisions here
1299 if (!( collpair->flag & COLLISION_IN_FUTURE ))
1302 cloth_collision_moving_edges ( clmd, collmd, collpair);
1303 // cloth_collision_moving_tris ( clmd, collmd, collpair);
1310 static void add_collision_object(Object ***objs, int *numobj, int *maxobj, Object *ob, Object *self, int level)
1312 CollisionModifierData *cmd= NULL;
1317 /* only get objects with collision modifier */
1318 if(ob->pd && ob->pd->deflect)
1319 cmd= (CollisionModifierData *)modifiers_findByType(ob, eModifierType_Collision);
1323 if(*numobj >= *maxobj) {
1325 *objs= MEM_reallocN(*objs, sizeof(Object*)*(*maxobj));
1328 (*objs)[*numobj] = ob;
1332 /* objects in dupli groups, one level only for now */
1333 if(ob->dup_group && level == 0) {
1335 Group *group= ob->dup_group;
1338 for(go= group->gobject.first; go; go= go->next)
1339 add_collision_object(objs, numobj, maxobj, go->ob, self, level+1);
1343 // return all collision objects in scene
1344 // collision object will exclude self
1345 Object **get_collisionobjects(Scene *scene, Object *self, Group *group, unsigned int *numcollobj)
1350 unsigned int numobj= 0, maxobj= 100;
1352 objs= MEM_callocN(sizeof(Object *)*maxobj, "CollisionObjectsArray");
1354 /* gather all collision objects */
1356 /* use specified group */
1357 for(go= group->gobject.first; go; go= go->next)
1358 add_collision_object(&objs, &numobj, &maxobj, go->ob, self, 0);
1361 Scene *sce; /* for SETLOOPER macro */
1362 /* add objects in same layer in scene */
1363 for(SETLOOPER(scene, base)) {
1364 if(base->lay & self->lay)
1365 add_collision_object(&objs, &numobj, &maxobj, base->object, self, 0);
1370 *numcollobj= numobj;
1375 static void add_collider_cache_object(ListBase **objs, Object *ob, Object *self, int level)
1377 CollisionModifierData *cmd= NULL;
1383 if(ob->pd && ob->pd->deflect)
1384 cmd =(CollisionModifierData *)modifiers_findByType(ob, eModifierType_Collision);
1386 if(cmd && cmd->bvhtree) {
1388 *objs = MEM_callocN(sizeof(ListBase), "ColliderCache array");
1390 col = MEM_callocN(sizeof(ColliderCache), "ColliderCache");
1393 /* make sure collider is properly set up */
1394 collision_move_object(cmd, 1.0, 0.0);
1395 BLI_addtail(*objs, col);
1398 /* objects in dupli groups, one level only for now */
1399 if(ob->dup_group && level == 0) {
1401 Group *group= ob->dup_group;
1404 for(go= group->gobject.first; go; go= go->next)
1405 add_collider_cache_object(objs, go->ob, self, level+1);
1409 ListBase *get_collider_cache(Scene *scene, Object *self, Group *group)
1412 ListBase *objs= NULL;
1414 /* add object in same layer in scene */
1416 for(go= group->gobject.first; go; go= go->next)
1417 add_collider_cache_object(&objs, go->ob, self, 0);
1420 Scene *sce; /* for SETLOOPER macro */
1423 /* add objects in same layer in scene */
1424 for(SETLOOPER(scene, base)) {
1425 if(!self || (base->lay & self->lay))
1426 add_collider_cache_object(&objs, base->object, self, 0);
1434 void free_collider_cache(ListBase **colliders)
1437 BLI_freelistN(*colliders);
1438 MEM_freeN(*colliders);
1443 static void cloth_bvh_objcollisions_nearcheck ( ClothModifierData * clmd, CollisionModifierData *collmd, CollPair **collisions, CollPair **collisions_index, int numresult, BVHTreeOverlap *overlap)
1447 *collisions = ( CollPair* ) MEM_mallocN ( sizeof ( CollPair ) * numresult * 4, "collision array" ); //*4 since cloth_collision_static can return more than 1 collision
1448 *collisions_index = *collisions;
1450 for ( i = 0; i < numresult; i++ )
1452 *collisions_index = cloth_collision ( ( ModifierData * ) clmd, ( ModifierData * ) collmd, overlap+i, *collisions_index );
1456 static int cloth_bvh_objcollisions_resolve ( ClothModifierData * clmd, CollisionModifierData *collmd, CollPair *collisions, CollPair *collisions_index)
1458 Cloth *cloth = clmd->clothObject;
1459 int i=0, j = 0, numfaces = 0, numverts = 0;
1460 ClothVertex *verts = NULL;
1463 float tnull[3] = {0,0,0};
1465 numfaces = clmd->clothObject->numfaces;
1466 numverts = clmd->clothObject->numverts;
1468 verts = cloth->verts;
1470 // process all collisions (calculate impulses, TODO: also repulses if distance too short)
1472 for ( j = 0; j < 5; j++ ) // 5 is just a value that ensures convergence
1476 if ( collmd->bvhtree )
1478 result += cloth_collision_response_static ( clmd, collmd, collisions, collisions_index );
1480 // apply impulses in parallel
1483 for ( i = 0; i < numverts; i++ )
1485 // calculate "velocities" (just xnew = xold + v; no dt in v)
1486 if ( verts[i].impulse_count )
1488 VECADDMUL ( verts[i].tv, verts[i].impulse, 1.0f / verts[i].impulse_count );
1489 VECCOPY ( verts[i].impulse, tnull );
1490 verts[i].impulse_count = 0;
1501 // cloth - object collisions
1502 int cloth_bvh_objcollision (Object *ob, ClothModifierData * clmd, float step, float dt )
1504 Cloth *cloth= clmd->clothObject;
1505 BVHTree *cloth_bvh= cloth->bvhtree;
1506 unsigned int i=0, numfaces = 0, numverts = 0, k, l, j;
1507 int rounds = 0; // result counts applied collisions; ic is for debug output;
1508 ClothVertex *verts = NULL;
1509 int ret = 0, ret2 = 0;
1510 Object **collobjs = NULL;
1511 unsigned int numcollobj = 0;
1513 if ((clmd->sim_parms->flags & CLOTH_SIMSETTINGS_FLAG_COLLOBJ) || cloth_bvh==NULL)
1516 verts = cloth->verts;
1517 numfaces = cloth->numfaces;
1518 numverts = cloth->numverts;
1520 ////////////////////////////////////////////////////////////
1521 // static collisions
1522 ////////////////////////////////////////////////////////////
1525 bvhtree_update_from_cloth ( clmd, 1 ); // 0 means STATIC, 1 means MOVING (see later in this function)
1526 bvhselftree_update_from_cloth ( clmd, 0 ); // 0 means STATIC, 1 means MOVING (see later in this function)
1528 collobjs = get_collisionobjects(clmd->scene, ob, clmd->coll_parms->group, &numcollobj);
1535 CollPair **collisions, **collisions_index;
1539 collisions = MEM_callocN(sizeof(CollPair *) *numcollobj , "CollPair");
1540 collisions_index = MEM_callocN(sizeof(CollPair *) *numcollobj , "CollPair");
1542 // check all collision objects
1543 for(i = 0; i < numcollobj; i++)
1545 Object *collob= collobjs[i];
1546 CollisionModifierData *collmd = (CollisionModifierData*)modifiers_findByType(collob, eModifierType_Collision);
1547 BVHTreeOverlap *overlap = NULL;
1550 if(!collmd->bvhtree)
1553 /* move object to position (step) in time */
1554 collision_move_object ( collmd, step + dt, step );
1556 /* search for overlapping collision pairs */
1557 overlap = BLI_bvhtree_overlap ( cloth_bvh, collmd->bvhtree, &result );
1559 // go to next object if no overlap is there
1560 if( result && overlap ) {
1561 /* check if collisions really happen (costly near check) */
1562 cloth_bvh_objcollisions_nearcheck ( clmd, collmd, &collisions[i], &collisions_index[i], result, overlap);
1564 // resolve nearby collisions
1565 ret += cloth_bvh_objcollisions_resolve ( clmd, collmd, collisions[i], collisions_index[i]);
1570 MEM_freeN ( overlap );
1574 for(i = 0; i < numcollobj; i++)
1576 if ( collisions[i] ) MEM_freeN ( collisions[i] );
1579 MEM_freeN(collisions);
1580 MEM_freeN(collisions_index);
1582 ////////////////////////////////////////////////////////////
1584 // this is needed for bvh_calc_DOP_hull_moving() [kdop.c]
1585 ////////////////////////////////////////////////////////////
1587 // verts come from clmd
1588 for ( i = 0; i < numverts; i++ )
1590 if ( clmd->sim_parms->flags & CLOTH_SIMSETTINGS_FLAG_GOAL )
1592 if ( verts [i].flags & CLOTH_VERT_FLAG_PINNED )
1598 VECADD ( verts[i].tx, verts[i].txold, verts[i].tv );
1600 ////////////////////////////////////////////////////////////
1603 ////////////////////////////////////////////////////////////
1604 // Test on *simple* selfcollisions
1605 ////////////////////////////////////////////////////////////
1606 if ( clmd->coll_parms->flags & CLOTH_COLLSETTINGS_FLAG_SELF )
1608 for(l = 0; l < (unsigned int)clmd->coll_parms->self_loop_count; l++)
1610 // TODO: add coll quality rounds again
1611 BVHTreeOverlap *overlap = NULL;
1612 unsigned int result = 0;
1615 verts = cloth->verts; // needed for openMP
1617 numfaces = cloth->numfaces;
1618 numverts = cloth->numverts;
1620 verts = cloth->verts;
1622 if ( cloth->bvhselftree )
1624 // search for overlapping collision pairs
1625 overlap = BLI_bvhtree_overlap ( cloth->bvhselftree, cloth->bvhselftree, &result );
1627 // #pragma omp parallel for private(k, i, j) schedule(static)
1628 for ( k = 0; k < result; k++ )
1634 i = overlap[k].indexA;
1635 j = overlap[k].indexB;
1637 mindistance = clmd->coll_parms->selfepsilon* ( cloth->verts[i].avg_spring_len + cloth->verts[j].avg_spring_len );
1639 if ( clmd->sim_parms->flags & CLOTH_SIMSETTINGS_FLAG_GOAL )
1641 if ( ( cloth->verts [i].flags & CLOTH_VERT_FLAG_PINNED )
1642 && ( cloth->verts [j].flags & CLOTH_VERT_FLAG_PINNED ) )
1648 VECSUB ( temp, verts[i].tx, verts[j].tx );
1650 if ( ( ABS ( temp[0] ) > mindistance ) || ( ABS ( temp[1] ) > mindistance ) || ( ABS ( temp[2] ) > mindistance ) ) continue;
1652 // check for adjacent points (i must be smaller j)
1653 if ( BLI_edgehash_haskey ( cloth->edgehash, MIN2(i, j), MAX2(i, j) ) )
1658 length = normalize_v3( temp );
1660 if ( length < mindistance )
1662 float correction = mindistance - length;
1664 if ( cloth->verts [i].flags & CLOTH_VERT_FLAG_PINNED )
1666 mul_v3_fl( temp, -correction );
1667 VECADD ( verts[j].tx, verts[j].tx, temp );
1669 else if ( cloth->verts [j].flags & CLOTH_VERT_FLAG_PINNED )
1671 mul_v3_fl( temp, correction );
1672 VECADD ( verts[i].tx, verts[i].tx, temp );
1676 mul_v3_fl( temp, -correction*0.5 );
1677 VECADD ( verts[j].tx, verts[j].tx, temp );
1679 VECSUB ( verts[i].tx, verts[i].tx, temp );
1686 // check for approximated time collisions
1691 MEM_freeN ( overlap );
1695 ////////////////////////////////////////////////////////////
1697 ////////////////////////////////////////////////////////////
1698 // SELFCOLLISIONS: update velocities
1699 ////////////////////////////////////////////////////////////
1702 for ( i = 0; i < cloth->numverts; i++ )
1704 if ( ! ( verts [i].flags & CLOTH_VERT_FLAG_PINNED ) )
1706 VECSUB ( verts[i].tv, verts[i].tx, verts[i].txold );
1710 ////////////////////////////////////////////////////////////
1713 while ( ret2 && ( clmd->coll_parms->loop_count>rounds ) );
1716 MEM_freeN(collobjs);
1718 return MIN2 ( ret, 1 );