diff options
Diffstat (limited to 'source/blender/blenkernel/intern/collision.c')
| -rw-r--r-- | source/blender/blenkernel/intern/collision.c | 563 |
1 files changed, 560 insertions, 3 deletions
diff --git a/source/blender/blenkernel/intern/collision.c b/source/blender/blenkernel/intern/collision.c index 2f600935b1e..6b8eac44dfa 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" @@ -648,12 +650,12 @@ 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); } } @@ -913,3 +915,558 @@ 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 = collpair->distance - 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], MVert *mverts, int bp1, int bp2, int bp3, + int index_cloth, int index_coll, float epsilon, CollPair *collpair) +{ + 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; + MFace *face = NULL; + MVert *mverts = collmd->current_x; + + vert = &clmd->clothObject->verts[overlap->indexA]; + face = &collmd->mfaces[overlap->indexB]; + + collpair = cloth_point_collpair(vert->tx, vert->x, mverts, face->v1, face->v2, face->v3, overlap->indexA, overlap->indexB, epsilon, collpair); + if (face->v4) + collpair = cloth_point_collpair(vert->tx, vert->x, mverts, face->v3, face->v4, face->v1, 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, numverts = clmd->clothObject->numverts; + 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 < numverts; 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, numverts = 0; + ClothVertex *verts = NULL; + int ret = 0, ret2 = 0; + Object **collobjs = NULL; + unsigned int numcollobj = 0; + + verts = cloth->verts; + numverts = cloth->numverts; + + //////////////////////////////////////////////////////////// + // static collisions + //////////////////////////////////////////////////////////// + + // create temporary cloth points bvh + cloth_bvh = BLI_bvhtree_new(numverts, MAX2(clmd->coll_parms->epsilon, clmd->coll_parms->distance_repel), 4, 6); + /* fill tree */ + for (i = 0; i < numverts; 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) + 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 ); + 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 < numverts; 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, numverts = 0; + ClothVertex *verts = NULL; + + ColliderContacts *collider_contacts; + + Object **collobjs = NULL; + unsigned int numcollobj = 0; + + verts = cloth->verts; + numverts = cloth->numverts; + + //////////////////////////////////////////////////////////// + // static collisions + //////////////////////////////////////////////////////////// + + // create temporary cloth points bvh + cloth_bvh = BLI_bvhtree_new(numverts, MAX2(clmd->coll_parms->epsilon, clmd->coll_parms->distance_repel), 4, 6); + /* fill tree */ + for (i = 0; i < numverts; 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); + 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 < numverts; 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); + } +} |