diff options
Diffstat (limited to 'source/blender/blenkernel/intern/collision.c')
| -rw-r--r-- | source/blender/blenkernel/intern/collision.c | 780 |
1 files changed, 650 insertions, 130 deletions
diff --git a/source/blender/blenkernel/intern/collision.c b/source/blender/blenkernel/intern/collision.c index 2f600935b1e..d35762a6f13 100644 --- a/source/blender/blenkernel/intern/collision.c +++ b/source/blender/blenkernel/intern/collision.c @@ -33,6 +33,7 @@ #include "MEM_guardedalloc.h" #include "DNA_cloth_types.h" +#include "DNA_effect_types.h" #include "DNA_group_types.h" #include "DNA_object_types.h" #include "DNA_object_force.h" @@ -45,6 +46,7 @@ #include "BLI_edgehash.h" #include "BKE_cloth.h" +#include "BKE_effect.h" #include "BKE_modifier.h" #include "BKE_scene.h" @@ -69,82 +71,92 @@ void collision_move_object(CollisionModifierData *collmd, float step, float prev float tv[3] = {0, 0, 0}; unsigned int i = 0; - for ( i = 0; i < collmd->numverts; i++ ) { + for (i = 0; i < collmd->mvert_num; i++) { sub_v3_v3v3(tv, collmd->xnew[i].co, collmd->x[i].co); VECADDS(collmd->current_x[i].co, collmd->x[i].co, tv, prevstep); VECADDS(collmd->current_xnew[i].co, collmd->x[i].co, tv, step); sub_v3_v3v3(collmd->current_v[i].co, collmd->current_xnew[i].co, collmd->current_x[i].co); } - bvhtree_update_from_mvert ( collmd->bvhtree, collmd->mfaces, collmd->numfaces, collmd->current_x, collmd->current_xnew, collmd->numverts, 1 ); + bvhtree_update_from_mvert( + collmd->bvhtree, collmd->current_x, collmd->current_xnew, + collmd->tri, collmd->tri_num, true); } -BVHTree *bvhtree_build_from_mvert ( MFace *mfaces, unsigned int numfaces, MVert *x, unsigned int UNUSED(numverts), float epsilon ) +BVHTree *bvhtree_build_from_mvert( + const MVert *mvert, + const struct MVertTri *tri, int tri_num, + float epsilon) { BVHTree *tree; - float co[12]; - unsigned int i; - MFace *tface = mfaces; + const MVertTri *vt; + int i; + + tree = BLI_bvhtree_new(tri_num, epsilon, 4, 26); - tree = BLI_bvhtree_new ( numfaces*2, epsilon, 4, 26 ); + /* fill tree */ + for (i = 0, vt = tri; i < tri_num; i++, vt++) { + float co[3][3]; - // fill tree - for ( i = 0; i < numfaces; i++, tface++ ) { - copy_v3_v3 ( &co[0*3], x[tface->v1].co ); - copy_v3_v3 ( &co[1*3], x[tface->v2].co ); - copy_v3_v3 ( &co[2*3], x[tface->v3].co ); - if ( tface->v4 ) - copy_v3_v3 ( &co[3*3], x[tface->v4].co ); + copy_v3_v3(co[0], mvert[vt->tri[0]].co); + copy_v3_v3(co[1], mvert[vt->tri[1]].co); + copy_v3_v3(co[2], mvert[vt->tri[2]].co); - BLI_bvhtree_insert ( tree, i, co, ( mfaces->v4 ? 4 : 3 ) ); + BLI_bvhtree_insert(tree, i, co[0], 3); } - // balance tree - BLI_bvhtree_balance ( tree ); + /* balance tree */ + BLI_bvhtree_balance(tree); return tree; } -void bvhtree_update_from_mvert(BVHTree *bvhtree, MFace *faces, int numfaces, MVert *x, MVert *xnew, int UNUSED(numverts), int moving ) +void bvhtree_update_from_mvert( + BVHTree *bvhtree, + const MVert *mvert, const MVert *mvert_moving, + const MVertTri *tri, int tri_num, + bool moving) { + const MVertTri *vt; int i; - MFace *mfaces = faces; - float co[12], co_moving[12]; - bool ret = false; - if ( !bvhtree ) + if ((bvhtree == NULL) || (mvert == NULL)) { return; + } - if ( x ) { - for ( i = 0; i < numfaces; i++, mfaces++ ) { - copy_v3_v3 ( &co[0*3], x[mfaces->v1].co ); - copy_v3_v3 ( &co[1*3], x[mfaces->v2].co ); - copy_v3_v3 ( &co[2*3], x[mfaces->v3].co ); - if ( mfaces->v4 ) - copy_v3_v3 ( &co[3*3], x[mfaces->v4].co ); - - // copy new locations into array - if ( moving && xnew ) { - // update moving positions - copy_v3_v3 ( &co_moving[0*3], xnew[mfaces->v1].co ); - copy_v3_v3 ( &co_moving[1*3], xnew[mfaces->v2].co ); - copy_v3_v3 ( &co_moving[2*3], xnew[mfaces->v3].co ); - if ( mfaces->v4 ) - copy_v3_v3 ( &co_moving[3*3], xnew[mfaces->v4].co ); - - ret = BLI_bvhtree_update_node ( bvhtree, i, co, co_moving, ( mfaces->v4 ? 4 : 3 ) ); - } - else { - ret = BLI_bvhtree_update_node ( bvhtree, i, co, NULL, ( mfaces->v4 ? 4 : 3 ) ); - } + if (mvert_moving == NULL) { + moving = false; + } + + for (i = 0, vt = tri; i < tri_num; i++, vt++) { + float co[3][3]; + bool ret; + + copy_v3_v3(co[0], mvert[vt->tri[0]].co); + copy_v3_v3(co[1], mvert[vt->tri[1]].co); + copy_v3_v3(co[2], mvert[vt->tri[2]].co); - // check if tree is already full - if ( !ret ) - break; + /* copy new locations into array */ + if (moving) { + float co_moving[3][3]; + /* update moving positions */ + copy_v3_v3(co_moving[0], mvert_moving[vt->tri[0]].co); + copy_v3_v3(co_moving[1], mvert_moving[vt->tri[1]].co); + copy_v3_v3(co_moving[2], mvert_moving[vt->tri[2]].co); + + ret = BLI_bvhtree_update_node(bvhtree, i, &co[0][0], &co_moving[0][0], 3); + } + else { + ret = BLI_bvhtree_update_node(bvhtree, i, &co[0][0], NULL, 3); } - BLI_bvhtree_update_tree ( bvhtree ); + /* check if tree is already full */ + if (ret == false) { + break; + } } + + BLI_bvhtree_update_tree(bvhtree); } /*********************************** @@ -378,80 +390,29 @@ static CollPair* cloth_collision(ModifierData *md1, ModifierData *md2, ClothModifierData *clmd = (ClothModifierData *)md1; CollisionModifierData *collmd = (CollisionModifierData *) md2; /* Cloth *cloth = clmd->clothObject; */ /* UNUSED */ - MFace *face1=NULL, *face2 = NULL; + const MVertTri *tri_a, *tri_b; #ifdef WITH_BULLET ClothVertex *verts1 = clmd->clothObject->verts; #endif double distance = 0; float epsilon1 = clmd->coll_parms->epsilon; float epsilon2 = BLI_bvhtree_getepsilon ( collmd->bvhtree ); - int i; - face1 = & ( clmd->clothObject->mfaces[overlap->indexA] ); - face2 = & ( collmd->mfaces[overlap->indexB] ); - - // check all 4 possible collisions - for ( i = 0; i < 4; i++ ) { - if ( i == 0 ) { - // fill faceA - collpair->ap1 = face1->v1; - collpair->ap2 = face1->v2; - collpair->ap3 = face1->v3; - - // fill faceB - collpair->bp1 = face2->v1; - collpair->bp2 = face2->v2; - collpair->bp3 = face2->v3; - } - else if ( i == 1 ) { - if ( face1->v4 ) { - // fill faceA - collpair->ap1 = face1->v1; - collpair->ap2 = face1->v3; - collpair->ap3 = face1->v4; - - // fill faceB - collpair->bp1 = face2->v1; - collpair->bp2 = face2->v2; - collpair->bp3 = face2->v3; - } - else { - i++; - } - } - if ( i == 2 ) { - if ( face2->v4 ) { - // fill faceA - collpair->ap1 = face1->v1; - collpair->ap2 = face1->v2; - collpair->ap3 = face1->v3; - - // fill faceB - collpair->bp1 = face2->v1; - collpair->bp2 = face2->v4; - collpair->bp3 = face2->v3; - } - else { - break; - } - } - else if ( i == 3 ) { - if ( face1->v4 && face2->v4 ) { - // fill faceA - collpair->ap1 = face1->v1; - collpair->ap2 = face1->v3; - collpair->ap3 = face1->v4; - - // fill faceB - collpair->bp1 = face2->v1; - collpair->bp2 = face2->v3; - collpair->bp3 = face2->v4; - } - else { - break; - } - } - + tri_a = &clmd->clothObject->tri[overlap->indexA]; + tri_b = &collmd->tri[overlap->indexB]; + + /* fill face_a */ + collpair->ap1 = tri_a->tri[0]; + collpair->ap2 = tri_a->tri[1]; + collpair->ap3 = tri_a->tri[2]; + + /* fill face_b */ + collpair->bp1 = tri_b->tri[0]; + collpair->bp2 = tri_b->tri[1]; + collpair->bp3 = tri_b->tri[2]; + + { + #ifdef WITH_BULLET // calc distance + normal distance = plNearestPoints ( @@ -648,24 +609,24 @@ static void cloth_bvh_objcollisions_nearcheck ( ClothModifierData * clmd, Collis { int i; - *collisions = (CollPair *) MEM_mallocN(sizeof(CollPair) * numresult * 64, "collision array" ); // * 4 since cloth_collision_static can return more than 1 collision + *collisions = (CollPair *) MEM_mallocN(sizeof(CollPair) * numresult * 4, "collision array" ); // * 4 since cloth_collision_static can return more than 1 collision *collisions_index = *collisions; for ( i = 0; i < numresult; i++ ) { - *collisions_index = cloth_collision ( (ModifierData *)clmd, (ModifierData *)collmd, - overlap+i, *collisions_index, dt ); + *collisions_index = cloth_collision((ModifierData *)clmd, (ModifierData *)collmd, + overlap+i, *collisions_index, dt); } } static int cloth_bvh_objcollisions_resolve ( ClothModifierData * clmd, CollisionModifierData *collmd, CollPair *collisions, CollPair *collisions_index) { Cloth *cloth = clmd->clothObject; - int i=0, j = 0, /*numfaces = 0, */ numverts = 0; + int i=0, j = 0, /*numfaces = 0, */ mvert_num = 0; ClothVertex *verts = NULL; int ret = 0; int result = 0; - numverts = clmd->clothObject->numverts; + mvert_num = clmd->clothObject->mvert_num; verts = cloth->verts; // process all collisions (calculate impulses, TODO: also repulses if distance too short) @@ -678,7 +639,7 @@ static int cloth_bvh_objcollisions_resolve ( ClothModifierData * clmd, Collision // apply impulses in parallel if (result) { - for (i = 0; i < numverts; i++) { + for (i = 0; i < mvert_num; i++) { // calculate "velocities" (just xnew = xold + v; no dt in v) if (verts[i].impulse_count) { // VECADDMUL ( verts[i].tv, verts[i].impulse, 1.0f / verts[i].impulse_count ); @@ -704,7 +665,7 @@ int cloth_bvh_objcollision(Object *ob, ClothModifierData *clmd, float step, floa { Cloth *cloth= clmd->clothObject; BVHTree *cloth_bvh= cloth->bvhtree; - unsigned int i=0, /* numfaces = 0, */ /* UNUSED */ numverts = 0, k, l, j; + unsigned int i=0, /* numfaces = 0, */ /* UNUSED */ mvert_num = 0, k, l, j; int rounds = 0; // result counts applied collisions; ic is for debug output; ClothVertex *verts = NULL; int ret = 0, ret2 = 0; @@ -716,7 +677,7 @@ int cloth_bvh_objcollision(Object *ob, ClothModifierData *clmd, float step, floa verts = cloth->verts; /* numfaces = cloth->numfaces; */ /* UNUSED */ - numverts = cloth->numverts; + mvert_num = cloth->mvert_num; //////////////////////////////////////////////////////////// // static collisions @@ -762,7 +723,7 @@ int cloth_bvh_objcollision(Object *ob, ClothModifierData *clmd, float step, floa continue; /* search for overlapping collision pairs */ - overlap = BLI_bvhtree_overlap ( cloth_bvh, collmd->bvhtree, &result ); + overlap = BLI_bvhtree_overlap(cloth_bvh, collmd->bvhtree, &result, NULL, NULL); // go to next object if no overlap is there if ( result && overlap ) { @@ -792,8 +753,8 @@ int cloth_bvh_objcollision(Object *ob, ClothModifierData *clmd, float step, floa // this is needed for bvh_calc_DOP_hull_moving() [kdop.c] //////////////////////////////////////////////////////////// - // verts come from clmd - for ( i = 0; i < numverts; i++ ) { + /* verts come from clmd */ + for (i = 0; i < mvert_num; i++) { if ( clmd->sim_parms->flags & CLOTH_SIMSETTINGS_FLAG_GOAL ) { if ( verts [i].flags & CLOTH_VERT_FLAG_PINNED ) { continue; @@ -818,13 +779,13 @@ int cloth_bvh_objcollision(Object *ob, ClothModifierData *clmd, float step, floa verts = cloth->verts; // needed for openMP /* numfaces = cloth->numfaces; */ /* UNUSED */ - numverts = cloth->numverts; + mvert_num = cloth->mvert_num; verts = cloth->verts; if ( cloth->bvhselftree ) { // search for overlapping collision pairs - overlap = BLI_bvhtree_overlap ( cloth->bvhselftree, cloth->bvhselftree, &result ); + overlap = BLI_bvhtree_overlap(cloth->bvhselftree, cloth->bvhselftree, &result, NULL, NULL); // #pragma omp parallel for private(k, i, j) schedule(static) for ( k = 0; k < result; k++ ) { @@ -896,8 +857,8 @@ int cloth_bvh_objcollision(Object *ob, ClothModifierData *clmd, float step, floa //////////////////////////////////////////////////////////// // SELFCOLLISIONS: update velocities //////////////////////////////////////////////////////////// - if ( ret2 ) { - for ( i = 0; i < cloth->numverts; i++ ) { + if (ret2) { + for (i = 0; i < cloth->mvert_num; i++) { if ( ! ( verts [i].flags & CLOTH_VERT_FLAG_PINNED ) ) { sub_v3_v3v3(verts[i].tv, verts[i].tx, verts[i].txold); } @@ -913,3 +874,562 @@ int cloth_bvh_objcollision(Object *ob, ClothModifierData *clmd, float step, floa return 1|MIN2 ( ret, 1 ); } + +BLI_INLINE void max_v3_v3v3(float r[3], const float a[3], const float b[3]) +{ + r[0] = max_ff(a[0], b[0]); + r[1] = max_ff(a[1], b[1]); + r[2] = max_ff(a[2], b[2]); +} + +void collision_get_collider_velocity(float vel_old[3], float vel_new[3], CollisionModifierData *collmd, CollPair *collpair) +{ + float u1, u2, u3; + + /* compute barycentric coordinates */ + collision_compute_barycentric(collpair->pb, + collmd->current_x[collpair->bp1].co, + collmd->current_x[collpair->bp2].co, + collmd->current_x[collpair->bp3].co, + &u1, &u2, &u3); + + collision_interpolateOnTriangle(vel_new, collmd->current_v[collpair->bp1].co, collmd->current_v[collpair->bp2].co, collmd->current_v[collpair->bp3].co, u1, u2, u3); + /* XXX assume constant velocity of the collider for now */ + copy_v3_v3(vel_old, vel_new); +} + +static bool cloth_points_collision_response_static(ClothModifierData *clmd, CollisionModifierData *collmd, PartDeflect *pd, + CollPair *collpair, CollPair *collision_end, float dt) +{ + bool result = false; + float restitution = (1.0f - clmd->coll_parms->damping) * (1.0f - pd->pdef_sbdamp); + float inv_dt = 1.0f / dt; + Cloth *cloth1 = clmd->clothObject; + + // float w1, w2; + float u1, u2, u3; + float v1[3], v2_old[3], v2_new[3], v_rel_old[3], v_rel_new[3]; + float epsilon2 = BLI_bvhtree_getepsilon ( collmd->bvhtree ); + + for ( ; collpair != collision_end; collpair++ ) { + float margin_distance = (float)(collpair->distance - (double)epsilon2); + float impulse[3]; + float mag_v_rel; + + if (margin_distance > 0.0f) + continue; + + zero_v3(impulse); + + /* only handle static collisions here */ + if ( collpair->flag & COLLISION_IN_FUTURE ) + continue; + + /* compute barycentric coordinates for both collision points */ + // w1 = 1.0f - collpair->time; + // w2 = collpair->time; + + /* was: txold */ + collision_compute_barycentric ( collpair->pb, + collmd->current_x[collpair->bp1].co, + collmd->current_x[collpair->bp2].co, + collmd->current_x[collpair->bp3].co, + &u1, &u2, &u3 ); + + /* Calculate relative velocity */ + copy_v3_v3(v1, cloth1->verts[collpair->ap1].tv); + + collision_interpolateOnTriangle ( v2_new, collmd->current_v[collpair->bp1].co, collmd->current_v[collpair->bp2].co, collmd->current_v[collpair->bp3].co, u1, u2, u3 ); + /* XXX assume constant velocity of the collider for now */ + copy_v3_v3(v2_old, v2_new); + + sub_v3_v3v3(v_rel_old, v1, v2_old); + sub_v3_v3v3(v_rel_new, v1, v2_new); + + /* normal component of the relative velocity */ + mag_v_rel = dot_v3v3(v_rel_old, collpair->normal); + + /**** DEBUG ****/ + BKE_sim_debug_data_add_dot(collpair->pa, 0.9, 0.2, 0.2, "collision", 833, collpair->face1, collpair->face2); + BKE_sim_debug_data_add_dot(collpair->pb, 0.2, 0.9, 0.2, "collision", 834, collpair->face1, collpair->face2); + BKE_sim_debug_data_add_line(collpair->pa, collpair->pb, 0.8, 0.8, 0.8, "collision", 835, collpair->face1, collpair->face2); + /********/ + + if (mag_v_rel < -ALMOST_ZERO) { + float v_nor_old, v_nor_new; + float v_tan_old[3], v_tan_new[3]; + float bounce, repulse; + + /* Collision response based on + * "Simulating Complex Hair with Robust Collision Handling" (Choe, Choi, Ko, ACM SIGGRAPH 2005) + * http://graphics.snu.ac.kr/publications/2005-choe-HairSim/Choe_2005_SCA.pdf + */ + + v_nor_old = mag_v_rel; + v_nor_new = dot_v3v3(v_rel_new, collpair->normal); + + madd_v3_v3v3fl(v_tan_old, v_rel_old, collpair->normal, -v_nor_old); + madd_v3_v3v3fl(v_tan_new, v_rel_new, collpair->normal, -v_nor_new); + + repulse = -margin_distance * inv_dt + dot_v3v3(v1, collpair->normal); + + if (margin_distance < -epsilon2) { + bounce = -v_nor_new + v_nor_old * restitution; + mul_v3_v3fl(impulse, collpair->normal, max_ff(repulse, bounce)); + } + else { + bounce = 0.0f; + mul_v3_v3fl(impulse, collpair->normal, repulse); + } + cloth1->verts[collpair->ap1].impulse_count++; + + result = true; + } + + if (result) { + int i = 0; + + for (i = 0; i < 3; i++) { + if (cloth1->verts[collpair->ap1].impulse_count > 0 && fabsf(cloth1->verts[collpair->ap1].impulse[i]) < fabsf(impulse[i])) + cloth1->verts[collpair->ap1].impulse[i] = impulse[i]; + } + } + } + return result; +} + +BLI_INLINE bool cloth_point_face_collision_params(const float p1[3], const float p2[3], const float v0[3], const float v1[3], const float v2[3], + float r_nor[3], float *r_lambda, float r_w[4]) +{ + float edge1[3], edge2[3], p2face[3], p1p2[3], v0p2[3]; + float nor_v0p2, nor_p1p2; + + sub_v3_v3v3(edge1, v1, v0); + sub_v3_v3v3(edge2, v2, v0); + cross_v3_v3v3(r_nor, edge1, edge2); + normalize_v3(r_nor); + + nor_v0p2 = dot_v3v3(v0p2, r_nor); + madd_v3_v3v3fl(p2face, p2, r_nor, -nor_v0p2); + interp_weights_face_v3(r_w, v0, v1, v2, NULL, p2face); + + sub_v3_v3v3(p1p2, p2, p1); + sub_v3_v3v3(v0p2, p2, v0); + nor_p1p2 = dot_v3v3(p1p2, r_nor); + *r_lambda = (nor_p1p2 != 0.0f ? nor_v0p2 / nor_p1p2 : 0.0f); + + return r_w[1] >= 0.0f && r_w[2] >= 0.0f && r_w[1] + r_w[2] <= 1.0f; + +#if 0 /* XXX this method uses the intersection point, but is broken and doesn't work well in general */ + float p[3], vec1[3], line[3], edge1[3], edge2[3], q[3]; + float a, f, u, v; + + sub_v3_v3v3(edge1, v1, v0); + sub_v3_v3v3(edge2, v2, v0); + sub_v3_v3v3(line, p2, p1); + + cross_v3_v3v3(p, line, edge2); + a = dot_v3v3(edge1, p); + if (a == 0.0f) return 0; + f = 1.0f / a; + + sub_v3_v3v3(vec1, p1, v0); + + u = f * dot_v3v3(vec1, p); + if ((u < 0.0f) || (u > 1.0f)) + return false; + + cross_v3_v3v3(q, vec1, edge1); + + v = f * dot_v3v3(line, q); + if ((v < 0.0f) || ((u + v) > 1.0f)) + return false; + + *r_lambda = f * dot_v3v3(edge2, q); + /* don't care about 0..1 lambda range here */ + /*if ((*r_lambda < 0.0f) || (*r_lambda > 1.0f)) + * return 0; + */ + + r_w[0] = 1.0f - u - v; + r_w[1] = u; + r_w[2] = v; + r_w[3] = 0.0f; + + cross_v3_v3v3(r_nor, edge1, edge2); + normalize_v3(r_nor); + + return true; +#endif +} + +static CollPair *cloth_point_collpair( + float p1[3], float p2[3], const MVert *mverts, int bp1, int bp2, int bp3, + int index_cloth, int index_coll, float epsilon, CollPair *collpair) +{ + const float *co1 = mverts[bp1].co, *co2 = mverts[bp2].co, *co3 = mverts[bp3].co; + float lambda /*, distance1 */, distance2; + float facenor[3], v1p1[3], v1p2[3]; + float w[4]; + + if (!cloth_point_face_collision_params(p1, p2, co1, co2, co3, facenor, &lambda, w)) + return collpair; + + sub_v3_v3v3(v1p1, p1, co1); +// distance1 = dot_v3v3(v1p1, facenor); + sub_v3_v3v3(v1p2, p2, co1); + distance2 = dot_v3v3(v1p2, facenor); +// if (distance2 > epsilon || (distance1 < 0.0f && distance2 < 0.0f)) + if (distance2 > epsilon) + return collpair; + + collpair->face1 = index_cloth; /* XXX actually not a face, but equivalent index for point */ + collpair->face2 = index_coll; + collpair->ap1 = index_cloth; + collpair->ap2 = collpair->ap3 = -1; /* unused */ + collpair->bp1 = bp1; + collpair->bp2 = bp2; + collpair->bp3 = bp3; + + /* note: using the second point here, which is + * the current updated position that needs to be corrected + */ + copy_v3_v3(collpair->pa, p2); + collpair->distance = distance2; + mul_v3_v3fl(collpair->vector, facenor, -distance2); + + interp_v3_v3v3v3(collpair->pb, co1, co2, co3, w); + + copy_v3_v3(collpair->normal, facenor); + collpair->time = lambda; + collpair->flag = 0; + + collpair++; + return collpair; +} + +//Determines collisions on overlap, collisions are written to collpair[i] and collision+number_collision_found is returned +static CollPair *cloth_point_collision( + ModifierData *md1, ModifierData *md2, + BVHTreeOverlap *overlap, float epsilon, CollPair *collpair, float UNUSED(dt)) +{ + ClothModifierData *clmd = (ClothModifierData *)md1; + CollisionModifierData *collmd = (CollisionModifierData *) md2; + /* Cloth *cloth = clmd->clothObject; */ /* UNUSED */ + ClothVertex *vert = NULL; + const MVertTri *vt; + const MVert *mverts = collmd->current_x; + + vert = &clmd->clothObject->verts[overlap->indexA]; + vt = &collmd->tri[overlap->indexB]; + + collpair = cloth_point_collpair( + vert->tx, vert->x, mverts, + vt->tri[0], vt->tri[1], vt->tri[2], + overlap->indexA, overlap->indexB, + epsilon, collpair); + + return collpair; +} + +static void cloth_points_objcollisions_nearcheck(ClothModifierData * clmd, CollisionModifierData *collmd, + CollPair **collisions, CollPair **collisions_index, + int numresult, BVHTreeOverlap *overlap, float epsilon, double dt) +{ + int i; + + /* can return 2 collisions in total */ + *collisions = (CollPair *) MEM_mallocN(sizeof(CollPair) * numresult * 2, "collision array" ); + *collisions_index = *collisions; + + for ( i = 0; i < numresult; i++ ) { + *collisions_index = cloth_point_collision((ModifierData *)clmd, (ModifierData *)collmd, + overlap+i, epsilon, *collisions_index, dt); + } +} + +static int cloth_points_objcollisions_resolve(ClothModifierData * clmd, CollisionModifierData *collmd, PartDeflect *pd, + CollPair *collisions, CollPair *collisions_index, float dt) +{ + Cloth *cloth = clmd->clothObject; + int i = 0, mvert_num = clmd->clothObject->mvert_num; + ClothVertex *verts = cloth->verts; + int ret = 0; + + // process all collisions + if ( collmd->bvhtree ) { + bool result = cloth_points_collision_response_static(clmd, collmd, pd, collisions, collisions_index, dt); + + // apply impulses in parallel + if (result) { + for (i = 0; i < mvert_num; i++) { + // calculate "velocities" (just xnew = xold + v; no dt in v) + if (verts[i].impulse_count) { + // VECADDMUL ( verts[i].tv, verts[i].impulse, 1.0f / verts[i].impulse_count ); + VECADD ( verts[i].tv, verts[i].tv, verts[i].impulse); + zero_v3(verts[i].impulse); + verts[i].impulse_count = 0; + + ret++; + } + } + } + } + + return ret; +} + +// cloth - object collisions +int cloth_points_objcollision(Object *ob, ClothModifierData *clmd, float step, float dt) +{ + Cloth *cloth= clmd->clothObject; + BVHTree *cloth_bvh; + int rounds = 0; // result counts applied collisions; ic is for debug output; + float round_dt = dt / (float)clmd->coll_parms->loop_count; + unsigned int i = 0, mvert_num = 0; + ClothVertex *verts = NULL; + int ret = 0, ret2 = 0; + Object **collobjs = NULL; + unsigned int numcollobj = 0; + + verts = cloth->verts; + mvert_num = cloth->mvert_num; + + //////////////////////////////////////////////////////////// + // static collisions + //////////////////////////////////////////////////////////// + + // create temporary cloth points bvh + cloth_bvh = BLI_bvhtree_new(mvert_num, max_ff(clmd->coll_parms->epsilon, clmd->coll_parms->distance_repel), 4, 6); + /* fill tree */ + for (i = 0; i < mvert_num; i++) { + float co[2][3]; + + copy_v3_v3(co[0], verts[i].x); + copy_v3_v3(co[1], verts[i].tx); + + BLI_bvhtree_insert(cloth_bvh, i, co[0], 2); + } + /* balance tree */ + BLI_bvhtree_balance(cloth_bvh); + + collobjs = get_collisionobjects(clmd->scene, ob, clmd->coll_parms->group, &numcollobj, eModifierType_Collision); + if (!collobjs) + return 0; + + /* move object to position (step) in time */ + for (i = 0; i < numcollobj; i++) { + Object *collob= collobjs[i]; + CollisionModifierData *collmd = (CollisionModifierData *)modifiers_findByType(collob, eModifierType_Collision); + if (!collmd->bvhtree) + continue; + + /* move object to position (step) in time */ + collision_move_object ( collmd, step + dt, step ); + } + + do { + CollPair **collisions, **collisions_index; + + ret2 = 0; + + collisions = MEM_callocN(sizeof(CollPair *) *numcollobj, "CollPair"); + collisions_index = MEM_callocN(sizeof(CollPair *) *numcollobj, "CollPair"); + + // check all collision objects + for (i = 0; i < numcollobj; i++) { + Object *collob= collobjs[i]; + CollisionModifierData *collmd = (CollisionModifierData *)modifiers_findByType(collob, eModifierType_Collision); + BVHTreeOverlap *overlap = NULL; + unsigned int result = 0; + float epsilon; + + if (!collmd->bvhtree) + continue; + + /* search for overlapping collision pairs */ + overlap = BLI_bvhtree_overlap(cloth_bvh, collmd->bvhtree, &result, NULL, NULL); + epsilon = BLI_bvhtree_getepsilon(collmd->bvhtree); + + // go to next object if no overlap is there + if (result && overlap) { + /* check if collisions really happen (costly near check) */ + cloth_points_objcollisions_nearcheck(clmd, collmd, &collisions[i], &collisions_index[i], + result, overlap, epsilon, round_dt); + + // resolve nearby collisions + ret += cloth_points_objcollisions_resolve(clmd, collmd, collob->pd, collisions[i], collisions_index[i], round_dt); + ret2 += ret; + } + + if (overlap) + MEM_freeN ( overlap ); + } + rounds++; + + for (i = 0; i < numcollobj; i++) { + if (collisions[i]) + MEM_freeN(collisions[i]); + } + + MEM_freeN(collisions); + MEM_freeN(collisions_index); + + //////////////////////////////////////////////////////////// + // update positions + // this is needed for bvh_calc_DOP_hull_moving() [kdop.c] + //////////////////////////////////////////////////////////// + + // verts come from clmd + for (i = 0; i < mvert_num; i++) { + if ( clmd->sim_parms->flags & CLOTH_SIMSETTINGS_FLAG_GOAL ) { + if ( verts [i].flags & CLOTH_VERT_FLAG_PINNED ) { + continue; + } + } + + VECADD ( verts[i].tx, verts[i].txold, verts[i].tv ); + } + //////////////////////////////////////////////////////////// + } + while ( ret2 && ( clmd->coll_parms->loop_count>rounds ) ); + + if (collobjs) + MEM_freeN(collobjs); + + BLI_bvhtree_free(cloth_bvh); + + return 1|MIN2 ( ret, 1 ); +} + +void cloth_find_point_contacts(Object *ob, ClothModifierData *clmd, float step, float dt, + ColliderContacts **r_collider_contacts, int *r_totcolliders) +{ + Cloth *cloth= clmd->clothObject; + BVHTree *cloth_bvh; + unsigned int i = 0, mvert_num = 0; + ClothVertex *verts = NULL; + + ColliderContacts *collider_contacts; + + Object **collobjs = NULL; + unsigned int numcollobj = 0; + + verts = cloth->verts; + mvert_num = cloth->mvert_num; + + //////////////////////////////////////////////////////////// + // static collisions + //////////////////////////////////////////////////////////// + + // create temporary cloth points bvh + cloth_bvh = BLI_bvhtree_new(mvert_num, max_ff(clmd->coll_parms->epsilon, clmd->coll_parms->distance_repel), 4, 6); + /* fill tree */ + for (i = 0; i < mvert_num; i++) { + float co[6]; + + copy_v3_v3(&co[0*3], verts[i].x); + copy_v3_v3(&co[1*3], verts[i].tx); + + BLI_bvhtree_insert(cloth_bvh, i, co, 2); + } + /* balance tree */ + BLI_bvhtree_balance(cloth_bvh); + + collobjs = get_collisionobjects(clmd->scene, ob, clmd->coll_parms->group, &numcollobj, eModifierType_Collision); + if (!collobjs) { + *r_collider_contacts = NULL; + *r_totcolliders = 0; + return; + } + + /* move object to position (step) in time */ + for (i = 0; i < numcollobj; i++) { + Object *collob= collobjs[i]; + CollisionModifierData *collmd = (CollisionModifierData *)modifiers_findByType(collob, eModifierType_Collision); + if (!collmd->bvhtree) + continue; + + /* move object to position (step) in time */ + collision_move_object ( collmd, step + dt, step ); + } + + collider_contacts = MEM_callocN(sizeof(ColliderContacts) * numcollobj, "CollPair"); + + // check all collision objects + for (i = 0; i < numcollobj; i++) { + ColliderContacts *ct = collider_contacts + i; + Object *collob= collobjs[i]; + CollisionModifierData *collmd = (CollisionModifierData *)modifiers_findByType(collob, eModifierType_Collision); + BVHTreeOverlap *overlap; + unsigned int result = 0; + float epsilon; + + ct->ob = collob; + ct->collmd = collmd; + ct->collisions = NULL; + ct->totcollisions = 0; + + if (!collmd->bvhtree) + continue; + + /* search for overlapping collision pairs */ + overlap = BLI_bvhtree_overlap(cloth_bvh, collmd->bvhtree, &result, NULL, NULL); + epsilon = BLI_bvhtree_getepsilon(collmd->bvhtree); + + // go to next object if no overlap is there + if (result && overlap) { + CollPair *collisions_index; + + /* check if collisions really happen (costly near check) */ + cloth_points_objcollisions_nearcheck(clmd, collmd, &ct->collisions, &collisions_index, + result, overlap, epsilon, dt); + ct->totcollisions = (int)(collisions_index - ct->collisions); + + // resolve nearby collisions +// ret += cloth_points_objcollisions_resolve(clmd, collmd, collob->pd, collisions[i], collisions_index[i], dt); + } + + if (overlap) + MEM_freeN(overlap); + } + + if (collobjs) + MEM_freeN(collobjs); + + BLI_bvhtree_free(cloth_bvh); + + //////////////////////////////////////////////////////////// + // update positions + // this is needed for bvh_calc_DOP_hull_moving() [kdop.c] + //////////////////////////////////////////////////////////// + + // verts come from clmd + for (i = 0; i < mvert_num; i++) { + if (clmd->sim_parms->flags & CLOTH_SIMSETTINGS_FLAG_GOAL) { + if (verts [i].flags & CLOTH_VERT_FLAG_PINNED) { + continue; + } + } + + VECADD(verts[i].tx, verts[i].txold, verts[i].tv); + } + //////////////////////////////////////////////////////////// + + *r_collider_contacts = collider_contacts; + *r_totcolliders = numcollobj; +} + +void cloth_free_contacts(ColliderContacts *collider_contacts, int totcolliders) +{ + if (collider_contacts) { + int i; + for (i = 0; i < totcolliders; ++i) { + ColliderContacts *ct = collider_contacts + i; + if (ct->collisions) { + MEM_freeN(ct->collisions); + } + } + MEM_freeN(collider_contacts); + } +} |