|
Blender
V2.59
|
00001 /* 00002 * $Id: collision.c 36423 2011-05-02 03:44:02Z campbellbarton $ 00003 * 00004 * ***** BEGIN GPL LICENSE BLOCK ***** 00005 * 00006 * This program is free software; you can redistribute it and/or 00007 * modify it under the terms of the GNU General Public License 00008 * as published by the Free Software Foundation; either version 2 00009 * of the License, or (at your option) any later version. 00010 * 00011 * This program is distributed in the hope that it will be useful, 00012 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00013 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00014 * GNU General Public License for more details. 00015 * 00016 * You should have received a copy of the GNU General Public License 00017 * along with this program; if not, write to the Free Software Foundation, 00018 * Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. 00019 * 00020 * The Original Code is Copyright (C) Blender Foundation 00021 * All rights reserved. 00022 * 00023 * The Original Code is: all of this file. 00024 * 00025 * Contributor(s): none yet. 00026 * 00027 * ***** END GPL LICENSE BLOCK ***** 00028 */ 00029 00035 #include "MEM_guardedalloc.h" 00036 00037 #include "BKE_cloth.h" 00038 00039 #include "DNA_cloth_types.h" 00040 #include "DNA_group_types.h" 00041 #include "DNA_mesh_types.h" 00042 #include "DNA_object_types.h" 00043 #include "DNA_object_force.h" 00044 #include "DNA_scene_types.h" 00045 #include "DNA_meshdata_types.h" 00046 00047 #include "BLI_blenlib.h" 00048 #include "BLI_math.h" 00049 #include "BLI_edgehash.h" 00050 #include "BLI_utildefines.h" 00051 #include "BLI_ghash.h" 00052 #include "BLI_memarena.h" 00053 #include "BLI_rand.h" 00054 00055 #include "BKE_DerivedMesh.h" 00056 #include "BKE_global.h" 00057 #include "BKE_scene.h" 00058 #include "BKE_mesh.h" 00059 #include "BKE_object.h" 00060 #include "BKE_modifier.h" 00061 00062 #include "BKE_DerivedMesh.h" 00063 #ifdef USE_BULLET 00064 #include "Bullet-C-Api.h" 00065 #endif 00066 #include "BLI_kdopbvh.h" 00067 #include "BKE_collision.h" 00068 00069 #ifdef WITH_ELTOPO 00070 #include "eltopo-capi.h" 00071 #endif 00072 00073 00074 /*********************************** 00075 Collision modifier code start 00076 ***********************************/ 00077 00078 /* step is limited from 0 (frame start position) to 1 (frame end position) */ 00079 void collision_move_object ( CollisionModifierData *collmd, float step, float prevstep ) 00080 { 00081 float tv[3] = {0, 0, 0}; 00082 unsigned int i = 0; 00083 00084 for ( i = 0; i < collmd->numverts; i++ ) 00085 { 00086 VECSUB ( tv, collmd->xnew[i].co, collmd->x[i].co ); 00087 VECADDS ( collmd->current_x[i].co, collmd->x[i].co, tv, prevstep ); 00088 VECADDS ( collmd->current_xnew[i].co, collmd->x[i].co, tv, step ); 00089 VECSUB ( collmd->current_v[i].co, collmd->current_xnew[i].co, collmd->current_x[i].co ); 00090 } 00091 00092 bvhtree_update_from_mvert ( collmd->bvhtree, collmd->mfaces, collmd->numfaces, collmd->current_x, collmd->current_xnew, collmd->numverts, 1 ); 00093 } 00094 00095 BVHTree *bvhtree_build_from_mvert ( MFace *mfaces, unsigned int numfaces, MVert *x, unsigned int UNUSED(numverts), float epsilon ) 00096 { 00097 BVHTree *tree; 00098 float co[12]; 00099 unsigned int i; 00100 MFace *tface = mfaces; 00101 00102 tree = BLI_bvhtree_new ( numfaces*2, epsilon, 4, 26 ); 00103 00104 // fill tree 00105 for ( i = 0; i < numfaces; i++, tface++ ) 00106 { 00107 VECCOPY ( &co[0*3], x[tface->v1].co ); 00108 VECCOPY ( &co[1*3], x[tface->v2].co ); 00109 VECCOPY ( &co[2*3], x[tface->v3].co ); 00110 if ( tface->v4 ) 00111 VECCOPY ( &co[3*3], x[tface->v4].co ); 00112 00113 BLI_bvhtree_insert ( tree, i, co, ( mfaces->v4 ? 4 : 3 ) ); 00114 } 00115 00116 // balance tree 00117 BLI_bvhtree_balance ( tree ); 00118 00119 return tree; 00120 } 00121 00122 void bvhtree_update_from_mvert ( BVHTree * bvhtree, MFace *faces, int numfaces, MVert *x, MVert *xnew, int UNUSED(numverts), int moving ) 00123 { 00124 int i; 00125 MFace *mfaces = faces; 00126 float co[12], co_moving[12]; 00127 int ret = 0; 00128 00129 if ( !bvhtree ) 00130 return; 00131 00132 if ( x ) 00133 { 00134 for ( i = 0; i < numfaces; i++, mfaces++ ) 00135 { 00136 VECCOPY ( &co[0*3], x[mfaces->v1].co ); 00137 VECCOPY ( &co[1*3], x[mfaces->v2].co ); 00138 VECCOPY ( &co[2*3], x[mfaces->v3].co ); 00139 if ( mfaces->v4 ) 00140 VECCOPY ( &co[3*3], x[mfaces->v4].co ); 00141 00142 // copy new locations into array 00143 if ( moving && xnew ) 00144 { 00145 // update moving positions 00146 VECCOPY ( &co_moving[0*3], xnew[mfaces->v1].co ); 00147 VECCOPY ( &co_moving[1*3], xnew[mfaces->v2].co ); 00148 VECCOPY ( &co_moving[2*3], xnew[mfaces->v3].co ); 00149 if ( mfaces->v4 ) 00150 VECCOPY ( &co_moving[3*3], xnew[mfaces->v4].co ); 00151 00152 ret = BLI_bvhtree_update_node ( bvhtree, i, co, co_moving, ( mfaces->v4 ? 4 : 3 ) ); 00153 } 00154 else 00155 { 00156 ret = BLI_bvhtree_update_node ( bvhtree, i, co, NULL, ( mfaces->v4 ? 4 : 3 ) ); 00157 } 00158 00159 // check if tree is already full 00160 if ( !ret ) 00161 break; 00162 } 00163 00164 BLI_bvhtree_update_tree ( bvhtree ); 00165 } 00166 } 00167 00168 /*********************************** 00169 Collision modifier code end 00170 ***********************************/ 00171 00178 #define mySWAP(a,b) do { double tmp = b ; b = a ; a = tmp ; } while(0) 00179 #if 0 /* UNUSED */ 00180 static int 00181 gsl_poly_solve_cubic (double a, double b, double c, 00182 double *x0, double *x1, double *x2) 00183 { 00184 double q = (a * a - 3 * b); 00185 double r = (2 * a * a * a - 9 * a * b + 27 * c); 00186 00187 double Q = q / 9; 00188 double R = r / 54; 00189 00190 double Q3 = Q * Q * Q; 00191 double R2 = R * R; 00192 00193 double CR2 = 729 * r * r; 00194 double CQ3 = 2916 * q * q * q; 00195 00196 if (R == 0 && Q == 0) 00197 { 00198 *x0 = - a / 3 ; 00199 *x1 = - a / 3 ; 00200 *x2 = - a / 3 ; 00201 return 3 ; 00202 } 00203 else if (CR2 == CQ3) 00204 { 00205 /* this test is actually R2 == Q3, written in a form suitable 00206 for exact computation with integers */ 00207 00208 /* Due to finite precision some double roots may be missed, and 00209 considered to be a pair of complex roots z = x +/- epsilon i 00210 close to the real axis. */ 00211 00212 double sqrtQ = sqrt (Q); 00213 00214 if (R > 0) 00215 { 00216 *x0 = -2 * sqrtQ - a / 3; 00217 *x1 = sqrtQ - a / 3; 00218 *x2 = sqrtQ - a / 3; 00219 } 00220 else 00221 { 00222 *x0 = - sqrtQ - a / 3; 00223 *x1 = - sqrtQ - a / 3; 00224 *x2 = 2 * sqrtQ - a / 3; 00225 } 00226 return 3 ; 00227 } 00228 else if (CR2 < CQ3) /* equivalent to R2 < Q3 */ 00229 { 00230 double sqrtQ = sqrt (Q); 00231 double sqrtQ3 = sqrtQ * sqrtQ * sqrtQ; 00232 double theta = acos (R / sqrtQ3); 00233 double norm = -2 * sqrtQ; 00234 *x0 = norm * cos (theta / 3) - a / 3; 00235 *x1 = norm * cos ((theta + 2.0 * M_PI) / 3) - a / 3; 00236 *x2 = norm * cos ((theta - 2.0 * M_PI) / 3) - a / 3; 00237 00238 /* Sort *x0, *x1, *x2 into increasing order */ 00239 00240 if (*x0 > *x1) 00241 mySWAP(*x0, *x1) ; 00242 00243 if (*x1 > *x2) 00244 { 00245 mySWAP(*x1, *x2) ; 00246 00247 if (*x0 > *x1) 00248 mySWAP(*x0, *x1) ; 00249 } 00250 00251 return 3; 00252 } 00253 else 00254 { 00255 double sgnR = (R >= 0 ? 1 : -1); 00256 double A = -sgnR * pow (fabs (R) + sqrt (R2 - Q3), 1.0/3.0); 00257 double B = Q / A ; 00258 *x0 = A + B - a / 3; 00259 return 1; 00260 } 00261 } 00262 00263 00264 00270 static int 00271 gsl_poly_solve_quadratic (double a, double b, double c, 00272 double *x0, double *x1) 00273 { 00274 double disc = b * b - 4 * a * c; 00275 00276 if (a == 0) /* Handle linear case */ 00277 { 00278 if (b == 0) 00279 { 00280 return 0; 00281 } 00282 else 00283 { 00284 *x0 = -c / b; 00285 return 1; 00286 }; 00287 } 00288 00289 if (disc > 0) 00290 { 00291 if (b == 0) 00292 { 00293 double r = fabs (0.5 * sqrt (disc) / a); 00294 *x0 = -r; 00295 *x1 = r; 00296 } 00297 else 00298 { 00299 double sgnb = (b > 0 ? 1 : -1); 00300 double temp = -0.5 * (b + sgnb * sqrt (disc)); 00301 double r1 = temp / a ; 00302 double r2 = c / temp ; 00303 00304 if (r1 < r2) 00305 { 00306 *x0 = r1 ; 00307 *x1 = r2 ; 00308 } 00309 else 00310 { 00311 *x0 = r2 ; 00312 *x1 = r1 ; 00313 } 00314 } 00315 return 2; 00316 } 00317 else if (disc == 0) 00318 { 00319 *x0 = -0.5 * b / a ; 00320 *x1 = -0.5 * b / a ; 00321 return 2 ; 00322 } 00323 else 00324 { 00325 return 0; 00326 } 00327 } 00328 #endif /* UNUSED */ 00329 00330 00331 00332 /* 00333 * See Bridson et al. "Robust Treatment of Collision, Contact and Friction for Cloth Animation" 00334 * page 4, left column 00335 */ 00336 #if 0 00337 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] ) 00338 { 00339 int num_sols = 0; 00340 00341 // x^0 - checked 00342 double g = a[0] * c[1] * e[2] - a[0] * c[2] * e[1] + 00343 a[1] * c[2] * e[0] - a[1] * c[0] * e[2] + 00344 a[2] * c[0] * e[1] - a[2] * c[1] * e[0]; 00345 00346 // x^1 00347 double h = -b[2] * c[1] * e[0] + b[1] * c[2] * e[0] - a[2] * d[1] * e[0] + 00348 a[1] * d[2] * e[0] + b[2] * c[0] * e[1] - b[0] * c[2] * e[1] + 00349 a[2] * d[0] * e[1] - a[0] * d[2] * e[1] - b[1] * c[0] * e[2] + 00350 b[0] * c[1] * e[2] - a[1] * d[0] * e[2] + a[0] * d[1] * e[2] - 00351 a[2] * c[1] * f[0] + a[1] * c[2] * f[0] + a[2] * c[0] * f[1] - 00352 a[0] * c[2] * f[1] - a[1] * c[0] * f[2] + a[0] * c[1] * f[2]; 00353 00354 // x^2 00355 double i = -b[2] * d[1] * e[0] + b[1] * d[2] * e[0] + 00356 b[2] * d[0] * e[1] - b[0] * d[2] * e[1] - 00357 b[1] * d[0] * e[2] + b[0] * d[1] * e[2] - 00358 b[2] * c[1] * f[0] + b[1] * c[2] * f[0] - 00359 a[2] * d[1] * f[0] + a[1] * d[2] * f[0] + 00360 b[2] * c[0] * f[1] - b[0] * c[2] * f[1] + 00361 a[2] * d[0] * f[1] - a[0] * d[2] * f[1] - 00362 b[1] * c[0] * f[2] + b[0] * c[1] * f[2] - 00363 a[1] * d[0] * f[2] + a[0] * d[1] * f[2]; 00364 00365 // x^3 - checked 00366 double j = -b[2] * d[1] * f[0] + b[1] * d[2] * f[0] + 00367 b[2] * d[0] * f[1] - b[0] * d[2] * f[1] - 00368 b[1] * d[0] * f[2] + b[0] * d[1] * f[2]; 00369 00370 /* 00371 printf("r1: %lf\n", a[0] * c[1] * e[2] - a[0] * c[2] * e[1]); 00372 printf("r2: %lf\n", a[1] * c[2] * e[0] - a[1] * c[0] * e[2]); 00373 printf("r3: %lf\n", a[2] * c[0] * e[1] - a[2] * c[1] * e[0]); 00374 00375 printf("x1 x: %f, y: %f, z: %f\n", a[0], a[1], a[2]); 00376 printf("x2 x: %f, y: %f, z: %f\n", c[0], c[1], c[2]); 00377 printf("x3 x: %f, y: %f, z: %f\n", e[0], e[1], e[2]); 00378 00379 printf("v1 x: %f, y: %f, z: %f\n", b[0], b[1], b[2]); 00380 printf("v2 x: %f, y: %f, z: %f\n", d[0], d[1], d[2]); 00381 printf("v3 x: %f, y: %f, z: %f\n", f[0], f[1], f[2]); 00382 00383 printf("t^3: %lf, t^2: %lf, t^1: %lf, t^0: %lf\n", j, i, h, g); 00384 00385 */ 00386 // Solve cubic equation to determine times t1, t2, t3, when the collision will occur. 00387 if ( ABS ( j ) > DBL_EPSILON ) 00388 { 00389 i /= j; 00390 h /= j; 00391 g /= j; 00392 num_sols = gsl_poly_solve_cubic ( i, h, g, &solution[0], &solution[1], &solution[2] ); 00393 } 00394 else 00395 { 00396 num_sols = gsl_poly_solve_quadratic ( i, h, g, &solution[0], &solution[1] ); 00397 solution[2] = -1.0; 00398 } 00399 00400 // printf("num_sols: %d, sol1: %lf, sol2: %lf, sol3: %lf\n", num_sols, solution[0], solution[1], solution[2]); 00401 00402 // Discard negative solutions 00403 if ( ( num_sols >= 1 ) && ( solution[0] < DBL_EPSILON ) ) 00404 { 00405 --num_sols; 00406 solution[0] = solution[num_sols]; 00407 } 00408 if ( ( num_sols >= 2 ) && ( solution[1] < DBL_EPSILON ) ) 00409 { 00410 --num_sols; 00411 solution[1] = solution[num_sols]; 00412 } 00413 if ( ( num_sols == 3 ) && ( solution[2] < DBL_EPSILON ) ) 00414 { 00415 --num_sols; 00416 } 00417 00418 // Sort 00419 if ( num_sols == 2 ) 00420 { 00421 if ( solution[0] > solution[1] ) 00422 { 00423 double tmp = solution[0]; 00424 solution[0] = solution[1]; 00425 solution[1] = tmp; 00426 } 00427 } 00428 else if ( num_sols == 3 ) 00429 { 00430 00431 // Bubblesort 00432 if ( solution[0] > solution[1] ) 00433 { 00434 double tmp = solution[0]; solution[0] = solution[1]; solution[1] = tmp; 00435 } 00436 if ( solution[1] > solution[2] ) 00437 { 00438 double tmp = solution[1]; solution[1] = solution[2]; solution[2] = tmp; 00439 } 00440 if ( solution[0] > solution[1] ) 00441 { 00442 double tmp = solution[0]; solution[0] = solution[1]; solution[1] = tmp; 00443 } 00444 } 00445 00446 return num_sols; 00447 } 00448 #endif 00449 00450 00451 // w3 is not perfect 00452 static void collision_compute_barycentric ( float pv[3], float p1[3], float p2[3], float p3[3], float *w1, float *w2, float *w3 ) 00453 { 00454 double tempV1[3], tempV2[3], tempV4[3]; 00455 double a,b,c,d,e,f; 00456 00457 VECSUB ( tempV1, p1, p3 ); 00458 VECSUB ( tempV2, p2, p3 ); 00459 VECSUB ( tempV4, pv, p3 ); 00460 00461 a = INPR ( tempV1, tempV1 ); 00462 b = INPR ( tempV1, tempV2 ); 00463 c = INPR ( tempV2, tempV2 ); 00464 e = INPR ( tempV1, tempV4 ); 00465 f = INPR ( tempV2, tempV4 ); 00466 00467 d = ( a * c - b * b ); 00468 00469 if ( ABS ( d ) < ALMOST_ZERO ) 00470 { 00471 *w1 = *w2 = *w3 = 1.0 / 3.0; 00472 return; 00473 } 00474 00475 w1[0] = ( float ) ( ( e * c - b * f ) / d ); 00476 00477 if ( w1[0] < 0 ) 00478 w1[0] = 0; 00479 00480 w2[0] = ( float ) ( ( f - b * ( double ) w1[0] ) / c ); 00481 00482 if ( w2[0] < 0 ) 00483 w2[0] = 0; 00484 00485 w3[0] = 1.0f - w1[0] - w2[0]; 00486 } 00487 00488 DO_INLINE void collision_interpolateOnTriangle ( float to[3], float v1[3], float v2[3], float v3[3], double w1, double w2, double w3 ) 00489 { 00490 to[0] = to[1] = to[2] = 0; 00491 VECADDMUL ( to, v1, w1 ); 00492 VECADDMUL ( to, v2, w2 ); 00493 VECADDMUL ( to, v3, w3 ); 00494 } 00495 00496 #ifndef WITH_ELTOPO 00497 static int cloth_collision_response_static ( ClothModifierData *clmd, CollisionModifierData *collmd, CollPair *collpair, CollPair *collision_end ) 00498 { 00499 int result = 0; 00500 Cloth *cloth1; 00501 float w1, w2, w3, u1, u2, u3; 00502 float v1[3], v2[3], relativeVelocity[3]; 00503 float magrelVel; 00504 float epsilon2 = BLI_bvhtree_getepsilon ( collmd->bvhtree ); 00505 00506 cloth1 = clmd->clothObject; 00507 00508 for ( ; collpair != collision_end; collpair++ ) 00509 { 00510 // only handle static collisions here 00511 if ( collpair->flag & COLLISION_IN_FUTURE ) 00512 continue; 00513 00514 // compute barycentric coordinates for both collision points 00515 collision_compute_barycentric ( collpair->pa, 00516 cloth1->verts[collpair->ap1].txold, 00517 cloth1->verts[collpair->ap2].txold, 00518 cloth1->verts[collpair->ap3].txold, 00519 &w1, &w2, &w3 ); 00520 00521 // was: txold 00522 collision_compute_barycentric ( collpair->pb, 00523 collmd->current_x[collpair->bp1].co, 00524 collmd->current_x[collpair->bp2].co, 00525 collmd->current_x[collpair->bp3].co, 00526 &u1, &u2, &u3 ); 00527 00528 // Calculate relative "velocity". 00529 collision_interpolateOnTriangle ( v1, cloth1->verts[collpair->ap1].tv, cloth1->verts[collpair->ap2].tv, cloth1->verts[collpair->ap3].tv, w1, w2, w3 ); 00530 00531 collision_interpolateOnTriangle ( v2, collmd->current_v[collpair->bp1].co, collmd->current_v[collpair->bp2].co, collmd->current_v[collpair->bp3].co, u1, u2, u3 ); 00532 00533 VECSUB ( relativeVelocity, v2, v1 ); 00534 00535 // Calculate the normal component of the relative velocity (actually only the magnitude - the direction is stored in 'normal'). 00536 magrelVel = INPR ( relativeVelocity, collpair->normal ); 00537 00538 // printf("magrelVel: %f\n", magrelVel); 00539 00540 // Calculate masses of points. 00541 // TODO 00542 00543 // If v_n_mag < 0 the edges are approaching each other. 00544 if ( magrelVel > ALMOST_ZERO ) 00545 { 00546 // Calculate Impulse magnitude to stop all motion in normal direction. 00547 float magtangent = 0, repulse = 0, d = 0; 00548 double impulse = 0.0; 00549 float vrel_t_pre[3]; 00550 float temp[3], spf; 00551 00552 // calculate tangential velocity 00553 VECCOPY ( temp, collpair->normal ); 00554 mul_v3_fl( temp, magrelVel ); 00555 VECSUB ( vrel_t_pre, relativeVelocity, temp ); 00556 00557 // Decrease in magnitude of relative tangential velocity due to coulomb friction 00558 // in original formula "magrelVel" should be the "change of relative velocity in normal direction" 00559 magtangent = MIN2 ( clmd->coll_parms->friction * 0.01 * magrelVel,sqrt ( INPR ( vrel_t_pre,vrel_t_pre ) ) ); 00560 00561 // Apply friction impulse. 00562 if ( magtangent > ALMOST_ZERO ) 00563 { 00564 normalize_v3( vrel_t_pre ); 00565 00566 impulse = magtangent / ( 1.0 + w1*w1 + w2*w2 + w3*w3 ); // 2.0 * 00567 VECADDMUL ( cloth1->verts[collpair->ap1].impulse, vrel_t_pre, w1 * impulse ); 00568 VECADDMUL ( cloth1->verts[collpair->ap2].impulse, vrel_t_pre, w2 * impulse ); 00569 VECADDMUL ( cloth1->verts[collpair->ap3].impulse, vrel_t_pre, w3 * impulse ); 00570 } 00571 00572 // Apply velocity stopping impulse 00573 // I_c = m * v_N / 2.0 00574 // no 2.0 * magrelVel normally, but looks nicer DG 00575 impulse = magrelVel / ( 1.0 + w1*w1 + w2*w2 + w3*w3 ); 00576 00577 VECADDMUL ( cloth1->verts[collpair->ap1].impulse, collpair->normal, w1 * impulse ); 00578 cloth1->verts[collpair->ap1].impulse_count++; 00579 00580 VECADDMUL ( cloth1->verts[collpair->ap2].impulse, collpair->normal, w2 * impulse ); 00581 cloth1->verts[collpair->ap2].impulse_count++; 00582 00583 VECADDMUL ( cloth1->verts[collpair->ap3].impulse, collpair->normal, w3 * impulse ); 00584 cloth1->verts[collpair->ap3].impulse_count++; 00585 00586 // Apply repulse impulse if distance too short 00587 // I_r = -min(dt*kd, m(0,1d/dt - v_n)) 00588 spf = (float)clmd->sim_parms->stepsPerFrame / clmd->sim_parms->timescale; 00589 00590 d = clmd->coll_parms->epsilon*8.0/9.0 + epsilon2*8.0/9.0 - collpair->distance; 00591 if ( ( magrelVel < 0.1*d*spf ) && ( d > ALMOST_ZERO ) ) 00592 { 00593 repulse = MIN2 ( d*1.0/spf, 0.1*d*spf - magrelVel ); 00594 00595 // stay on the safe side and clamp repulse 00596 if ( impulse > ALMOST_ZERO ) 00597 repulse = MIN2 ( repulse, 5.0*impulse ); 00598 repulse = MAX2 ( impulse, repulse ); 00599 00600 impulse = repulse / ( 1.0 + w1*w1 + w2*w2 + w3*w3 ); // original 2.0 / 0.25 00601 VECADDMUL ( cloth1->verts[collpair->ap1].impulse, collpair->normal, impulse ); 00602 VECADDMUL ( cloth1->verts[collpair->ap2].impulse, collpair->normal, impulse ); 00603 VECADDMUL ( cloth1->verts[collpair->ap3].impulse, collpair->normal, impulse ); 00604 } 00605 00606 result = 1; 00607 } 00608 } 00609 return result; 00610 } 00611 #endif /* !WITH_ELTOPO */ 00612 00613 #ifdef WITH_ELTOPO 00614 typedef struct edgepairkey { 00615 int a1, a2, b1, b2; 00616 } edgepairkey; 00617 00618 unsigned int edgepair_hash(void *vkey) 00619 { 00620 edgepairkey *key = vkey; 00621 int keys[4] = {key->a1, key->a2, key->b1, key->b2}; 00622 int i, j; 00623 00624 for (i=0; i<4; i++) { 00625 for (j=0; j<3; j++) { 00626 if (keys[j] >= keys[j+1]) { 00627 SWAP(int, keys[j], keys[j+1]); 00628 } 00629 } 00630 } 00631 00632 return keys[0]*101 + keys[1]*72 + keys[2]*53 + keys[3]*34; 00633 } 00634 00635 int edgepair_cmp(const void *va, const void *vb) 00636 { 00637 edgepairkey *a = va, *b = vb; 00638 int keysa[4] = {a->a1, a->a2, a->b1, a->b2}; 00639 int keysb[4] = {b->a1, b->a2, b->b1, b->b2}; 00640 int i; 00641 00642 for (i=0; i<4; i++) { 00643 int j, ok=0; 00644 for (j=0; j<4; j++) { 00645 if (keysa[i] == keysa[j]) { 00646 ok = 1; 00647 break; 00648 } 00649 } 00650 if (!ok) 00651 return -1; 00652 } 00653 00654 return 0; 00655 } 00656 00657 static void get_edgepairkey(edgepairkey *key, int a1, int a2, int b1, int b2) 00658 { 00659 key->a1 = a1; 00660 key->a2 = a2; 00661 key->b1 = b1; 00662 key->b2 = b2; 00663 } 00664 00665 /*an immense amount of duplication goes on here. . .a major performance hit, I'm sure*/ 00666 static CollPair* cloth_edge_collision ( ModifierData *md1, ModifierData *md2, 00667 BVHTreeOverlap *overlap, CollPair *collpair, 00668 GHash *visithash, MemArena *arena) 00669 { 00670 ClothModifierData *clmd = ( ClothModifierData * ) md1; 00671 CollisionModifierData *collmd = ( CollisionModifierData * ) md2; 00672 MFace *face1=NULL, *face2 = NULL; 00673 ClothVertex *verts1 = clmd->clothObject->verts; 00674 double distance = 0; 00675 edgepairkey *key, tstkey; 00676 float epsilon1 = clmd->coll_parms->epsilon; 00677 float epsilon2 = BLI_bvhtree_getepsilon ( collmd->bvhtree ); 00678 float no[3], uv[3], t, relnor; 00679 int i, i1, i2, i3, i4, i5, i6; 00680 Cloth *cloth = clmd->clothObject; 00681 float n1[3], n2[3], off[3], v1[2][3], v2[2][3], v3[2][3], v4[2][3], v5[2][3], v6[2][3]; 00682 void **verts[] = {v1, v2, v3, v4, v5, v6}; 00683 int j, ret, bp1, bp2, bp3, ap1, ap2, ap3, table[6]; 00684 00685 face1 = & ( clmd->clothObject->mfaces[overlap->indexA] ); 00686 face2 = & ( collmd->mfaces[overlap->indexB] ); 00687 00688 // check all 4 possible collisions 00689 for ( i = 0; i < 4; i++ ) 00690 { 00691 if ( i == 0 ) 00692 { 00693 // fill faceA 00694 ap1 = face1->v1; 00695 ap2 = face1->v2; 00696 ap3 = face1->v3; 00697 00698 // fill faceB 00699 bp1 = face2->v1; 00700 bp2 = face2->v2; 00701 bp3 = face2->v3; 00702 } 00703 else if ( i == 1 ) 00704 { 00705 if ( face1->v4 ) 00706 { 00707 // fill faceA 00708 ap1 = face1->v1; 00709 ap2 = face1->v3; 00710 ap3 = face1->v4; 00711 00712 // fill faceB 00713 bp1 = face2->v1; 00714 bp2 = face2->v2; 00715 bp3 = face2->v3; 00716 } 00717 else { 00718 continue; 00719 } 00720 } 00721 if ( i == 2 ) 00722 { 00723 if ( face2->v4 ) 00724 { 00725 // fill faceA 00726 ap1 = face1->v1; 00727 ap2 = face1->v2; 00728 ap3 = face1->v3; 00729 00730 // fill faceB 00731 bp1 = face2->v1; 00732 bp2 = face2->v3; 00733 bp3 = face2->v4; 00734 } 00735 else { 00736 continue; 00737 } 00738 } 00739 else if ( i == 3 ) 00740 { 00741 if ( face1->v4 && face2->v4 ) 00742 { 00743 // fill faceA 00744 ap1 = face1->v1; 00745 ap2 = face1->v3; 00746 ap3 = face1->v4; 00747 00748 // fill faceB 00749 bp1 = face2->v1; 00750 bp2 = face2->v3; 00751 bp3 = face2->v4; 00752 } 00753 else { 00754 continue; 00755 } 00756 } 00757 00758 copy_v3_v3(v1[0], cloth->verts[ap1].txold); 00759 copy_v3_v3(v1[1], cloth->verts[ap1].tx); 00760 copy_v3_v3(v2[0], cloth->verts[ap2].txold); 00761 copy_v3_v3(v2[1], cloth->verts[ap2].tx); 00762 copy_v3_v3(v3[0], cloth->verts[ap3].txold); 00763 copy_v3_v3(v3[1], cloth->verts[ap3].tx); 00764 00765 copy_v3_v3(v4[0], collmd->current_x[bp1].co); 00766 copy_v3_v3(v4[1], collmd->current_xnew[bp1].co); 00767 copy_v3_v3(v5[0], collmd->current_x[bp2].co); 00768 copy_v3_v3(v5[1], collmd->current_xnew[bp2].co); 00769 copy_v3_v3(v6[0], collmd->current_x[bp3].co); 00770 copy_v3_v3(v6[1], collmd->current_xnew[bp3].co); 00771 00772 normal_tri_v3(n2, v4[1], v5[1], v6[1]); 00773 00774 /*offset new positions a bit, to account for margins*/ 00775 i1 = ap1; i2 = ap2; i3 = ap3; 00776 i4 = bp1; i5 = bp2; i6 = bp3; 00777 00778 for (j=0; j<3; j++) { 00779 int collp1, collp2, k, j2 = (j+1)%3; 00780 00781 table[0] = ap1; table[1] = ap2; table[2] = ap3; 00782 table[3] = bp1; table[4] = bp2; table[5] = bp3; 00783 for (k=0; k<3; k++) { 00784 float p1[3], p2[3]; 00785 int k2 = (k+1)%3; 00786 00787 get_edgepairkey(&tstkey, table[j], table[j2], table[k+3], table[k2+3]); 00788 //if (BLI_ghash_haskey(visithash, &tstkey)) 00789 // continue; 00790 00791 key = BLI_memarena_alloc(arena, sizeof(edgepairkey)); 00792 *key = tstkey; 00793 BLI_ghash_insert(visithash, key, NULL); 00794 00795 sub_v3_v3v3(p1, verts[j], verts[j2]); 00796 sub_v3_v3v3(p2, verts[k+3], verts[k2+3]); 00797 00798 cross_v3_v3v3(off, p1, p2); 00799 normalize_v3(off); 00800 00801 if (dot_v3v3(n2, off) < 0.0) 00802 negate_v3(off); 00803 00804 mul_v3_fl(off, epsilon1 + epsilon2 + ALMOST_ZERO); 00805 copy_v3_v3(p1, verts[k+3]); 00806 copy_v3_v3(p2, verts[k2+3]); 00807 add_v3_v3(p1, off); 00808 add_v3_v3(p2, off); 00809 00810 ret = eltopo_line_line_moving_isect_v3v3_f(verts[j], table[j], verts[j2], table[j2], 00811 p1, table[k+3], p2, table[k2+3], 00812 no, uv, &t, &relnor); 00813 /*cloth vert versus coll face*/ 00814 if (ret) { 00815 collpair->ap1 = table[j]; collpair->ap2 = table[j2]; 00816 collpair->bp1 = table[k+3]; collpair->bp2 = table[k2+3]; 00817 00818 /*I'm not sure if this is correct, but hopefully it's 00819 better then simply ignoring back edges*/ 00820 if (dot_v3v3(n2, no) < 0.0) { 00821 negate_v3(no); 00822 } 00823 00824 copy_v3_v3(collpair->normal, no); 00825 mul_v3_v3fl(collpair->vector, collpair->normal, relnor); 00826 collpair->distance = relnor; 00827 collpair->time = t; 00828 00829 copy_v2_v2(collpair->bary, uv); 00830 00831 collpair->flag = COLLISION_IS_EDGES; 00832 collpair++; 00833 } 00834 } 00835 } 00836 } 00837 00838 return collpair; 00839 } 00840 00841 static int cloth_edge_collision_response_moving ( ClothModifierData *clmd, CollisionModifierData *collmd, CollPair *collpair, CollPair *collision_end ) 00842 { 00843 int result = 0; 00844 Cloth *cloth1; 00845 float w1, w2; 00846 float v1[3], v2[3], relativeVelocity[3]; 00847 float magrelVel, pimpulse[3]; 00848 00849 cloth1 = clmd->clothObject; 00850 00851 for ( ; collpair != collision_end; collpair++ ) 00852 { 00853 if (!(collpair->flag & COLLISION_IS_EDGES)) 00854 continue; 00855 00856 // was: txold 00857 w1 = collpair->bary[0]; w2 = collpair->bary[1]; 00858 00859 // Calculate relative "velocity". 00860 VECADDFAC(v1, cloth1->verts[collpair->ap1].tv, cloth1->verts[collpair->ap2].tv, w1); 00861 VECADDFAC(v2, collmd->current_v[collpair->bp1].co, collmd->current_v[collpair->bp2].co, w2); 00862 00863 VECSUB ( relativeVelocity, v2, v1); 00864 00865 // Calculate the normal component of the relative velocity (actually only the magnitude - the direction is stored in 'normal'). 00866 magrelVel = INPR ( relativeVelocity, collpair->normal ); 00867 00868 // If v_n_mag < 0 the edges are approaching each other. 00869 if ( magrelVel > ALMOST_ZERO ) 00870 { 00871 // Calculate Impulse magnitude to stop all motion in normal direction. 00872 float magtangent = 0, repulse = 0, d = 0; 00873 double impulse = 0.0; 00874 float vrel_t_pre[3]; 00875 float temp[3], spf; 00876 00877 zero_v3(pimpulse); 00878 00879 // calculate tangential velocity 00880 VECCOPY ( temp, collpair->normal ); 00881 mul_v3_fl( temp, magrelVel ); 00882 VECSUB ( vrel_t_pre, relativeVelocity, temp ); 00883 00884 // Decrease in magnitude of relative tangential velocity due to coulomb friction 00885 // in original formula "magrelVel" should be the "change of relative velocity in normal direction" 00886 magtangent = MIN2 ( clmd->coll_parms->friction * 0.01 * magrelVel,sqrt ( INPR ( vrel_t_pre,vrel_t_pre ) ) ); 00887 00888 // Apply friction impulse. 00889 if ( magtangent > ALMOST_ZERO ) 00890 { 00891 normalize_v3( vrel_t_pre ); 00892 00893 impulse = magtangent; 00894 VECADDMUL ( pimpulse, vrel_t_pre, impulse); 00895 } 00896 00897 // Apply velocity stopping impulse 00898 // I_c = m * v_N / 2.0 00899 // no 2.0 * magrelVel normally, but looks nicer DG 00900 impulse = magrelVel; 00901 00902 mul_v3_fl(collpair->normal, 0.5); 00903 VECADDMUL ( pimpulse, collpair->normal, impulse); 00904 00905 // Apply repulse impulse if distance too short 00906 // I_r = -min(dt*kd, m(0,1d/dt - v_n)) 00907 spf = (float)clmd->sim_parms->stepsPerFrame / clmd->sim_parms->timescale; 00908 00909 d = collpair->distance; 00910 if ( ( magrelVel < 0.1*d*spf && ( d > ALMOST_ZERO ) ) ) 00911 { 00912 repulse = MIN2 ( d*1.0/spf, 0.1*d*spf - magrelVel ); 00913 00914 // stay on the safe side and clamp repulse 00915 if ( impulse > ALMOST_ZERO ) 00916 repulse = MIN2 ( repulse, 5.0*impulse ); 00917 repulse = MAX2 ( impulse, repulse ); 00918 00919 impulse = repulse / ( 5.0 ); // original 2.0 / 0.25 00920 VECADDMUL ( pimpulse, collpair->normal, impulse); 00921 } 00922 00923 w2 = 1.0f-w1; 00924 if (w1 < 0.5) 00925 w1 *= 2.0; 00926 else 00927 w2 *= 2.0; 00928 00929 VECADDFAC(cloth1->verts[collpair->ap1].impulse, cloth1->verts[collpair->ap1].impulse, pimpulse, w1*2.0); 00930 VECADDFAC(cloth1->verts[collpair->ap2].impulse, cloth1->verts[collpair->ap2].impulse, pimpulse, w2*2.0); 00931 00932 cloth1->verts[collpair->ap1].impulse_count++; 00933 cloth1->verts[collpair->ap2].impulse_count++; 00934 00935 result = 1; 00936 } 00937 } 00938 00939 return result; 00940 } 00941 00942 static int cloth_collision_response_moving ( ClothModifierData *clmd, CollisionModifierData *collmd, CollPair *collpair, CollPair *collision_end ) 00943 { 00944 int result = 0; 00945 Cloth *cloth1; 00946 float w1, w2, w3, u1, u2, u3; 00947 float v1[3], v2[3], relativeVelocity[3]; 00948 float magrelVel; 00949 float epsilon2 = BLI_bvhtree_getepsilon ( collmd->bvhtree ); 00950 00951 cloth1 = clmd->clothObject; 00952 00953 for ( ; collpair != collision_end; collpair++ ) 00954 { 00955 if (collpair->flag & COLLISION_IS_EDGES) 00956 continue; 00957 00958 if ( collpair->flag & COLLISION_USE_COLLFACE ) { 00959 // was: txold 00960 w1 = collpair->bary[0]; w2 = collpair->bary[1]; w3 = collpair->bary[2]; 00961 00962 // Calculate relative "velocity". 00963 collision_interpolateOnTriangle ( v1, collmd->current_v[collpair->bp1].co, collmd->current_v[collpair->bp2].co, collmd->current_v[collpair->bp3].co, w1, w2, w3); 00964 00965 VECSUB ( relativeVelocity, v1, cloth1->verts[collpair->collp].tv); 00966 00967 // Calculate the normal component of the relative velocity (actually only the magnitude - the direction is stored in 'normal'). 00968 magrelVel = INPR ( relativeVelocity, collpair->normal ); 00969 00970 // If v_n_mag < 0 the edges are approaching each other. 00971 if ( magrelVel > ALMOST_ZERO ) 00972 { 00973 // Calculate Impulse magnitude to stop all motion in normal direction. 00974 float magtangent = 0, repulse = 0, d = 0; 00975 double impulse = 0.0; 00976 float vrel_t_pre[3]; 00977 float temp[3], spf; 00978 00979 // calculate tangential velocity 00980 VECCOPY ( temp, collpair->normal ); 00981 mul_v3_fl( temp, magrelVel ); 00982 VECSUB ( vrel_t_pre, relativeVelocity, temp ); 00983 00984 // Decrease in magnitude of relative tangential velocity due to coulomb friction 00985 // in original formula "magrelVel" should be the "change of relative velocity in normal direction" 00986 magtangent = MIN2 ( clmd->coll_parms->friction * 0.01 * magrelVel,sqrt ( INPR ( vrel_t_pre,vrel_t_pre ) ) ); 00987 00988 // Apply friction impulse. 00989 if ( magtangent > ALMOST_ZERO ) 00990 { 00991 normalize_v3( vrel_t_pre ); 00992 00993 impulse = magtangent; // 2.0 * 00994 VECADDMUL ( cloth1->verts[collpair->collp].impulse, vrel_t_pre, impulse); 00995 } 00996 00997 // Apply velocity stopping impulse 00998 // I_c = m * v_N / 2.0 00999 // no 2.0 * magrelVel normally, but looks nicer DG 01000 impulse = magrelVel/2.0; 01001 01002 VECADDMUL ( cloth1->verts[collpair->collp].impulse, collpair->normal, impulse); 01003 cloth1->verts[collpair->collp].impulse_count++; 01004 01005 // Apply repulse impulse if distance too short 01006 // I_r = -min(dt*kd, m(0,1d/dt - v_n)) 01007 spf = (float)clmd->sim_parms->stepsPerFrame / clmd->sim_parms->timescale; 01008 01009 d = -collpair->distance; 01010 if ( ( magrelVel < 0.1*d*spf ) && ( d > ALMOST_ZERO ) ) 01011 { 01012 repulse = MIN2 ( d*1.0/spf, 0.1*d*spf - magrelVel ); 01013 01014 // stay on the safe side and clamp repulse 01015 if ( impulse > ALMOST_ZERO ) 01016 repulse = MIN2 ( repulse, 5.0*impulse ); 01017 repulse = MAX2 ( impulse, repulse ); 01018 01019 impulse = repulse / ( 5.0 ); // original 2.0 / 0.25 01020 VECADDMUL ( cloth1->verts[collpair->collp].impulse, collpair->normal, impulse); 01021 } 01022 01023 result = 1; 01024 } 01025 } else { 01026 w1 = collpair->bary[0]; w2 = collpair->bary[1]; w3 = collpair->bary[2]; 01027 01028 // Calculate relative "velocity". 01029 collision_interpolateOnTriangle ( v1, cloth1->verts[collpair->ap1].tv, cloth1->verts[collpair->ap2].tv, cloth1->verts[collpair->ap3].tv, w1, w2, w3 ); 01030 01031 VECSUB ( relativeVelocity, collmd->current_v[collpair->collp].co, v1); 01032 01033 // Calculate the normal component of the relative velocity (actually only the magnitude - the direction is stored in 'normal'). 01034 magrelVel = INPR ( relativeVelocity, collpair->normal ); 01035 01036 // If v_n_mag < 0 the edges are approaching each other. 01037 if ( magrelVel > ALMOST_ZERO ) 01038 { 01039 // Calculate Impulse magnitude to stop all motion in normal direction. 01040 float magtangent = 0, repulse = 0, d = 0; 01041 double impulse = 0.0; 01042 float vrel_t_pre[3], pimpulse[3] = {0.0f, 0.0f, 0.0f}; 01043 float temp[3], spf; 01044 01045 // calculate tangential velocity 01046 VECCOPY ( temp, collpair->normal ); 01047 mul_v3_fl( temp, magrelVel ); 01048 VECSUB ( vrel_t_pre, relativeVelocity, temp ); 01049 01050 // Decrease in magnitude of relative tangential velocity due to coulomb friction 01051 // in original formula "magrelVel" should be the "change of relative velocity in normal direction" 01052 magtangent = MIN2 ( clmd->coll_parms->friction * 0.01 * magrelVel,sqrt ( INPR ( vrel_t_pre,vrel_t_pre ) ) ); 01053 01054 // Apply friction impulse. 01055 if ( magtangent > ALMOST_ZERO ) 01056 { 01057 normalize_v3( vrel_t_pre ); 01058 01059 impulse = magtangent; // 2.0 * 01060 VECADDMUL ( pimpulse, vrel_t_pre, impulse); 01061 } 01062 01063 // Apply velocity stopping impulse 01064 // I_c = m * v_N / 2.0 01065 // no 2.0 * magrelVel normally, but looks nicer DG 01066 impulse = magrelVel/2.0; 01067 01068 VECADDMUL ( pimpulse, collpair->normal, impulse); 01069 01070 // Apply repulse impulse if distance too short 01071 // I_r = -min(dt*kd, m(0,1d/dt - v_n)) 01072 spf = (float)clmd->sim_parms->stepsPerFrame / clmd->sim_parms->timescale; 01073 01074 d = -collpair->distance; 01075 if ( ( magrelVel < 0.1*d*spf ) && ( d > ALMOST_ZERO ) ) 01076 { 01077 repulse = MIN2 ( d*1.0/spf, 0.1*d*spf - magrelVel ); 01078 01079 // stay on the safe side and clamp repulse 01080 if ( impulse > ALMOST_ZERO ) 01081 repulse = MIN2 ( repulse, 5.0*impulse ); 01082 repulse = MAX2 ( impulse, repulse ); 01083 01084 impulse = repulse / ( 2.0 ); // original 2.0 / 0.25 01085 VECADDMUL ( pimpulse, collpair->normal, impulse); 01086 } 01087 01088 if (w1 < 0.5) w1 *= 2.0; 01089 if (w2 < 0.5) w2 *= 2.0; 01090 if (w3 < 0.5) w3 *= 2.0; 01091 01092 VECADDMUL(cloth1->verts[collpair->ap1].impulse, pimpulse, w1*2.0); 01093 VECADDMUL(cloth1->verts[collpair->ap2].impulse, pimpulse, w2*2.0); 01094 VECADDMUL(cloth1->verts[collpair->ap3].impulse, pimpulse, w3*2.0);; 01095 cloth1->verts[collpair->ap1].impulse_count++; 01096 cloth1->verts[collpair->ap2].impulse_count++; 01097 cloth1->verts[collpair->ap3].impulse_count++; 01098 01099 result = 1; 01100 } 01101 } 01102 } 01103 01104 return result; 01105 } 01106 01107 01108 typedef struct tripairkey { 01109 int p, a1, a2, a3; 01110 } tripairkey; 01111 01112 unsigned int tripair_hash(void *vkey) 01113 { 01114 tripairkey *key = vkey; 01115 int keys[4] = {key->p, key->a1, key->a2, key->a3}; 01116 int i, j; 01117 01118 for (i=0; i<4; i++) { 01119 for (j=0; j<3; j++) { 01120 if (keys[j] >= keys[j+1]) { 01121 SWAP(int, keys[j], keys[j+1]); 01122 } 01123 } 01124 } 01125 01126 return keys[0]*101 + keys[1]*72 + keys[2]*53 + keys[3]*34; 01127 } 01128 01129 int tripair_cmp(const void *va, const void *vb) 01130 { 01131 tripairkey *a = va, *b = vb; 01132 int keysa[4] = {a->p, a->a1, a->a2, a->a3}; 01133 int keysb[4] = {b->p, b->a1, b->a2, b->a3}; 01134 int i; 01135 01136 for (i=0; i<4; i++) { 01137 int j, ok=0; 01138 for (j=0; j<4; j++) { 01139 if (keysa[i] == keysa[j]) { 01140 ok = 1; 01141 break; 01142 } 01143 } 01144 if (!ok) 01145 return -1; 01146 } 01147 01148 return 0; 01149 } 01150 01151 static void get_tripairkey(tripairkey *key, int p, int a1, int a2, int a3) 01152 { 01153 key->a1 = a1; 01154 key->a2 = a2; 01155 key->a3 = a3; 01156 key->p = p; 01157 } 01158 01159 static int checkvisit(MemArena *arena, GHash *gh, int p, int a1, int a2, int a3) 01160 { 01161 tripairkey key, *key2; 01162 01163 get_tripairkey(&key, p, a1, a2, a3); 01164 if (BLI_ghash_haskey(gh, &key)) 01165 return 1; 01166 01167 key2 = BLI_memarena_alloc(arena, sizeof(*key2)); 01168 *key2 = key; 01169 BLI_ghash_insert(gh, key2, NULL); 01170 01171 return 0; 01172 } 01173 01174 int cloth_point_tri_moving_v3v3_f(float v1[2][3], int i1, float v2[2][3], int i2, 01175 float v3[2][3], int i3, float v4[2][3], int i4, 01176 float normal[3], float bary[3], float *t, 01177 float *relnor, GHash *gh, MemArena *arena) 01178 { 01179 if (checkvisit(arena, gh, i1, i2, i3, i4)) 01180 return 0; 01181 01182 return eltopo_point_tri_moving_v3v3_f(v1, i1, v2, i2, v3, i3, v4, i4, normal, bary, t, relnor); 01183 } 01184 01185 static CollPair* cloth_collision ( ModifierData *md1, ModifierData *md2, BVHTreeOverlap *overlap, 01186 CollPair *collpair, double dt, GHash *gh, MemArena *arena) 01187 { 01188 ClothModifierData *clmd = ( ClothModifierData * ) md1; 01189 CollisionModifierData *collmd = ( CollisionModifierData * ) md2; 01190 MFace *face1=NULL, *face2 = NULL; 01191 ClothVertex *verts1 = clmd->clothObject->verts; 01192 double distance = 0; 01193 float epsilon1 = clmd->coll_parms->epsilon; 01194 float epsilon2 = BLI_bvhtree_getepsilon ( collmd->bvhtree ); 01195 float no[3], uv[3], t, relnor; 01196 int i, i1, i2, i3, i4, i5, i6; 01197 Cloth *cloth = clmd->clothObject; 01198 float n1[3], sdis, p[3], l, n2[3], off[3], v1[2][3], v2[2][3], v3[2][3], v4[2][3], v5[2][3], v6[2][3]; 01199 int j, ret, bp1, bp2, bp3, ap1, ap2, ap3; 01200 01201 face1 = & ( clmd->clothObject->mfaces[overlap->indexA] ); 01202 face2 = & ( collmd->mfaces[overlap->indexB] ); 01203 01204 // check all 4 possible collisions 01205 for ( i = 0; i < 4; i++ ) 01206 { 01207 if ( i == 0 ) 01208 { 01209 // fill faceA 01210 ap1 = face1->v1; 01211 ap2 = face1->v2; 01212 ap3 = face1->v3; 01213 01214 // fill faceB 01215 bp1 = face2->v1; 01216 bp2 = face2->v2; 01217 bp3 = face2->v3; 01218 } 01219 else if ( i == 1 ) 01220 { 01221 if ( face1->v4 ) 01222 { 01223 // fill faceA 01224 ap1 = face1->v1; 01225 ap2 = face1->v3; 01226 ap3 = face1->v4; 01227 01228 // fill faceB 01229 bp1 = face2->v1; 01230 bp2 = face2->v2; 01231 bp3 = face2->v3; 01232 } 01233 else { 01234 continue; 01235 } 01236 } 01237 if ( i == 2 ) 01238 { 01239 if ( face2->v4 ) 01240 { 01241 // fill faceA 01242 ap1 = face1->v1; 01243 ap2 = face1->v2; 01244 ap3 = face1->v3; 01245 01246 // fill faceB 01247 bp1 = face2->v1; 01248 bp2 = face2->v3; 01249 bp3 = face2->v4; 01250 } 01251 else { 01252 continue; 01253 } 01254 } 01255 else if ( i == 3 ) 01256 { 01257 if ( face1->v4 && face2->v4 ) 01258 { 01259 // fill faceA 01260 ap1 = face1->v1; 01261 ap2 = face1->v3; 01262 ap3 = face1->v4; 01263 01264 // fill faceB 01265 bp1 = face2->v1; 01266 bp2 = face2->v3; 01267 bp3 = face2->v4; 01268 } 01269 else { 01270 continue; 01271 } 01272 } 01273 01274 copy_v3_v3(v1[0], cloth->verts[ap1].txold); 01275 copy_v3_v3(v1[1], cloth->verts[ap1].tx); 01276 copy_v3_v3(v2[0], cloth->verts[ap2].txold); 01277 copy_v3_v3(v2[1], cloth->verts[ap2].tx); 01278 copy_v3_v3(v3[0], cloth->verts[ap3].txold); 01279 copy_v3_v3(v3[1], cloth->verts[ap3].tx); 01280 01281 copy_v3_v3(v4[0], collmd->current_x[bp1].co); 01282 copy_v3_v3(v4[1], collmd->current_xnew[bp1].co); 01283 copy_v3_v3(v5[0], collmd->current_x[bp2].co); 01284 copy_v3_v3(v5[1], collmd->current_xnew[bp2].co); 01285 copy_v3_v3(v6[0], collmd->current_x[bp3].co); 01286 copy_v3_v3(v6[1], collmd->current_xnew[bp3].co); 01287 01288 normal_tri_v3(n2, v4[1], v5[1], v6[1]); 01289 01290 sdis = clmd->coll_parms->distance_repel + epsilon2 + FLT_EPSILON; 01291 01292 /*apply a repulsion force, to help the solver along*/ 01293 copy_v3_v3(off, n2); 01294 negate_v3(off); 01295 if (isect_ray_plane_v3(v1[1], off, v4[1], v5[1], v6[1], &l, 0)) { 01296 if (l >= 0.0 && l < sdis) { 01297 mul_v3_fl(off, (l-sdis)*cloth->verts[ap1].mass*dt*clmd->coll_parms->repel_force*0.1); 01298 01299 add_v3_v3(cloth->verts[ap1].tv, off); 01300 add_v3_v3(cloth->verts[ap2].tv, off); 01301 add_v3_v3(cloth->verts[ap3].tv, off); 01302 } 01303 } 01304 01305 /*offset new positions a bit, to account for margins*/ 01306 copy_v3_v3(off, n2); 01307 mul_v3_fl(off, epsilon1 + epsilon2 + ALMOST_ZERO); 01308 add_v3_v3(v4[1], off); add_v3_v3(v5[1], off); add_v3_v3(v6[1], off); 01309 01310 i1 = ap1; i2 = ap2; i3 = ap3; 01311 i4 = bp1+cloth->numverts; i5 = bp2+cloth->numverts; i6 = bp3+cloth->numverts; 01312 01313 for (j=0; j<6; j++) { 01314 int collp; 01315 01316 switch (j) { 01317 case 0: 01318 ret = cloth_point_tri_moving_v3v3_f(v1, i1, v4, i4, v5, i5, v6, i6, no, uv, &t, &relnor, gh, arena); 01319 collp = ap1; 01320 break; 01321 case 1: 01322 collp = ap2; 01323 ret = cloth_point_tri_moving_v3v3_f(v2, i2, v4, i4, v5, i5, v6, i6, no, uv, &t, &relnor, gh, arena); 01324 break; 01325 case 2: 01326 collp = ap3; 01327 ret = cloth_point_tri_moving_v3v3_f(v3, i3, v4, i4, v5, i5, v6, i6, no, uv, &t, &relnor, gh, arena); 01328 break; 01329 case 3: 01330 collp = bp1; 01331 ret = cloth_point_tri_moving_v3v3_f(v4, i4, v1, i1, v2, i2, v3, i3, no, uv, &t, &relnor, gh, arena); 01332 break; 01333 case 4: 01334 collp = bp2; 01335 ret = cloth_point_tri_moving_v3v3_f(v5, i5, v1, i1, v2, i2, v3, i3, no, uv, &t, &relnor, gh, arena); 01336 break; 01337 case 5: 01338 collp = bp3; 01339 ret = cloth_point_tri_moving_v3v3_f(v6, i6, v1, i1, v2, i2, v3, i3, no, uv, &t, &relnor, gh, arena); 01340 break; 01341 } 01342 01343 /*cloth vert versus coll face*/ 01344 if (ret && j < 3) { 01345 collpair->bp1 = bp1; collpair->bp2 = bp2; collpair->bp3 = bp3; 01346 collpair->collp = collp; 01347 01348 copy_v3_v3(collpair->normal, no); 01349 mul_v3_v3fl(collpair->vector, collpair->normal, relnor); 01350 collpair->distance = relnor; 01351 collpair->time = t; 01352 01353 copy_v3_v3(collpair->bary, uv); 01354 01355 collpair->flag = COLLISION_USE_COLLFACE; 01356 collpair++; 01357 } else if (ret && j >= 3) { /*coll vert versus cloth face*/ 01358 collpair->ap1 = ap1; collpair->ap2 = ap2; collpair->ap3 = ap3; 01359 collpair->collp = collp; 01360 01361 copy_v3_v3(collpair->normal, no); 01362 mul_v3_v3fl(collpair->vector, collpair->normal, relnor); 01363 collpair->distance = relnor; 01364 collpair->time = t; 01365 01366 copy_v3_v3(collpair->bary, uv); 01367 01368 collpair->flag = 0; 01369 collpair++; 01370 } 01371 } 01372 } 01373 01374 return collpair; 01375 } 01376 01377 static void machine_epsilon_offset(Cloth *cloth) { 01378 ClothVertex *cv; 01379 int i, j; 01380 01381 cv = cloth->verts; 01382 for (i=0; i<cloth->numverts; i++, cv++) { 01383 /*aggrevatingly enough, it's necassary to offset the coordinates 01384 by a multiple of the 32-bit floating point epsilon when switching 01385 into doubles*/ 01386 #define RNDSIGN (float)(-1*(BLI_rand()%2==0)|1) 01387 for (j=0; j<3; j++) { 01388 cv->tx[j] += FLT_EPSILON*30.0f*RNDSIGN; 01389 cv->txold[j] += FLT_EPSILON*30.0f*RNDSIGN; 01390 cv->tv[j] += FLT_EPSILON*30.0f*RNDSIGN; 01391 } 01392 } 01393 } 01394 01395 #else /* !WITH_ELTOPO */ 01396 01397 //Determines collisions on overlap, collisions are written to collpair[i] and collision+number_collision_found is returned 01398 static CollPair* cloth_collision ( ModifierData *md1, ModifierData *md2, 01399 BVHTreeOverlap *overlap, CollPair *collpair, float dt ) 01400 { 01401 ClothModifierData *clmd = ( ClothModifierData * ) md1; 01402 CollisionModifierData *collmd = ( CollisionModifierData * ) md2; 01403 Cloth *cloth = clmd->clothObject; 01404 MFace *face1=NULL, *face2 = NULL; 01405 #ifdef USE_BULLET 01406 ClothVertex *verts1 = clmd->clothObject->verts; 01407 #endif 01408 double distance = 0; 01409 float epsilon1 = clmd->coll_parms->epsilon; 01410 float epsilon2 = BLI_bvhtree_getepsilon ( collmd->bvhtree ); 01411 float n2[3], sdis, l; 01412 int i; 01413 01414 face1 = & ( clmd->clothObject->mfaces[overlap->indexA] ); 01415 face2 = & ( collmd->mfaces[overlap->indexB] ); 01416 01417 // check all 4 possible collisions 01418 for ( i = 0; i < 4; i++ ) 01419 { 01420 if ( i == 0 ) 01421 { 01422 // fill faceA 01423 collpair->ap1 = face1->v1; 01424 collpair->ap2 = face1->v2; 01425 collpair->ap3 = face1->v3; 01426 01427 // fill faceB 01428 collpair->bp1 = face2->v1; 01429 collpair->bp2 = face2->v2; 01430 collpair->bp3 = face2->v3; 01431 } 01432 else if ( i == 1 ) 01433 { 01434 if ( face1->v4 ) 01435 { 01436 // fill faceA 01437 collpair->ap1 = face1->v1; 01438 collpair->ap2 = face1->v4; 01439 collpair->ap3 = face1->v3; 01440 01441 // fill faceB 01442 collpair->bp1 = face2->v1; 01443 collpair->bp2 = face2->v2; 01444 collpair->bp3 = face2->v3; 01445 } 01446 else 01447 i++; 01448 } 01449 if ( i == 2 ) 01450 { 01451 if ( face2->v4 ) 01452 { 01453 // fill faceA 01454 collpair->ap1 = face1->v1; 01455 collpair->ap2 = face1->v2; 01456 collpair->ap3 = face1->v3; 01457 01458 // fill faceB 01459 collpair->bp1 = face2->v1; 01460 collpair->bp2 = face2->v4; 01461 collpair->bp3 = face2->v3; 01462 } 01463 else 01464 break; 01465 } 01466 else if ( i == 3 ) 01467 { 01468 if ( face1->v4 && face2->v4 ) 01469 { 01470 // fill faceA 01471 collpair->ap1 = face1->v1; 01472 collpair->ap2 = face1->v4; 01473 collpair->ap3 = face1->v3; 01474 01475 // fill faceB 01476 collpair->bp1 = face2->v1; 01477 collpair->bp2 = face2->v4; 01478 collpair->bp3 = face2->v3; 01479 } 01480 else 01481 break; 01482 } 01483 01484 normal_tri_v3(n2, collmd->current_xnew[collpair->bp1].co, 01485 collmd->current_xnew[collpair->bp2].co, 01486 collmd->current_xnew[collpair->bp3].co); 01487 01488 sdis = clmd->coll_parms->distance_repel + epsilon2 + FLT_EPSILON; 01489 01490 /*apply a repulsion force, to help the solver along. 01491 this is kindof crude, it only tests one vert of the triangle*/ 01492 if (isect_ray_plane_v3(cloth->verts[collpair->ap1].tx, n2, collmd->current_xnew[collpair->bp1].co, 01493 collmd->current_xnew[collpair->bp2].co, 01494 collmd->current_xnew[collpair->bp3].co, &l, 0)) 01495 { 01496 if (l >= 0.0 && l < sdis) { 01497 mul_v3_fl(n2, (l-sdis)*cloth->verts[collpair->ap1].mass*dt*clmd->coll_parms->repel_force*0.1); 01498 01499 add_v3_v3(cloth->verts[collpair->ap1].tv, n2); 01500 add_v3_v3(cloth->verts[collpair->ap2].tv, n2); 01501 add_v3_v3(cloth->verts[collpair->ap3].tv, n2); 01502 } 01503 } 01504 01505 #ifdef USE_BULLET 01506 // calc distance + normal 01507 distance = plNearestPoints ( 01508 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 ); 01509 #else 01510 // just be sure that we don't add anything 01511 distance = 2.0 * ( epsilon1 + epsilon2 + ALMOST_ZERO ); 01512 #endif 01513 01514 if ( distance <= ( epsilon1 + epsilon2 + ALMOST_ZERO ) ) 01515 { 01516 normalize_v3_v3( collpair->normal, collpair->vector ); 01517 01518 collpair->distance = distance; 01519 collpair->flag = 0; 01520 collpair++; 01521 }/* 01522 else 01523 { 01524 float w1, w2, w3, u1, u2, u3; 01525 float v1[3], v2[3], relativeVelocity[3]; 01526 01527 // calc relative velocity 01528 01529 // compute barycentric coordinates for both collision points 01530 collision_compute_barycentric ( collpair->pa, 01531 verts1[collpair->ap1].txold, 01532 verts1[collpair->ap2].txold, 01533 verts1[collpair->ap3].txold, 01534 &w1, &w2, &w3 ); 01535 01536 // was: txold 01537 collision_compute_barycentric ( collpair->pb, 01538 collmd->current_x[collpair->bp1].co, 01539 collmd->current_x[collpair->bp2].co, 01540 collmd->current_x[collpair->bp3].co, 01541 &u1, &u2, &u3 ); 01542 01543 // Calculate relative "velocity". 01544 collision_interpolateOnTriangle ( v1, verts1[collpair->ap1].tv, verts1[collpair->ap2].tv, verts1[collpair->ap3].tv, w1, w2, w3 ); 01545 01546 collision_interpolateOnTriangle ( v2, collmd->current_v[collpair->bp1].co, collmd->current_v[collpair->bp2].co, collmd->current_v[collpair->bp3].co, u1, u2, u3 ); 01547 01548 VECSUB ( relativeVelocity, v2, v1 ); 01549 01550 if(sqrt(INPR(relativeVelocity, relativeVelocity)) >= distance) 01551 { 01552 // check for collision in the future 01553 collpair->flag |= COLLISION_IN_FUTURE; 01554 collpair++; 01555 } 01556 }*/ 01557 } 01558 return collpair; 01559 } 01560 #endif /* WITH_ELTOPO */ 01561 01562 01563 #if 0 01564 static int cloth_collision_response_moving( ClothModifierData *clmd, CollisionModifierData *collmd, CollPair *collpair, CollPair *collision_end ) 01565 { 01566 int result = 0; 01567 Cloth *cloth1; 01568 float w1, w2, w3, u1, u2, u3; 01569 float v1[3], v2[3], relativeVelocity[3]; 01570 float magrelVel; 01571 01572 cloth1 = clmd->clothObject; 01573 01574 for ( ; collpair != collision_end; collpair++ ) 01575 { 01576 // compute barycentric coordinates for both collision points 01577 collision_compute_barycentric ( collpair->pa, 01578 cloth1->verts[collpair->ap1].txold, 01579 cloth1->verts[collpair->ap2].txold, 01580 cloth1->verts[collpair->ap3].txold, 01581 &w1, &w2, &w3 ); 01582 01583 // was: txold 01584 collision_compute_barycentric ( collpair->pb, 01585 collmd->current_x[collpair->bp1].co, 01586 collmd->current_x[collpair->bp2].co, 01587 collmd->current_x[collpair->bp3].co, 01588 &u1, &u2, &u3 ); 01589 01590 // Calculate relative "velocity". 01591 collision_interpolateOnTriangle ( v1, cloth1->verts[collpair->ap1].tv, cloth1->verts[collpair->ap2].tv, cloth1->verts[collpair->ap3].tv, w1, w2, w3 ); 01592 01593 collision_interpolateOnTriangle ( v2, collmd->current_v[collpair->bp1].co, collmd->current_v[collpair->bp2].co, collmd->current_v[collpair->bp3].co, u1, u2, u3 ); 01594 01595 VECSUB ( relativeVelocity, v2, v1 ); 01596 01597 // Calculate the normal component of the relative velocity (actually only the magnitude - the direction is stored in 'normal'). 01598 magrelVel = INPR ( relativeVelocity, collpair->normal ); 01599 01600 // printf("magrelVel: %f\n", magrelVel); 01601 01602 // Calculate masses of points. 01603 // TODO 01604 01605 // If v_n_mag < 0 the edges are approaching each other. 01606 if ( magrelVel > ALMOST_ZERO ) 01607 { 01608 // Calculate Impulse magnitude to stop all motion in normal direction. 01609 float magtangent = 0; 01610 double impulse = 0.0; 01611 float vrel_t_pre[3]; 01612 float temp[3]; 01613 01614 // calculate tangential velocity 01615 VECCOPY ( temp, collpair->normal ); 01616 mul_v3_fl( temp, magrelVel ); 01617 VECSUB ( vrel_t_pre, relativeVelocity, temp ); 01618 01619 // Decrease in magnitude of relative tangential velocity due to coulomb friction 01620 // in original formula "magrelVel" should be the "change of relative velocity in normal direction" 01621 magtangent = MIN2 ( clmd->coll_parms->friction * 0.01 * magrelVel,sqrt ( INPR ( vrel_t_pre,vrel_t_pre ) ) ); 01622 01623 // Apply friction impulse. 01624 if ( magtangent > ALMOST_ZERO ) 01625 { 01626 normalize_v3( vrel_t_pre ); 01627 01628 impulse = 2.0 * magtangent / ( 1.0 + w1*w1 + w2*w2 + w3*w3 ); 01629 VECADDMUL ( cloth1->verts[collpair->ap1].impulse, vrel_t_pre, w1 * impulse ); 01630 VECADDMUL ( cloth1->verts[collpair->ap2].impulse, vrel_t_pre, w2 * impulse ); 01631 VECADDMUL ( cloth1->verts[collpair->ap3].impulse, vrel_t_pre, w3 * impulse ); 01632 } 01633 01634 // Apply velocity stopping impulse 01635 // I_c = m * v_N / 2.0 01636 // no 2.0 * magrelVel normally, but looks nicer DG 01637 impulse = magrelVel / ( 1.0 + w1*w1 + w2*w2 + w3*w3 ); 01638 01639 VECADDMUL ( cloth1->verts[collpair->ap1].impulse, collpair->normal, w1 * impulse ); 01640 cloth1->verts[collpair->ap1].impulse_count++; 01641 01642 VECADDMUL ( cloth1->verts[collpair->ap2].impulse, collpair->normal, w2 * impulse ); 01643 cloth1->verts[collpair->ap2].impulse_count++; 01644 01645 VECADDMUL ( cloth1->verts[collpair->ap3].impulse, collpair->normal, w3 * impulse ); 01646 cloth1->verts[collpair->ap3].impulse_count++; 01647 01648 // Apply repulse impulse if distance too short 01649 // I_r = -min(dt*kd, m(0,1d/dt - v_n)) 01650 /* 01651 d = clmd->coll_parms->epsilon*8.0/9.0 + epsilon2*8.0/9.0 - collpair->distance; 01652 if ( ( magrelVel < 0.1*d*clmd->sim_parms->stepsPerFrame ) && ( d > ALMOST_ZERO ) ) 01653 { 01654 repulse = MIN2 ( d*1.0/clmd->sim_parms->stepsPerFrame, 0.1*d*clmd->sim_parms->stepsPerFrame - magrelVel ); 01655 01656 // stay on the safe side and clamp repulse 01657 if ( impulse > ALMOST_ZERO ) 01658 repulse = MIN2 ( repulse, 5.0*impulse ); 01659 repulse = MAX2 ( impulse, repulse ); 01660 01661 impulse = repulse / ( 1.0 + w1*w1 + w2*w2 + w3*w3 ); // original 2.0 / 0.25 01662 VECADDMUL ( cloth1->verts[collpair->ap1].impulse, collpair->normal, impulse ); 01663 VECADDMUL ( cloth1->verts[collpair->ap2].impulse, collpair->normal, impulse ); 01664 VECADDMUL ( cloth1->verts[collpair->ap3].impulse, collpair->normal, impulse ); 01665 } 01666 */ 01667 result = 1; 01668 } 01669 } 01670 return result; 01671 } 01672 #endif 01673 01674 #if 0 01675 static float projectPointOntoLine(float *p, float *a, float *b) 01676 { 01677 float ba[3], pa[3]; 01678 VECSUB(ba, b, a); 01679 VECSUB(pa, p, a); 01680 return INPR(pa, ba) / INPR(ba, ba); 01681 } 01682 01683 static void calculateEENormal(float *np1, float *np2, float *np3, float *np4,float *out_normal) 01684 { 01685 float line1[3], line2[3]; 01686 float length; 01687 01688 VECSUB(line1, np2, np1); 01689 VECSUB(line2, np3, np1); 01690 01691 // printf("l1: %f, l1: %f, l2: %f, l2: %f\n", line1[0], line1[1], line2[0], line2[1]); 01692 01693 cross_v3_v3v3(out_normal, line1, line2); 01694 01695 01696 01697 length = normalize_v3(out_normal); 01698 if (length <= FLT_EPSILON) 01699 { // lines are collinear 01700 VECSUB(out_normal, np2, np1); 01701 normalize_v3(out_normal); 01702 } 01703 } 01704 01705 static void findClosestPointsEE(float *x1, float *x2, float *x3, float *x4, float *w1, float *w2) 01706 { 01707 float temp[3], temp2[3]; 01708 01709 double a, b, c, e, f; 01710 01711 VECSUB(temp, x2, x1); 01712 a = INPR(temp, temp); 01713 01714 VECSUB(temp2, x4, x3); 01715 b = -INPR(temp, temp2); 01716 01717 c = INPR(temp2, temp2); 01718 01719 VECSUB(temp2, x3, x1); 01720 e = INPR(temp, temp2); 01721 01722 VECSUB(temp, x4, x3); 01723 f = -INPR(temp, temp2); 01724 01725 *w1 = (e * c - b * f) / (a * c - b * b); 01726 *w2 = (f - b * *w1) / c; 01727 01728 } 01729 01730 // calculates the distance of 2 edges 01731 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) 01732 { 01733 float line1[3], line2[3], cross[3]; 01734 float length; 01735 float temp[3], temp2[3]; 01736 float dist_a1, dist_a2; 01737 01738 VECSUB(line1, np12, np11); 01739 VECSUB(line2, np22, np21); 01740 01741 cross_v3_v3v3(cross, line1, line2); 01742 length = INPR(cross, cross); 01743 01744 if (length < FLT_EPSILON) 01745 { 01746 *out_a2 = projectPointOntoLine(np11, np21, np22); 01747 if ((*out_a2 >= -FLT_EPSILON) && (*out_a2 <= 1.0 + FLT_EPSILON)) 01748 { 01749 *out_a1 = 0; 01750 calculateEENormal(np11, np12, np21, np22, out_normal); 01751 VECSUB(temp, np22, np21); 01752 mul_v3_fl(temp, *out_a2); 01753 VECADD(temp2, temp, np21); 01754 VECADD(temp2, temp2, np11); 01755 return INPR(temp2, temp2); 01756 } 01757 01758 CLAMP(*out_a2, 0.0, 1.0); 01759 if (*out_a2 > .5) 01760 { // == 1.0 01761 *out_a1 = projectPointOntoLine(np22, np11, np12); 01762 if ((*out_a1 >= -FLT_EPSILON) && (*out_a1 <= 1.0 + FLT_EPSILON)) 01763 { 01764 calculateEENormal(np11, np12, np21, np22, out_normal); 01765 01766 // return (np22 - (np11 + (np12 - np11) * out_a1)).lengthSquared(); 01767 VECSUB(temp, np12, np11); 01768 mul_v3_fl(temp, *out_a1); 01769 VECADD(temp2, temp, np11); 01770 VECSUB(temp2, np22, temp2); 01771 return INPR(temp2, temp2); 01772 } 01773 } 01774 else 01775 { // == 0.0 01776 *out_a1 = projectPointOntoLine(np21, np11, np12); 01777 if ((*out_a1 >= -FLT_EPSILON) && (*out_a1 <= 1.0 + FLT_EPSILON)) 01778 { 01779 calculateEENormal(np11, np11, np21, np22, out_normal); 01780 01781 // return (np21 - (np11 + (np12 - np11) * out_a1)).lengthSquared(); 01782 VECSUB(temp, np12, np11); 01783 mul_v3_fl(temp, *out_a1); 01784 VECADD(temp2, temp, np11); 01785 VECSUB(temp2, np21, temp2); 01786 return INPR(temp2, temp2); 01787 } 01788 } 01789 01790 CLAMP(*out_a1, 0.0, 1.0); 01791 calculateEENormal(np11, np12, np21, np22, out_normal); 01792 if(*out_a1 > .5) 01793 { 01794 if(*out_a2 > .5) 01795 { 01796 VECSUB(temp, np12, np22); 01797 } 01798 else 01799 { 01800 VECSUB(temp, np12, np21); 01801 } 01802 } 01803 else 01804 { 01805 if(*out_a2 > .5) 01806 { 01807 VECSUB(temp, np11, np22); 01808 } 01809 else 01810 { 01811 VECSUB(temp, np11, np21); 01812 } 01813 } 01814 01815 return INPR(temp, temp); 01816 } 01817 else 01818 { 01819 01820 // If the lines aren't parallel (but coplanar) they have to intersect 01821 01822 findClosestPointsEE(np11, np12, np21, np22, out_a1, out_a2); 01823 01824 // If both points are on the finite edges, we're done. 01825 if (*out_a1 >= 0.0 && *out_a1 <= 1.0 && *out_a2 >= 0.0 && *out_a2 <= 1.0) 01826 { 01827 float p1[3], p2[3]; 01828 01829 // p1= np11 + (np12 - np11) * out_a1; 01830 VECSUB(temp, np12, np11); 01831 mul_v3_fl(temp, *out_a1); 01832 VECADD(p1, np11, temp); 01833 01834 // p2 = np21 + (np22 - np21) * out_a2; 01835 VECSUB(temp, np22, np21); 01836 mul_v3_fl(temp, *out_a2); 01837 VECADD(p2, np21, temp); 01838 01839 calculateEENormal(np11, np12, np21, np22, out_normal); 01840 VECSUB(temp, p1, p2); 01841 return INPR(temp, temp); 01842 } 01843 01844 01845 /* 01846 * Clamp both points to the finite edges. 01847 * The one that moves most during clamping is one part of the solution. 01848 */ 01849 dist_a1 = *out_a1; 01850 CLAMP(dist_a1, 0.0, 1.0); 01851 dist_a2 = *out_a2; 01852 CLAMP(dist_a2, 0.0, 1.0); 01853 01854 // Now project the "most clamped" point on the other line. 01855 if (dist_a1 > dist_a2) 01856 { 01857 /* keep out_a1 */ 01858 float p1[3]; 01859 01860 // p1 = np11 + (np12 - np11) * out_a1; 01861 VECSUB(temp, np12, np11); 01862 mul_v3_fl(temp, *out_a1); 01863 VECADD(p1, np11, temp); 01864 01865 *out_a2 = projectPointOntoLine(p1, np21, np22); 01866 CLAMP(*out_a2, 0.0, 1.0); 01867 01868 calculateEENormal(np11, np12, np21, np22, out_normal); 01869 01870 // return (p1 - (np21 + (np22 - np21) * out_a2)).lengthSquared(); 01871 VECSUB(temp, np22, np21); 01872 mul_v3_fl(temp, *out_a2); 01873 VECADD(temp, temp, np21); 01874 VECSUB(temp, p1, temp); 01875 return INPR(temp, temp); 01876 } 01877 else 01878 { 01879 /* keep out_a2 */ 01880 float p2[3]; 01881 01882 // p2 = np21 + (np22 - np21) * out_a2; 01883 VECSUB(temp, np22, np21); 01884 mul_v3_fl(temp, *out_a2); 01885 VECADD(p2, np21, temp); 01886 01887 *out_a1 = projectPointOntoLine(p2, np11, np12); 01888 CLAMP(*out_a1, 0.0, 1.0); 01889 01890 calculateEENormal(np11, np12, np21, np22, out_normal); 01891 01892 // return ((np11 + (np12 - np11) * out_a1) - p2).lengthSquared(); 01893 VECSUB(temp, np12, np11); 01894 mul_v3_fl(temp, *out_a1); 01895 VECADD(temp, temp, np11); 01896 VECSUB(temp, temp, p2); 01897 return INPR(temp, temp); 01898 } 01899 } 01900 01901 printf("Error in edgedge_distance: end of function\n"); 01902 return 0; 01903 } 01904 01905 static int cloth_collision_moving_edges ( ClothModifierData *clmd, CollisionModifierData *collmd, CollPair *collpair ) 01906 { 01907 EdgeCollPair edgecollpair; 01908 Cloth *cloth1=NULL; 01909 ClothVertex *verts1=NULL; 01910 unsigned int i = 0, k = 0; 01911 int numsolutions = 0; 01912 double x1[3], v1[3], x2[3], v2[3], x3[3], v3[3]; 01913 double solution[3], solution2[3]; 01914 MVert *verts2 = collmd->current_x; // old x 01915 MVert *velocity2 = collmd->current_v; // velocity 01916 float distance = 0; 01917 float triA[3][3], triB[3][3]; 01918 int result = 0; 01919 01920 cloth1 = clmd->clothObject; 01921 verts1 = cloth1->verts; 01922 01923 for(i = 0; i < 9; i++) 01924 { 01925 // 9 edge - edge possibilities 01926 01927 if(i == 0) // cloth edge: 1-2; coll edge: 1-2 01928 { 01929 edgecollpair.p11 = collpair->ap1; 01930 edgecollpair.p12 = collpair->ap2; 01931 01932 edgecollpair.p21 = collpair->bp1; 01933 edgecollpair.p22 = collpair->bp2; 01934 } 01935 else if(i == 1) // cloth edge: 1-2; coll edge: 2-3 01936 { 01937 edgecollpair.p11 = collpair->ap1; 01938 edgecollpair.p12 = collpair->ap2; 01939 01940 edgecollpair.p21 = collpair->bp2; 01941 edgecollpair.p22 = collpair->bp3; 01942 } 01943 else if(i == 2) // cloth edge: 1-2; coll edge: 1-3 01944 { 01945 edgecollpair.p11 = collpair->ap1; 01946 edgecollpair.p12 = collpair->ap2; 01947 01948 edgecollpair.p21 = collpair->bp1; 01949 edgecollpair.p22 = collpair->bp3; 01950 } 01951 else if(i == 3) // cloth edge: 2-3; coll edge: 1-2 01952 { 01953 edgecollpair.p11 = collpair->ap2; 01954 edgecollpair.p12 = collpair->ap3; 01955 01956 edgecollpair.p21 = collpair->bp1; 01957 edgecollpair.p22 = collpair->bp2; 01958 } 01959 else if(i == 4) // cloth edge: 2-3; coll edge: 2-3 01960 { 01961 edgecollpair.p11 = collpair->ap2; 01962 edgecollpair.p12 = collpair->ap3; 01963 01964 edgecollpair.p21 = collpair->bp2; 01965 edgecollpair.p22 = collpair->bp3; 01966 } 01967 else if(i == 5) // cloth edge: 2-3; coll edge: 1-3 01968 { 01969 edgecollpair.p11 = collpair->ap2; 01970 edgecollpair.p12 = collpair->ap3; 01971 01972 edgecollpair.p21 = collpair->bp1; 01973 edgecollpair.p22 = collpair->bp3; 01974 } 01975 else if(i ==6) // cloth edge: 1-3; coll edge: 1-2 01976 { 01977 edgecollpair.p11 = collpair->ap1; 01978 edgecollpair.p12 = collpair->ap3; 01979 01980 edgecollpair.p21 = collpair->bp1; 01981 edgecollpair.p22 = collpair->bp2; 01982 } 01983 else if(i ==7) // cloth edge: 1-3; coll edge: 2-3 01984 { 01985 edgecollpair.p11 = collpair->ap1; 01986 edgecollpair.p12 = collpair->ap3; 01987 01988 edgecollpair.p21 = collpair->bp2; 01989 edgecollpair.p22 = collpair->bp3; 01990 } 01991 else if(i == 8) // cloth edge: 1-3; coll edge: 1-3 01992 { 01993 edgecollpair.p11 = collpair->ap1; 01994 edgecollpair.p12 = collpair->ap3; 01995 01996 edgecollpair.p21 = collpair->bp1; 01997 edgecollpair.p22 = collpair->bp3; 01998 } 01999 /* 02000 if((edgecollpair.p11 == 3) && (edgecollpair.p12 == 16)) 02001 printf("Ahier!\n"); 02002 if((edgecollpair.p11 == 16) && (edgecollpair.p12 == 3)) 02003 printf("Ahier!\n"); 02004 */ 02005 02006 // if ( !cloth_are_edges_adjacent ( clmd, collmd, &edgecollpair ) ) 02007 { 02008 // always put coll points in p21/p22 02009 VECSUB ( x1, verts1[edgecollpair.p12].txold, verts1[edgecollpair.p11].txold ); 02010 VECSUB ( v1, verts1[edgecollpair.p12].tv, verts1[edgecollpair.p11].tv ); 02011 02012 VECSUB ( x2, verts2[edgecollpair.p21].co, verts1[edgecollpair.p11].txold ); 02013 VECSUB ( v2, velocity2[edgecollpair.p21].co, verts1[edgecollpair.p11].tv ); 02014 02015 VECSUB ( x3, verts2[edgecollpair.p22].co, verts1[edgecollpair.p11].txold ); 02016 VECSUB ( v3, velocity2[edgecollpair.p22].co, verts1[edgecollpair.p11].tv ); 02017 02018 numsolutions = cloth_get_collision_time ( x1, v1, x2, v2, x3, v3, solution ); 02019 02020 if((edgecollpair.p11 == 3 && edgecollpair.p12==16)|| (edgecollpair.p11==16 && edgecollpair.p12==3)) 02021 { 02022 if(edgecollpair.p21==6 || edgecollpair.p22 == 6) 02023 { 02024 printf("dist: %f, sol[k]: %lf, sol2[k]: %lf\n", distance, solution[k], solution2[k]); 02025 printf("a1: %f, a2: %f, b1: %f, b2: %f\n", x1[0], x2[0], x3[0], v1[0]); 02026 printf("b21: %d, b22: %d\n", edgecollpair.p21, edgecollpair.p22); 02027 } 02028 } 02029 02030 for ( k = 0; k < numsolutions; k++ ) 02031 { 02032 // printf("sol %d: %lf\n", k, solution[k]); 02033 if ( ( solution[k] >= ALMOST_ZERO ) && ( solution[k] <= 1.0 ) && ( solution[k] > ALMOST_ZERO)) 02034 { 02035 float a,b; 02036 float out_normal[3]; 02037 float distance; 02038 float impulse = 0; 02039 float I_mag; 02040 02041 // move verts 02042 VECADDS(triA[0], verts1[edgecollpair.p11].txold, verts1[edgecollpair.p11].tv, solution[k]); 02043 VECADDS(triA[1], verts1[edgecollpair.p12].txold, verts1[edgecollpair.p12].tv, solution[k]); 02044 02045 VECADDS(triB[0], collmd->current_x[edgecollpair.p21].co, collmd->current_v[edgecollpair.p21].co, solution[k]); 02046 VECADDS(triB[1], collmd->current_x[edgecollpair.p22].co, collmd->current_v[edgecollpair.p22].co, solution[k]); 02047 02048 // TODO: check for collisions 02049 distance = edgedge_distance(triA[0], triA[1], triB[0], triB[1], &a, &b, out_normal); 02050 02051 if ((distance <= clmd->coll_parms->epsilon + BLI_bvhtree_getepsilon ( collmd->bvhtree ) + ALMOST_ZERO) && (INPR(out_normal, out_normal) > 0)) 02052 { 02053 float vrel_1_to_2[3], temp[3], temp2[3], out_normalVelocity; 02054 float desiredVn; 02055 02056 VECCOPY(vrel_1_to_2, verts1[edgecollpair.p11].tv); 02057 mul_v3_fl(vrel_1_to_2, 1.0 - a); 02058 VECCOPY(temp, verts1[edgecollpair.p12].tv); 02059 mul_v3_fl(temp, a); 02060 02061 VECADD(vrel_1_to_2, vrel_1_to_2, temp); 02062 02063 VECCOPY(temp, verts1[edgecollpair.p21].tv); 02064 mul_v3_fl(temp, 1.0 - b); 02065 VECCOPY(temp2, verts1[edgecollpair.p22].tv); 02066 mul_v3_fl(temp2, b); 02067 VECADD(temp, temp, temp2); 02068 02069 VECSUB(vrel_1_to_2, vrel_1_to_2, temp); 02070 02071 out_normalVelocity = INPR(vrel_1_to_2, out_normal); 02072 /* 02073 // this correction results in wrong normals sometimes? 02074 if(out_normalVelocity < 0.0) 02075 { 02076 out_normalVelocity*= -1.0; 02077 negate_v3(out_normal); 02078 } 02079 */ 02080 /* Inelastic repulsion impulse. */ 02081 02082 // Calculate which normal velocity we need. 02083 desiredVn = (out_normalVelocity * (float)solution[k] - (.1 * (clmd->coll_parms->epsilon + BLI_bvhtree_getepsilon ( collmd->bvhtree )) - sqrt(distance)) - ALMOST_ZERO); 02084 02085 // Now calculate what impulse we need to reach that velocity. 02086 I_mag = (out_normalVelocity - desiredVn) / 2.0; // / (1/m1 + 1/m2); 02087 02088 // Finally apply that impulse. 02089 impulse = (2.0 * -I_mag) / (a*a + (1.0-a)*(1.0-a) + b*b + (1.0-b)*(1.0-b)); 02090 02091 VECADDMUL ( verts1[edgecollpair.p11].impulse, out_normal, (1.0-a) * impulse ); 02092 verts1[edgecollpair.p11].impulse_count++; 02093 02094 VECADDMUL ( verts1[edgecollpair.p12].impulse, out_normal, a * impulse ); 02095 verts1[edgecollpair.p12].impulse_count++; 02096 02097 // return true; 02098 result = 1; 02099 break; 02100 } 02101 else 02102 { 02103 // missing from collision.hpp 02104 } 02105 // mintime = MIN2(mintime, (float)solution[k]); 02106 02107 break; 02108 } 02109 } 02110 } 02111 } 02112 return result; 02113 } 02114 02115 static int cloth_collision_moving ( ClothModifierData *clmd, CollisionModifierData *collmd, CollPair *collpair, CollPair *collision_end ) 02116 { 02117 Cloth *cloth1; 02118 cloth1 = clmd->clothObject; 02119 02120 for ( ; collpair != collision_end; collpair++ ) 02121 { 02122 // only handle moving collisions here 02123 if (!( collpair->flag & COLLISION_IN_FUTURE )) 02124 continue; 02125 02126 cloth_collision_moving_edges ( clmd, collmd, collpair); 02127 // cloth_collision_moving_tris ( clmd, collmd, collpair); 02128 } 02129 02130 return 1; 02131 } 02132 #endif 02133 02134 static void add_collision_object(Object ***objs, unsigned int *numobj, unsigned int *maxobj, Object *ob, Object *self, int level) 02135 { 02136 CollisionModifierData *cmd= NULL; 02137 02138 if(ob == self) 02139 return; 02140 02141 /* only get objects with collision modifier */ 02142 if(ob->pd && ob->pd->deflect) 02143 cmd= (CollisionModifierData *)modifiers_findByType(ob, eModifierType_Collision); 02144 02145 if(cmd) { 02146 /* extend array */ 02147 if(*numobj >= *maxobj) { 02148 *maxobj *= 2; 02149 *objs= MEM_reallocN(*objs, sizeof(Object*)*(*maxobj)); 02150 } 02151 02152 (*objs)[*numobj] = ob; 02153 (*numobj)++; 02154 } 02155 02156 /* objects in dupli groups, one level only for now */ 02157 if(ob->dup_group && level == 0) { 02158 GroupObject *go; 02159 Group *group= ob->dup_group; 02160 02161 /* add objects */ 02162 for(go= group->gobject.first; go; go= go->next) 02163 add_collision_object(objs, numobj, maxobj, go->ob, self, level+1); 02164 } 02165 } 02166 02167 // return all collision objects in scene 02168 // collision object will exclude self 02169 Object **get_collisionobjects(Scene *scene, Object *self, Group *group, unsigned int *numcollobj) 02170 { 02171 Base *base; 02172 Object **objs; 02173 GroupObject *go; 02174 unsigned int numobj= 0, maxobj= 100; 02175 02176 objs= MEM_callocN(sizeof(Object *)*maxobj, "CollisionObjectsArray"); 02177 02178 /* gather all collision objects */ 02179 if(group) { 02180 /* use specified group */ 02181 for(go= group->gobject.first; go; go= go->next) 02182 add_collision_object(&objs, &numobj, &maxobj, go->ob, self, 0); 02183 } 02184 else { 02185 Scene *sce_iter; 02186 /* add objects in same layer in scene */ 02187 for(SETLOOPER(scene, sce_iter, base)) { 02188 if(base->lay & self->lay) 02189 add_collision_object(&objs, &numobj, &maxobj, base->object, self, 0); 02190 02191 } 02192 } 02193 02194 *numcollobj= numobj; 02195 02196 return objs; 02197 } 02198 02199 static void add_collider_cache_object(ListBase **objs, Object *ob, Object *self, int level) 02200 { 02201 CollisionModifierData *cmd= NULL; 02202 ColliderCache *col; 02203 02204 if(ob == self) 02205 return; 02206 02207 if(ob->pd && ob->pd->deflect) 02208 cmd =(CollisionModifierData *)modifiers_findByType(ob, eModifierType_Collision); 02209 02210 if(cmd && cmd->bvhtree) { 02211 if(*objs == NULL) 02212 *objs = MEM_callocN(sizeof(ListBase), "ColliderCache array"); 02213 02214 col = MEM_callocN(sizeof(ColliderCache), "ColliderCache"); 02215 col->ob = ob; 02216 col->collmd = cmd; 02217 /* make sure collider is properly set up */ 02218 collision_move_object(cmd, 1.0, 0.0); 02219 BLI_addtail(*objs, col); 02220 } 02221 02222 /* objects in dupli groups, one level only for now */ 02223 if(ob->dup_group && level == 0) { 02224 GroupObject *go; 02225 Group *group= ob->dup_group; 02226 02227 /* add objects */ 02228 for(go= group->gobject.first; go; go= go->next) 02229 add_collider_cache_object(objs, go->ob, self, level+1); 02230 } 02231 } 02232 02233 ListBase *get_collider_cache(Scene *scene, Object *self, Group *group) 02234 { 02235 GroupObject *go; 02236 ListBase *objs= NULL; 02237 02238 /* add object in same layer in scene */ 02239 if(group) { 02240 for(go= group->gobject.first; go; go= go->next) 02241 add_collider_cache_object(&objs, go->ob, self, 0); 02242 } 02243 else { 02244 Scene *sce_iter; 02245 Base *base; 02246 02247 /* add objects in same layer in scene */ 02248 for(SETLOOPER(scene, sce_iter, base)) { 02249 if(!self || (base->lay & self->lay)) 02250 add_collider_cache_object(&objs, base->object, self, 0); 02251 02252 } 02253 } 02254 02255 return objs; 02256 } 02257 02258 void free_collider_cache(ListBase **colliders) 02259 { 02260 if(*colliders) { 02261 BLI_freelistN(*colliders); 02262 MEM_freeN(*colliders); 02263 *colliders = NULL; 02264 } 02265 } 02266 02267 02268 static void cloth_bvh_objcollisions_nearcheck ( ClothModifierData * clmd, CollisionModifierData *collmd, 02269 CollPair **collisions, CollPair **collisions_index, int numresult, BVHTreeOverlap *overlap, double dt) 02270 { 02271 int i; 02272 #ifdef WITH_ELTOPO 02273 GHash *visithash = BLI_ghash_new(edgepair_hash, edgepair_cmp, "visthash, collision.c"); 02274 GHash *tri_visithash = BLI_ghash_new(tripair_hash, tripair_cmp, "tri_visthash, collision.c"); 02275 MemArena *arena = BLI_memarena_new(1<<16, "edge hash arena, collision.c"); 02276 #endif 02277 02278 *collisions = ( CollPair* ) MEM_mallocN ( sizeof ( CollPair ) * numresult * 64, "collision array" ); //*4 since cloth_collision_static can return more than 1 collision 02279 *collisions_index = *collisions; 02280 02281 #ifdef WITH_ELTOPO 02282 machine_epsilon_offset(clmd->clothObject); 02283 02284 for ( i = 0; i < numresult; i++ ) 02285 { 02286 *collisions_index = cloth_collision ( ( ModifierData * ) clmd, ( ModifierData * ) collmd, 02287 overlap+i, *collisions_index, dt, tri_visithash, arena ); 02288 } 02289 02290 for ( i = 0; i < numresult; i++ ) 02291 { 02292 *collisions_index = cloth_edge_collision ( ( ModifierData * ) clmd, ( ModifierData * ) collmd, 02293 overlap+i, *collisions_index, visithash, arena ); 02294 } 02295 BLI_ghash_free(visithash, NULL, NULL); 02296 BLI_ghash_free(tri_visithash, NULL, NULL); 02297 BLI_memarena_free(arena); 02298 #else /* WITH_ELTOPO */ 02299 for ( i = 0; i < numresult; i++ ) 02300 { 02301 *collisions_index = cloth_collision ( ( ModifierData * ) clmd, ( ModifierData * ) collmd, 02302 overlap+i, *collisions_index, dt ); 02303 } 02304 #endif /* WITH_ELTOPO */ 02305 02306 } 02307 02308 static int cloth_bvh_objcollisions_resolve ( ClothModifierData * clmd, CollisionModifierData *collmd, CollPair *collisions, CollPair *collisions_index) 02309 { 02310 Cloth *cloth = clmd->clothObject; 02311 int i=0, j = 0, /*numfaces = 0,*/ numverts = 0; 02312 ClothVertex *verts = NULL; 02313 int ret = 0; 02314 int result = 0; 02315 float tnull[3] = {0,0,0}; 02316 02317 /*numfaces = clmd->clothObject->numfaces;*/ /*UNUSED*/ 02318 numverts = clmd->clothObject->numverts; 02319 02320 verts = cloth->verts; 02321 02322 // process all collisions (calculate impulses, TODO: also repulses if distance too short) 02323 result = 1; 02324 for ( j = 0; j < 5; j++ ) // 5 is just a value that ensures convergence 02325 { 02326 result = 0; 02327 02328 if ( collmd->bvhtree ) 02329 { 02330 #ifdef WITH_ELTOPO 02331 result += cloth_collision_response_moving(clmd, collmd, collisions, collisions_index); 02332 result += cloth_edge_collision_response_moving(clmd, collmd, collisions, collisions_index); 02333 #else 02334 result += cloth_collision_response_static ( clmd, collmd, collisions, collisions_index ); 02335 #endif 02336 #ifdef WITH_ELTOPO 02337 { 02338 #else 02339 // apply impulses in parallel 02340 if ( result ) 02341 { 02342 #endif 02343 for ( i = 0; i < numverts; i++ ) 02344 { 02345 // calculate "velocities" (just xnew = xold + v; no dt in v) 02346 if ( verts[i].impulse_count ) 02347 { 02348 VECADDMUL ( verts[i].tv, verts[i].impulse, 1.0f / verts[i].impulse_count ); 02349 VECCOPY ( verts[i].impulse, tnull ); 02350 verts[i].impulse_count = 0; 02351 02352 ret++; 02353 } 02354 } 02355 } 02356 } 02357 } 02358 return ret; 02359 } 02360 02361 // cloth - object collisions 02362 int cloth_bvh_objcollision (Object *ob, ClothModifierData * clmd, float step, float dt ) 02363 { 02364 Cloth *cloth= clmd->clothObject; 02365 BVHTree *cloth_bvh= cloth->bvhtree; 02366 unsigned int i=0, numfaces = 0, numverts = 0, k, l, j; 02367 int rounds = 0; // result counts applied collisions; ic is for debug output; 02368 ClothVertex *verts = NULL; 02369 int ret = 0, ret2 = 0; 02370 Object **collobjs = NULL; 02371 unsigned int numcollobj = 0; 02372 02373 if ((clmd->sim_parms->flags & CLOTH_SIMSETTINGS_FLAG_COLLOBJ) || cloth_bvh==NULL) 02374 return 0; 02375 02376 verts = cloth->verts; 02377 numfaces = cloth->numfaces; 02378 numverts = cloth->numverts; 02379 02381 // static collisions 02383 02384 // update cloth bvh 02385 bvhtree_update_from_cloth ( clmd, 1 ); // 0 means STATIC, 1 means MOVING (see later in this function) 02386 bvhselftree_update_from_cloth ( clmd, 0 ); // 0 means STATIC, 1 means MOVING (see later in this function) 02387 02388 collobjs = get_collisionobjects(clmd->scene, ob, clmd->coll_parms->group, &numcollobj); 02389 02390 if(!collobjs) 02391 return 0; 02392 02393 do 02394 { 02395 CollPair **collisions, **collisions_index; 02396 02397 ret2 = 0; 02398 02399 collisions = MEM_callocN(sizeof(CollPair *) *numcollobj , "CollPair"); 02400 collisions_index = MEM_callocN(sizeof(CollPair *) *numcollobj , "CollPair"); 02401 02402 // check all collision objects 02403 for(i = 0; i < numcollobj; i++) 02404 { 02405 Object *collob= collobjs[i]; 02406 CollisionModifierData *collmd = (CollisionModifierData*)modifiers_findByType(collob, eModifierType_Collision); 02407 BVHTreeOverlap *overlap = NULL; 02408 unsigned int result = 0; 02409 02410 if(!collmd->bvhtree) 02411 continue; 02412 02413 /* move object to position (step) in time */ 02414 02415 collision_move_object ( collmd, step + dt, step ); 02416 02417 /* search for overlapping collision pairs */ 02418 overlap = BLI_bvhtree_overlap ( cloth_bvh, collmd->bvhtree, &result ); 02419 02420 // go to next object if no overlap is there 02421 if( result && overlap ) { 02422 /* check if collisions really happen (costly near check) */ 02423 cloth_bvh_objcollisions_nearcheck ( clmd, collmd, &collisions[i], 02424 &collisions_index[i], result, overlap, dt/(float)clmd->coll_parms->loop_count); 02425 02426 // resolve nearby collisions 02427 ret += cloth_bvh_objcollisions_resolve ( clmd, collmd, collisions[i], collisions_index[i]); 02428 ret2 += ret; 02429 } 02430 02431 if ( overlap ) 02432 MEM_freeN ( overlap ); 02433 } 02434 rounds++; 02435 02436 for(i = 0; i < numcollobj; i++) 02437 { 02438 if ( collisions[i] ) MEM_freeN ( collisions[i] ); 02439 } 02440 02441 MEM_freeN(collisions); 02442 MEM_freeN(collisions_index); 02443 02445 // update positions 02446 // this is needed for bvh_calc_DOP_hull_moving() [kdop.c] 02448 02449 // verts come from clmd 02450 for ( i = 0; i < numverts; i++ ) 02451 { 02452 if ( clmd->sim_parms->flags & CLOTH_SIMSETTINGS_FLAG_GOAL ) 02453 { 02454 if ( verts [i].flags & CLOTH_VERT_FLAG_PINNED ) 02455 { 02456 continue; 02457 } 02458 } 02459 02460 VECADD ( verts[i].tx, verts[i].txold, verts[i].tv ); 02461 } 02463 02464 02466 // Test on *simple* selfcollisions 02468 if ( clmd->coll_parms->flags & CLOTH_COLLSETTINGS_FLAG_SELF ) 02469 { 02470 for(l = 0; l < (unsigned int)clmd->coll_parms->self_loop_count; l++) 02471 { 02472 // TODO: add coll quality rounds again 02473 BVHTreeOverlap *overlap = NULL; 02474 unsigned int result = 0; 02475 02476 // collisions = 1; 02477 verts = cloth->verts; // needed for openMP 02478 02479 numfaces = cloth->numfaces; 02480 numverts = cloth->numverts; 02481 02482 verts = cloth->verts; 02483 02484 if ( cloth->bvhselftree ) 02485 { 02486 // search for overlapping collision pairs 02487 overlap = BLI_bvhtree_overlap ( cloth->bvhselftree, cloth->bvhselftree, &result ); 02488 02489 // #pragma omp parallel for private(k, i, j) schedule(static) 02490 for ( k = 0; k < result; k++ ) 02491 { 02492 float temp[3]; 02493 float length = 0; 02494 float mindistance; 02495 02496 i = overlap[k].indexA; 02497 j = overlap[k].indexB; 02498 02499 mindistance = clmd->coll_parms->selfepsilon* ( cloth->verts[i].avg_spring_len + cloth->verts[j].avg_spring_len ); 02500 02501 if ( clmd->sim_parms->flags & CLOTH_SIMSETTINGS_FLAG_GOAL ) 02502 { 02503 if ( ( cloth->verts [i].flags & CLOTH_VERT_FLAG_PINNED ) 02504 && ( cloth->verts [j].flags & CLOTH_VERT_FLAG_PINNED ) ) 02505 { 02506 continue; 02507 } 02508 } 02509 02510 VECSUB ( temp, verts[i].tx, verts[j].tx ); 02511 02512 if ( ( ABS ( temp[0] ) > mindistance ) || ( ABS ( temp[1] ) > mindistance ) || ( ABS ( temp[2] ) > mindistance ) ) continue; 02513 02514 // check for adjacent points (i must be smaller j) 02515 if ( BLI_edgehash_haskey ( cloth->edgehash, MIN2(i, j), MAX2(i, j) ) ) 02516 { 02517 continue; 02518 } 02519 02520 length = normalize_v3( temp ); 02521 02522 if ( length < mindistance ) 02523 { 02524 float correction = mindistance - length; 02525 02526 if ( cloth->verts [i].flags & CLOTH_VERT_FLAG_PINNED ) 02527 { 02528 mul_v3_fl( temp, -correction ); 02529 VECADD ( verts[j].tx, verts[j].tx, temp ); 02530 } 02531 else if ( cloth->verts [j].flags & CLOTH_VERT_FLAG_PINNED ) 02532 { 02533 mul_v3_fl( temp, correction ); 02534 VECADD ( verts[i].tx, verts[i].tx, temp ); 02535 } 02536 else 02537 { 02538 mul_v3_fl( temp, -correction*0.5 ); 02539 VECADD ( verts[j].tx, verts[j].tx, temp ); 02540 02541 VECSUB ( verts[i].tx, verts[i].tx, temp ); 02542 } 02543 ret = 1; 02544 ret2 += ret; 02545 } 02546 else 02547 { 02548 // check for approximated time collisions 02549 } 02550 } 02551 02552 if ( overlap ) 02553 MEM_freeN ( overlap ); 02554 02555 } 02556 } 02558 02560 // SELFCOLLISIONS: update velocities 02562 if ( ret2 ) 02563 { 02564 for ( i = 0; i < cloth->numverts; i++ ) 02565 { 02566 if ( ! ( verts [i].flags & CLOTH_VERT_FLAG_PINNED ) ) 02567 { 02568 VECSUB ( verts[i].tv, verts[i].tx, verts[i].txold ); 02569 } 02570 } 02571 } 02573 } 02574 } 02575 while ( ret2 && ( clmd->coll_parms->loop_count>rounds ) ); 02576 02577 if(collobjs) 02578 MEM_freeN(collobjs); 02579 02580 return 1|MIN2 ( ret, 1 ); 02581 }